fixing bug of estimating translation vector

PointCloudLibrary · Sep 5, 2014 · 5cb1b92 · 5cb1b92
1 parent 8cb8e87
commit 5cb1b92
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp b/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp
@@ -98,7 +98,7 @@ pcl::registration::TransformationEstimationSVDScale<PointSource, PointTarget, Sc
   scale2 = sum_tt_ / sum_ss;
   float scale = scale2;
   transformation_matrix.topLeftCorner (3, 3) = scale * R;
-  const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3));
+  const Eigen::Matrix<Scalar, 3, 1> Rc (scale * R * centroid_src.head (3));
   transformation_matrix.block (0, 3, 3, 1) = centroid_tgt. head (3) - Rc;
 }