Merge pull request #41 from koide3/quad

Remove unnecessary 0.5 factor in matching cost factors
koide3 · Jan 14, 2025 · 3509828 · 3509828
2 parents dae3112 + c4913d6
commit 3509828
Show file tree

Hide file tree

Showing 9 changed files with 14 additions and 14 deletions.
diff --git a/include/gtsam_points/cuda/kernels/vgicp_derivatives.cuh b/include/gtsam_points/cuda/kernels/vgicp_derivatives.cuh
@@ -61,7 +61,7 @@ struct vgicp_derivatives_kernel {
     Eigen::Matrix<float, 6, 3> J_source_RCR_inv = J_source.transpose() * RCR_inv;
 
     LinearizedSystem6 linearized;
-    linearized.error = 0.5f * error.transpose() * RCR_inv * error;
+    linearized.error = error.transpose() * RCR_inv * error;
     linearized.H_target = J_target_RCR_inv * J_target;
     linearized.H_source = J_source_RCR_inv * J_source;
     linearized.H_target_source = J_target_RCR_inv * J_source;
@@ -123,7 +123,7 @@ struct vgicp_error_kernel {
     const Eigen::Matrix3f RCR_inv = (cov_B + RCR).inverse();
     Eigen::Vector3f error = mean_B - transed_mean_A;
 
-    return 0.5f * error.transpose() * RCR_inv * error;
+    return error.transpose() * RCR_inv * error;
   }
 
   const Eigen::Isometry3f* linearization_point_ptr;

diff --git a/include/gtsam_points/factors/impl/integrated_color_consistency_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_color_consistency_factor_impl.hpp
@@ -169,7 +169,7 @@ double IntegratedColorConsistencyFactor_<TargetFrame, SourceFrame, IntensityGrad
     const Eigen::Vector4d offset = projected - mean_B;
     const double error_photo = intensity_B + gradient_B.dot(offset) - intensity_A;
 
-    const double err = 0.5 * error_photo * photometric_term_weight * error_photo;
+    const double err = error_photo * photometric_term_weight * error_photo;
 
     if (!H_target) {
       return err;

diff --git a/include/gtsam_points/factors/impl/integrated_colored_gicp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_colored_gicp_factor_impl.hpp
@@ -182,14 +182,14 @@ double IntegratedColoredGICPFactor_<TargetFrame, SourceFrame, IntensityGradients
 
     // geometric error
     const Eigen::Vector4d residual_geom = transed_A - mean_B;
-    const double error_geom = 0.5 * residual_geom.transpose() * geometric_term_weight * mahalanobis[i] * residual_geom;
+    const double error_geom = residual_geom.transpose() * geometric_term_weight * mahalanobis[i] * residual_geom;
 
     // photometric error
     const Eigen::Vector4d projected = transed_A - (transed_A - mean_B).dot(normal_B) * normal_B;
     const Eigen::Vector4d offset = projected - mean_B;
     const double residual_photo = intensity_B + gradient_B.dot(offset) - intensity_A;
 
-    const double error_photo = 0.5 * residual_photo * photometric_term_weight * residual_photo;
+    const double error_photo = residual_photo * photometric_term_weight * residual_photo;
 
     if (!H_target) {
       return error_geom + error_photo;

diff --git a/include/gtsam_points/factors/impl/integrated_ct_gicp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_ct_gicp_factor_impl.hpp
@@ -73,7 +73,7 @@ double IntegratedCT_GICPFactor_<TargetFrame, SourceFrame>::error(const gtsam::Va
 
     const Eigen::Vector4d transed_source_pt = pose * source_pt;
     const Eigen::Vector4d residual = transed_source_pt - target_pt;
-    const double error = 0.5 * residual.transpose() * mahalanobis[i] * residual;
+    const double error = residual.transpose() * mahalanobis[i] * residual;
 
     return error;
   };
@@ -126,7 +126,7 @@ boost::shared_ptr<gtsam::GaussianFactor> IntegratedCT_GICPFactor_<TargetFrame, S
     const gtsam::Matrix64 H_0_mahalanobis = H_0.transpose() * mahalanobis[i];
     const gtsam::Matrix64 H_1_mahalanobis = H_1.transpose() * mahalanobis[i];
 
-    const double error = 0.5 * residual.transpose() * mahalanobis_residual;
+    const double error = residual.transpose() * mahalanobis_residual;
     *H_00 += H_0_mahalanobis * H_0;
     *H_11 += H_1_mahalanobis * H_1;
     *H_01 += H_0_mahalanobis * H_1;

diff --git a/include/gtsam_points/factors/impl/integrated_ct_icp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_ct_icp_factor_impl.hpp
@@ -102,7 +102,7 @@ double IntegratedCT_ICPFactor_<TargetFrame, SourceFrame>::error(const gtsam::Val
     gtsam::Point3 residual = transed_source_pt - target_pt.template head<3>();
     double error = gtsam::dot(residual, target_normal.template head<3>());
 
-    return 0.5 * error * error;
+    return error * error;
   };
 
   if (is_omp_default() || num_threads == 1) {
@@ -162,7 +162,7 @@ boost::shared_ptr<gtsam::GaussianFactor> IntegratedCT_ICPFactor_<TargetFrame, So
     *b_0 += H_0.transpose() * error;
     *b_1 += H_1.transpose() * error;
 
-    return 0.5 * error * error;
+    return error * error;
   };
 
   double error = 0.0;

diff --git a/include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp
@@ -193,7 +193,7 @@ double IntegratedGICPFactor_<TargetFrame, SourceFrame>::evaluate(
     const Eigen::Vector4d transed_mean_A = delta * mean_A;
     const Eigen::Vector4d residual = mean_B - transed_mean_A;
 
-    const double error = 0.5 * residual.transpose() * mahalanobis[i] * residual;
+    const double error = residual.transpose() * mahalanobis[i] * residual;
     if (H_target == nullptr) {
       return error;
     }

diff --git a/include/gtsam_points/factors/impl/integrated_icp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_icp_factor_impl.hpp
@@ -194,7 +194,7 @@ double IntegratedICPFactor_<TargetFrame, SourceFrame>::evaluate(
       residual = normal_B.array() * residual.array();
     }
 
-    const double error = 0.5 * residual.transpose() * residual;
+    const double error = residual.transpose() * residual;
     if (H_target == nullptr) {
       return error;
     }

diff --git a/include/gtsam_points/factors/impl/integrated_loam_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_loam_factor_impl.hpp
@@ -142,7 +142,7 @@ double IntegratedPointToPlaneFactor_<TargetFrame, SourceFrame>::evaluate(
 
     Eigen::Vector4d residual = x_j - transed_x_i;
     Eigen::Vector4d plane_residual = residual.array() * normal.array();
-    const double error = 0.5 * plane_residual.transpose() * plane_residual;
+    const double error = plane_residual.transpose() * plane_residual;
 
     if (H_target == nullptr) {
       return error;
@@ -297,7 +297,7 @@ double IntegratedPointToEdgeFactor_<TargetFrame, SourceFrame>::evaluate(
 
     Eigen::Vector4d residual = Eigen::Vector4d::Zero();
     residual.head<3>() = x_ij.head<3>().cross(x_il.head<3>()) * c_inv;
-    const double error = 0.5 * residual.dot(residual);
+    const double error = residual.dot(residual);
 
     if (H_target == nullptr) {
       return error;

diff --git a/include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp
@@ -153,7 +153,7 @@ double IntegratedVGICPFactor_<SourceFrame>::evaluate(
     Eigen::Vector4d transed_mean_A = delta * mean_A;
     Eigen::Vector4d residual = mean_B - transed_mean_A;
 
-    const double error = 0.5 * residual.transpose() * mahalanobis[i] * residual;
+    const double error = residual.transpose() * mahalanobis[i] * residual;
 
     if (!H_target) {
       return error;