diff --git a/launch/default_camera.yaml b/launch/default_camera.yaml
new file mode 100644
index 0000000..5c496f7
--- /dev/null
+++ b/launch/default_camera.yaml
@@ -0,0 +1,19 @@
+camera_model: double_sphere
+
+camera_parameters:
+    fx: 294.912
+    fy: 294.912
+    cx: 512
+    cy: 512
+    chi: 0.3
+    alpha: 0.6666667
+
+stereo_camera_parameters:
+    fx: 294.912
+    fy: 294.912
+    cx: 512
+    cy: 512
+    chi: 0.3
+    alpha: 0.6666667
+    tf_t: [2.0, 0.0, 0.0]
+    tf_r: [0.0, 0.0, 0.0, 1.0]
diff --git a/launch/default_camera_60.yaml b/launch/default_camera_60.yaml
new file mode 100644
index 0000000..768aff0
--- /dev/null
+++ b/launch/default_camera_60.yaml
@@ -0,0 +1,15 @@
+camera_model: perspective
+
+camera_parameters:
+    fx: 1024
+    fy: 1024
+    cx: 512
+    cy: 512
+
+stereo_camera_parameters:
+    fx: 1024
+    fy: 1024
+    cx: 512
+    cy: 512
+    tf_t: [2.0, 0.0, 0.0]
+    tf_r: [0.0, 0.0, 0.0, 1.0]
diff --git a/launch/kitti_camera.yaml b/launch/kitti_camera.yaml
new file mode 100644
index 0000000..282ef09
--- /dev/null
+++ b/launch/kitti_camera.yaml
@@ -0,0 +1,15 @@
+camera_model: perspective
+
+camera_parameters:
+    fx: 721.5377
+    fy: 721.5377
+    cx: 609.5593
+    cy: 172.854
+
+stereo_camera_parameters:
+    fx: 721.5377
+    fy: 721.5377
+    cx: 609.5593
+    cy: 172.854
+    tf_t: [0.533, 0.005, -0.001]
+    tf_r: [0, 0, 0, 1.0]
diff --git a/launch/matching_eval.launch b/launch/matching_eval.launch
index 0678b54..f830eed 100644
--- a/launch/matching_eval.launch
+++ b/launch/matching_eval.launch
@@ -2,8 +2,7 @@
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).matching.hdf5" />
     <arg name="rate" default="1" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 1024, fy: 1024, cx: 512, cy: 512, chi: 0.0, alpha: 0.0}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/default_camera.yaml" />
     <arg name="detector_type" default="ORB" />
     <arg name="detector_params" default="{nfeatures: 100}" />
     <arg name="descriptor_type" default="ORB" />
@@ -17,9 +16,8 @@
         <param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" />
         <param name="matched_image_topic" value="/omni_slam/matched" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
             detector_type: '$(arg detector_type)'
             detector_parameters: $(arg detector_params)
             descriptor_type: '$(arg descriptor_type)'
diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch
index c6ee1c0..b47f918 100644
--- a/launch/odometry_eval.launch
+++ b/launch/odometry_eval.launch
@@ -1,8 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).odometry.hdf5" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/default_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_odometry_eval_node" name="omni_slam_odometry_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -19,9 +18,8 @@
         <param name="path_ground_truth_topic" value="/omni_slam/odometry_path_truth" />
         <param name="output_frame" value="map" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 2000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             keyframe_interval: 1
diff --git a/launch/reconstruction_eval.launch b/launch/reconstruction_eval.launch
index c3922fb..89dcb63 100644
--- a/launch/reconstruction_eval.launch
+++ b/launch/reconstruction_eval.launch
@@ -1,8 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).reconstruction.hdf5" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/default_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_reconstruction_eval_node" name="omni_slam_reconstruction_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -14,9 +13,8 @@
         <param name="point_cloud_topic" value="/omni_slam/reconstructed" />
         <param name="output_frame" value="map" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             keyframe_interval: 1
diff --git a/launch/slam_eval.launch b/launch/slam_eval.launch
index c9842e7..5f92a9d 100644
--- a/launch/slam_eval.launch
+++ b/launch/slam_eval.launch
@@ -1,9 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).slam.hdf5" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
-    <arg name="stereo_camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/default_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -23,10 +21,8 @@
         <param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />
         <param name="output_frame" value="map" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
