From d27119da3269816566d260b7a6269a0120abd53f Mon Sep 17 00:00:00 2001 From: jvallve <jvallve@iri.upc.edu> Date: Fri, 7 Oct 2022 13:21:15 +0200 Subject: [PATCH] hotfix bug in factor --- include/core/factor/factor_relative_pose_2d_with_extrinsics.h | 2 +- test/gtest_factor_relative_pose_2d_with_extrinsics.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h index 4455b0960..f0f70a5c1 100644 --- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h +++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h @@ -175,7 +175,7 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re // s1_p_s2 = s1_R_r1*(r1_R_w*(w_p_r2 - w_p_r1) - r1_p_s1) expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target)); - expected_measurement(2) = o_target - o_ref; + expected_measurement(2) = o_target - o_ref - o_sensor; } // Error diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp index a10190e0f..b5b4c9d0f 100644 --- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp @@ -113,7 +113,7 @@ void generateRandomProblemLandmark() x_s(2) = pi2pi(x_s(2)); // landmark pose - x_l(2) = pi2pi(x_f(2) + delta(2)); + x_l(2) = pi2pi(x_f(2) + x_s(2) + delta(2)); x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta.head<2>()); // Set states -- GitLab