correct rotation Jacobians

koide3 · Sep 5, 2023 · a00e978 · a00e978
1 parent 0f54bc7
commit a00e978
Show file tree

Hide file tree

Showing 5 changed files with 6 additions and 6 deletions.
diff --git a/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp b/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp
@@ -186,7 +186,7 @@ double FastGICP<PointSource, PointTarget>::linearize(const Eigen::Isometry3d& tr
     }
 
     Eigen::Matrix<double, 4, 6> dtdx0 = Eigen::Matrix<double, 4, 6>::Zero();
-    dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>());
+    dtdx0.block<3, 3>(0, 0) = skewd(trans.linear() * mean_A.head<3>());
     dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();
 
     Eigen::Matrix<double, 4, 6> jlossexp = dtdx0;

diff --git a/include/fast_gicp/gicp/impl/fast_gicp_st_impl.hpp b/include/fast_gicp/gicp/impl/fast_gicp_st_impl.hpp
@@ -107,7 +107,7 @@ double FastGICPSingleThread<PointSource, PointTarget>::linearize(const Eigen::Is
     }
 
     Eigen::Matrix<double, 4, 6> dtdx0 = Eigen::Matrix<double, 4, 6>::Zero();
-    dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>());
+    dtdx0.block<3, 3>(0, 0) = skewd(trans.linear() * mean_A.head<3>());
     dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();
 
     Eigen::Matrix<double, 4, 6> jlossexp = dtdx0;

diff --git a/include/fast_gicp/gicp/impl/fast_vgicp_impl.hpp b/include/fast_gicp/gicp/impl/fast_vgicp_impl.hpp
@@ -154,7 +154,7 @@ double FastVGICP<PointSource, PointTarget>::linearize(const Eigen::Isometry3d& t
     }
 
     Eigen::Matrix<double, 4, 6> dtdx0 = Eigen::Matrix<double, 4, 6>::Zero();
-    dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>());
+    dtdx0.block<3, 3>(0, 0) = skewd(trans.linear() * mean_A.head<3>());
     dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();
 
     Eigen::Matrix<double, 4, 6> jlossexp = dtdx0;

diff --git a/src/fast_gicp/cuda/compute_derivatives.cu b/src/fast_gicp/cuda/compute_derivatives.cu
@@ -79,7 +79,7 @@ struct compute_derivatives_kernel {
     Eigen::Vector3f error = mean_B - transed_mean_A;
 
     Eigen::Matrix<float, 3, 6> dtdx0;
-    dtdx0.block<3, 3>(0, 0) = skew_symmetric(transed_mean_A);
+    dtdx0.block<3, 3>(0, 0) = skew_symmetric(R * mean_A);
     dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3f::Identity();
 
     Eigen::Matrix<float, 3, 6> J = dtdx0;

diff --git a/src/fast_gicp/cuda/ndt_compute_derivatives.cu b/src/fast_gicp/cuda/ndt_compute_derivatives.cu
@@ -79,7 +79,7 @@ struct p2d_ndt_compute_derivatives_kernel {
     float err = w * error.transpose() * RCR_inv * error;
 
     Eigen::Matrix<float, 3, 6> dtdx0;
-    dtdx0.block<3, 3>(0, 0) = skew_symmetric(transed_mean_A);
+    dtdx0.block<3, 3>(0, 0) = skew_symmetric(R * mean_A);
     dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3f::Identity();
 
     Eigen::Matrix<float, 3, 6> J = dtdx0;
@@ -151,7 +151,7 @@ struct d2d_ndt_compute_derivatives_kernel {
     float err = w * error.transpose() * RCR_inv * error;
 
     Eigen::Matrix<float, 3, 6> dtdx0;
-    dtdx0.block<3, 3>(0, 0) = skew_symmetric(transed_mean_A);
+    dtdx0.block<3, 3>(0, 0) = skew_symmetric(R * mean_A);
     dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3f::Identity();
 
     Eigen::Matrix<float, 3, 6> J = dtdx0;