-            stereo_camera_parameters: $(arg stereo_camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             keyframe_interval: 1
@@ -54,8 +50,6 @@
             stereo_matcher_num_scales: 5
             stereo_matcher_error_threshold: 20
             stereo_matcher_epipolar_threshold: 0.008
-            stereo_tf_t: [2.0, 0.0, 0.0]
-            stereo_tf_r: [0.0, 0.0, 0.0, 1.0]
         </rosparam>
     </node>
 </launch>
diff --git a/launch/slam_eval_kitti.launch b/launch/slam_eval_kitti.launch
index fbafbea..3b3422d 100644
--- a/launch/slam_eval_kitti.launch
+++ b/launch/slam_eval_kitti.launch
@@ -1,9 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).slam.hdf5" />
-    <arg name="camera_model" default="perspective" />
-    <arg name="camera_params" default="{fx: 721.5377, fy: 721.5377, cx: 609.5593, cy: 172.854}" />
-    <arg name="stereo_camera_params" default="{fx: 721.5377, fy: 721.5377, cx: 609.5593, cy: 172.854}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/kitti_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -23,10 +21,8 @@
         <param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />
         <param name="output_frame" value="imu" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
-            stereo_camera_parameters: $(arg stereo_camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             keyframe_interval: 1
@@ -54,8 +50,6 @@
             stereo_matcher_num_scales: 5
             stereo_matcher_error_threshold: 20
             stereo_matcher_epipolar_threshold: 0.008
-            stereo_tf_t: [0.533, 0.005, -0.001]
-            stereo_tf_r: [0, 0, 0, 1.0]
         </rosparam>
     </node>
 </launch>
diff --git a/launch/slam_eval_t265.launch b/launch/slam_eval_t265.launch
index cf1c761..34c9ab2 100644
--- a/launch/slam_eval_t265.launch
+++ b/launch/slam_eval_t265.launch
@@ -1,9 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).slam.hdf5" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 287.405, fy: 287.092, cx: 418.432, cy: 399.257, chi: -0.0000588, alpha: 0.675621}" />
-    <arg name="stereo_camera_params" default="{fx: 287.405, fy: 287.092, cx: 418.432, cy: 399.257, chi: -0.0000588, alpha: 0.675621}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/t265_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -23,10 +21,8 @@
         <param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />
         <param name="output_frame" value="map" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
-            stereo_camera_parameters: $(arg stereo_camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             keyframe_interval: 1
@@ -54,8 +50,6 @@
             stereo_matcher_num_scales: 5
             stereo_matcher_error_threshold: 20
             stereo_matcher_epipolar_threshold: 0.008
-            stereo_tf_t: [0.06410918, 0.0, 0.0]
-            stereo_tf_r: [0.0, 0.0, 0.0, 1.0]
         </rosparam>
     </node>
 </launch>
diff --git a/launch/slam_eval_tum.launch b/launch/slam_eval_tum.launch
index 83cdb9c..25b3e8a 100644
--- a/launch/slam_eval_tum.launch
+++ b/launch/slam_eval_tum.launch
@@ -1,9 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).slam.hdf5" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 313.533725, fy: 313.51138991, cx: 510.37735, cy: 514.419593, chi: -0.18065, alpha: 0.59186286}" />
-    <arg name="stereo_camera_params" default="{fx: 313.4386621, fy: 313.359903, cx: 505.72413717, cy: 510.5791992, chi: -0.17829397, alpha: 0.591774861}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/tum_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -23,10 +21,8 @@
         <param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />
         <param name="output_frame" value="map" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
-            stereo_camera_parameters: $(arg stereo_camera_params)
             detector_type: 'ORB'
             detector_parameters: {nfeatures: 1000}
             descriptor_type: 'ORB'
@@ -55,8 +51,6 @@
             stereo_matcher_num_scales: 5
             stereo_matcher_error_threshold: 20
             stereo_matcher_epipolar_threshold: 0.008
-            stereo_tf_t: [0.1010976, 0.001878, 0.00117]
-            stereo_tf_r: [-0.0237542, 0.0002876, -0.0004555, 0.9997177]
         </rosparam>
     </node>
 </launch>
