Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Remove unnecessary 0.5 factor in matching cost factors #41

Merged
merged 1 commit into from
Jan 14, 2025
Merged

Remove unnecessary 0.5 factor in matching cost factors #41

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions include/gtsam_points/cuda/kernels/vgicp_derivatives.cuh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion include/gtsam_points/factors/impl/integrated_color_consistency_factor_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions include/gtsam_points/factors/impl/integrated_colored_gicp_factor_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions include/gtsam_points/factors/impl/integrated_ct_gicp_factor_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};
Expand Down Expand Up @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions include/gtsam_points/factors/impl/integrated_ct_icp_factor_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
2 changes: 1 addition & 1 deletion include/gtsam_points/factors/impl/integrated_icp_factor_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
4 changes: 2 additions & 2 deletions include/gtsam_points/factors/impl/integrated_loam_factor_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Loading