diff --git a/launch/t265_camera.yaml b/launch/t265_camera.yaml
new file mode 100644
index 0000000..b397df1
--- /dev/null
+++ b/launch/t265_camera.yaml
@@ -0,0 +1,19 @@
+camera_model: double_sphere
+
+camera_parameters:
+    fx: 287.405
+    fy: 287.092
+    cx: 418.432
+    cy: 399.257
+    chi: -0.0000588
+    alpha: 0.675621
+
+stereo_camera_parameters:
+    fx: 287.405
+    fy: 287.092
+    cx: 418.432
+    cy: 399.257
+    chi: -0.0000588
+    alpha: 0.675621
+    tf_t: [0.06410918, 0.0, 0.0]
+    tf_r: [0.0, 0.0, 0.0, 1.0]
diff --git a/launch/tracking_eval.launch b/launch/tracking_eval.launch
index 1dd1fdb..2a3f5e5 100644
--- a/launch/tracking_eval.launch
+++ b/launch/tracking_eval.launch
@@ -1,8 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).tracking.hdf5" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/default_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_tracking_eval_node" name="omni_slam_tracking_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -12,9 +11,8 @@
         <param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" />
         <param name="tracked_image_topic" value="/omni_slam/tracked" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 100, qualityLevel: 0.01, minDistance: 5, blockSize: 5}
             tracker_type: 'lk'
diff --git a/launch/tum_camera.yaml b/launch/tum_camera.yaml
new file mode 100644
index 0000000..dd07b83
--- /dev/null
+++ b/launch/tum_camera.yaml
@@ -0,0 +1,19 @@
+camera_model: double_sphere
+
+camera_parameters:
+    fx: 313.533725
+    fy: 313.51138991
+    cx: 510.37735
+    cy: 514.419593
+    chi: -0.18065
+    alpha: 0.59186286
+
+stereo_camera_parameters:
+    fx: 313.4386621
+    fy: 313.359903
+    cx: 505.72413717
+    cy: 510.5791992
+    chi: -0.17829397
+    alpha: 0.591774861
+    tf_t: [0.1010976, 0.001878, 0.00117]
+    tf_r: [-0.0237542, 0.0002876, -0.0004555, 0.9997177]
diff --git a/src/odometry/five_point.cc b/src/odometry/five_point.cc
index ae5d127..429e3ea 100644
--- a/src/odometry/five_point.cc
+++ b/src/odometry/five_point.cc
@@ -6,6 +6,7 @@
 #include <random>
 #include <ceres/ceres.h>
 #include "optimization/reprojection_error.h"
+#include "optimization/scale_parameterization.h"
 #include "camera/double_sphere.h"
 #include "camera/perspective.h"
 
@@ -14,11 +15,12 @@ namespace omni_slam
 namespace odometry
 {
 
-FivePoint::FivePoint(int ransac_iterations, double epipolar_threshold, int trans_ransac_iterations, double reprojection_threshold, int num_ceres_threads)
+FivePoint::FivePoint(int ransac_iterations, double epipolar_threshold, int trans_ransac_iterations, double reprojection_threshold, bool fix_translation_vector, int num_ceres_threads)
     : ransacIterations_(ransac_iterations),
     epipolarThreshold_(epipolar_threshold),
     transIterations_(trans_ransac_iterations),
     reprojectionThreshold_(reprojection_threshold),
+    fixTransVec_(fix_translation_vector),
     numCeresThreads_(num_ceres_threads)
 {
 }
@@ -74,25 +76,25 @@ int FivePoint::Compute(const std::vector<data::Landmark> &landmarks, data::Frame
             bestIds = ids;
         }
     }
+    Vector3d tvec = bestPose.block<3, 1>(0, 3);
+    Vector3d t = bestPose.block<3, 1>(0, 3);
     Matrix<double, 3, 4> estPose;
-    Vector3d t;
     if (prev_frame.HasEstimatedPose())
     {
         estPose = util::TFUtil::CombineTransforms(bestPose, prev_frame.GetEstimatedInversePose());
-        t = prev_frame.GetEstimatedInversePose().block<3, 1>(0, 3);
+        t = estPose.block<3, 1>(0, 3);
     }
     else if (prev_frame.HasPose())
     {
         estPose = util::TFUtil::CombineTransforms(bestPose, prev_frame.GetInversePose());
-        t = prev_frame.GetInversePose().block<3, 1>(0, 3);
+        t = estPose.block<3, 1>(0, 3);
     }
     else
     {
         estPose = bestPose;
-        t << 0, 0, 0;
     }
     std::vector<int> transInliers;
-    inliers = ComputeTranslation(bestXs, bestFeats, util::TFUtil::GetRotationFromPoseMatrix(estPose), t, transInliers);
+    inliers = ComputeTranslation(bestXs, bestFeats, util::TFUtil::GetRotationFromPoseMatrix(estPose), t, tvec, transInliers);
     std::vector<int> transInlierIds;
     transInlierIds.reserve(inliers);
     for (int i : transInliers)
@@ -303,9 +305,9 @@ void FivePoint::EssentialToPoses(const Matrix3d &E, std::vector<Matrix3d> &rs, s
     ts.emplace_back(-UWtVt.transpose() * U.col(2));
 }
 
-int FivePoint::ComputeTranslation(const std::vector<Vector3d> &xs, const std::vector<std::pair<const data::Feature*, const data::Feature*>> &ys, const Matrix3d &R, Vector3d &t, std::vector<int> &inlier_indices) const
+int FivePoint::ComputeTranslation(const std::vector<Vector3d> &xs, const std::vector<std::pair<const data::Feature*, const data::Feature*>> &ys, const Matrix3d &R, Vector3d &t, const Vector3d &tvec, std::vector<int> &inlier_indices) const
 {
-    int inliers = TranslationRANSAC(xs, ys, R, t);
+    int inliers = TranslationRANSAC(xs, ys, R, t, tvec);
     inlier_indices = GetTranslationInlierIndices(xs, ys, R, t);
     std::vector<Vector3d> xs_inliers;
     std::vector<std::pair<const data::Feature*, const data::Feature*>> ys_inliers;
@@ -316,11 +318,11 @@ int FivePoint::ComputeTranslation(const std::vector<Vector3d> &xs, const std::ve
         xs_inliers.push_back(xs[inx]);
         ys_inliers.push_back(ys[inx]);
     }
-    OptimizeTranslation(xs_inliers, ys_inliers, R, t);
+    OptimizeTranslation(xs_inliers, ys_inliers, R, t, tvec);
     return inliers;
 }
 
-int FivePoint::TranslationRANSAC(const std::vector<Vector3d> &xs, const std::vector<std::pair<const data::Feature*, const data::Feature*>> &ys, const Matrix3d &R, Vector3d &t) const
+int FivePoint::TranslationRANSAC(const std::vector<Vector3d> &xs, const std::vector<std::pair<const data::Feature*, const data::Feature*>> &ys, const Matrix3d &R, Vector3d &t, const Vector3d &tvec) const
 {
     int bestInliers = 0;
     std::random_device rd;
@@ -340,7 +342,7 @@ int FivePoint::TranslationRANSAC(const std::vector<Vector3d> &xs, const std::vec
         std::vector<Vector3d> x{xs[indices[i]]};
         std::vector<std::pair<const data::Feature*, const data::Feature*>> y{ys[indices[i]]};
         Vector3d tmpT = t;
-        if (OptimizeTranslation(x, y, R, tmpT))
+        if (OptimizeTranslation(x, y, R, tmpT, tvec))
         {
             int inliers = GetTranslationInlierIndices(xs, ys, R, tmpT).size();
             #pragma omp critical
@@ -356,7 +358,7 @@ int FivePoint::TranslationRANSAC(const std::vector<Vector3d> &xs, const std::vec
     return bestInliers;
 }
 
-bool FivePoint::OptimizeTranslation(const std::vector<Vector3d> &xs, const std::vector<std::pair<const data::Feature*, const data::Feature*>> &ys, const Matrix3d &R, Vector3d &t) const
+bool FivePoint::OptimizeTranslation(const std::vector<Vector3d> &xs, const std::vector<std::pair<const data::Feature*, const data::Feature*>> &ys, const Matrix3d &R, Vector3d &t, const Vector3d &tvec) const
 {
     ceres::Problem problem;
     ceres::LossFunction *loss_function = nullptr;
@@ -385,9 +387,13 @@ bool FivePoint::OptimizeTranslation(const std::vector<Vector3d> &xs, const std::
         {
             problem.AddResidualBlock(cost_function, loss_function, &quatData[0], &tData[0], &landmarks[landmarks.size() - 3]);
             problem.SetParameterBlockConstant(&landmarks[landmarks.size() - 3]);
-            problem.SetParameterBlockConstant(&quatData[0]);
         }
     }
+    problem.SetParameterBlockConstant(&quatData[0]);
+    if (fixTransVec_)
+    {
+        problem.SetParameterization(&tData[0], optimization::ScaleParameterization<3>::Create(tvec));
+    }
     ceres::Solver::Options options;
     options.max_num_iterations = 100;
     options.linear_solver_type = ceres::DENSE_SCHUR;
diff --git a/src/odometry/five_point.h b/src/odometry/five_point.h
index 7f5e98e..45d2798 100644
--- a/src/odometry/five_point.h
+++ b/src/odometry/five_point.h
@@ -15,7 +15,7 @@ namespace odometry
 class FivePoint : public PoseEstimator
 {
 public:
-    FivePoint(int ransac_iterations, double epipolar_threshold, int trans_ransac_iterations, double reprojection_threshold, int num_ceres_threads = 1);
+    FivePoint(int ransac_iterations, double epipolar_threshold, int trans_ransac_iterations, double reprojection_threshold, bool fix_translation_vector, int num_ceres_threads = 1);
 
     int Compute(const std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, const data::Frame &prev_frame, std::vector<int> &inlier_indices) const;
     int ComputeE(const std::vector<data::Landmark> &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector<int> &inlier_indices, bool stereo = false) const;
@@ -25,9 +25,9 @@ class FivePoint : public PoseEstimator
     void FivePointRelativePose(const std::vector<Vector3d> &x1, const std::vector<Vector3d> &x2, std::vector<Matrix3d> &Es) const;
     std::vector<int> GetEInlierIndices(const std::vector<Vector3d> &x1, const std::vector<Vector3d> &x2, const Matrix3d &E) const;
     void EssentialToPoses(const Matrix3d &E, std::vector<Matrix3d> &rs, std::vector<Vector3d> &ts) const;
-    int ComputeTranslation(const std::vector<Vector3d> &xs, const std::vector<std::pair<const data::Feature*, const data::Feature*>> &ys, const Matrix3d &R, Vector3d &t, std::vector<int> &inlier_indices) const;
-    int TranslationRANSAC(const std::vector<Vector3d> &xs, const std::vector<std::pair<const data::Feature*, const data::Feature*>> &ys, const Matrix3d &R, Vector3d &t) const;
-    bool OptimizeTranslation(const std::vector<Vector3d> &xs, const std::vector<std::pair<const data::Feature*, const data::Feature*>> &ys, const Matrix3d &R, Vector3d &t) const;
+    int ComputeTranslation(const std::vector<Vector3d> &xs, const std::vector<std::pair<const data::Feature*, const data::Feature*>> &ys, const Matrix3d &R, Vector3d &t, const Vector3d &tvec, std::vector<int> &inlier_indices) const;
+    int TranslationRANSAC(const std::vector<Vector3d> &xs, const std::vector<std::pair<const data::Feature*, const data::Feature*>> &ys, const Matrix3d &R, Vector3d &t, const Vector3d &tvec) const;
+    bool OptimizeTranslation(const std::vector<Vector3d> &xs, const std::vector<std::pair<const data::Feature*, const data::Feature*>> &ys, const Matrix3d &R, Vector3d &t, const Vector3d &tvec) const;
     std::vector<int> GetTranslationInlierIndices(const std::vector<Vector3d> &xs, const std::vector<std::pair<const data::Feature*, const data::Feature*>> &ys, const Matrix3d &R, const Vector3d &t) const;
     Vector3d TriangulateDLT(const Vector3d &x1, const Vector3d &x2, const Matrix<double, 3, 4> &pose1, const Matrix<double, 3, 4> &pose2) const;
 
@@ -35,6 +35,7 @@ class FivePoint : public PoseEstimator
     int transIterations_;
     double epipolarThreshold_;
     double reprojectionThreshold_;
+    bool fixTransVec_;
     int numCeresThreads_;
 };
 
diff --git a/src/optimization/scale_parameterization.h b/src/optimization/scale_parameterization.h
new file mode 100644
index 0000000..e88aa58
--- /dev/null
+++ b/src/optimization/scale_parameterization.h
@@ -0,0 +1,46 @@
+#ifndef _SCALE_PARAMETERIZATION_H_
+#define _SCALE_PARAMETERIZATION_H_
+
+#include <ceres/ceres.h>
+#include <Eigen/Dense>
+
+using namespace Eigen;
+
+namespace omni_slam
+{
+namespace optimization
+{
+
+template<int G>
+class ScaleParameterization
+{
+public:
+    ScaleParameterization(const Matrix<double, G, 1> &vec)
+        : vec_(vec.normalized())
+    {
+    }
+
+    template<typename T>
+    bool operator()(const T *x, const T *delta, T *x_plus_delta) const
+    {
+        Matrix<T, G, 1> vec = vec_.template cast<T>();
+        for (int i = 0; i < G; i++)
+        {
+            x_plus_delta[i] = x[i] + delta[0] * vec[i];
+        }
+        return true;
+    }
+
+    static ceres::LocalParameterization* Create(const Matrix<double, G, 1> &vec)
+    {
+        return new ceres::AutoDiffLocalParameterization<ScaleParameterization, G, 1>(new ScaleParameterization<G>(vec));
+    }
+
+private:
+    const Matrix<double, G, 1> vec_;
+};
+
+}
+}
+
+#endif /* _SCALE_PARAMETERIZATION_H_ */
diff --git a/src/ros/eval_base.cc b/src/ros/eval_base.cc
index 3ca4180..51d13e4 100644
--- a/src/ros/eval_base.cc
+++ b/src/ros/eval_base.cc
@@ -59,8 +59,8 @@ EvalBase<true>::EvalBase(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &n
     stereoT.reserve(3);
     std::vector<double> stereoR;
     stereoR.reserve(4);
-    nhp_.getParam("stereo_tf_t", stereoT);
-    nhp_.getParam("stereo_tf_r", stereoR);
+    nhp_.getParam("stereo_camera_parameters/tf_t", stereoT);
+    nhp_.getParam("stereo_camera_parameters/tf_r", stereoR);
     Quaterniond q(stereoR[3], stereoR[0], stereoR[1], stereoR[2]);
     Vector3d t(stereoT[0], stereoT[1], stereoT[2]);
     stereoPose_ = util::TFUtil::QuaternionTranslationToPoseMatrix(q, t);
diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc
index 112e11f..dd6f7c0 100644
--- a/src/ros/odometry_eval.cc
+++ b/src/ros/odometry_eval.cc
@@ -51,7 +51,11 @@ OdometryEval<Stereo>::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::Nod
     }
     else if (odometryType == "five_point")
     {
-        poseEstimator.reset(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold, iterations, reprojThresh, numCeresThreads));
+        poseEstimator.reset(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold, iterations, reprojThresh, false, numCeresThreads));
+    }
+    else if (odometryType == "five_point_fixed_translation")
+    {
+        poseEstimator.reset(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold, iterations, reprojThresh, true, numCeresThreads));
     }
     else
     {
diff --git a/src/ros/tracking_eval.cc b/src/ros/tracking_eval.cc
index b727271..e144d29 100644
--- a/src/ros/tracking_eval.cc
+++ b/src/ros/tracking_eval.cc
@@ -77,7 +77,7 @@ TrackingEval<Stereo>::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod
         ROS_ERROR("Invalid tracker type specified");
     }
 
-    unique_ptr<odometry::FivePoint> checker(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold, 0, 0));
+    unique_ptr<odometry::FivePoint> checker(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold, 0, false, 0));
 
     trackingModule_.reset(new module::TrackingModule(detector, tracker, checker, minFeaturesRegion, maxFeaturesRegion));
 }