From 76f83f147eb614cd090d9c2b77745bab834f5364 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Sat, 4 Apr 2020 14:48:19 +0200
Subject: [PATCH] FactorRelative2dWithExtrinsics, pi2pi in some factors,
missing gtests
---
CMakeLists.txt | 1 +
include/core/factor/factor_diff_drive.h | 6 +-
include/core/factor/factor_odom_2d.h | 6 +-
.../core/factor/factor_odom_2d_closeloop.h | 6 +-
include/core/factor/factor_pose_2d.h | 6 +-
.../core/factor/factor_relative_2d_analytic.h | 14 +-
.../factor_relative_pose_2d_with_extrinsics.h | 46 ++--
include/core/math/rotations.h | 2 +-
test/CMakeLists.txt | 12 +
test/gtest_factor_odom_2d.cpp | 109 +++++++++
test/gtest_factor_relative_2d_analytic.cpp | 109 +++++++++
...actor_relative_pose_2d_with_extrinsics.cpp | 219 ++++++++++++++++++
12 files changed, 487 insertions(+), 49 deletions(-)
create mode 100644 test/gtest_factor_odom_2d.cpp
create mode 100644 test/gtest_factor_relative_2d_analytic.cpp
create mode 100644 test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 98de6dbd9..c46ad291f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -246,6 +246,7 @@ SET(HDRS_FACTOR
include/core/factor/factor_pose_3d.h
include/core/factor/factor_quaternion_absolute.h
include/core/factor/factor_relative_2d_analytic.h
+ include/core/factor/factor_relative_pose_2d_with_extrinsics.h
)
SET(HDRS_FEATURE
include/core/feature/feature_base.h
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index d936a99d6..7b189e442 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -14,6 +14,7 @@
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/feature/feature_motion.h"
+#include "core/math/rotations.h"
namespace
{
@@ -126,10 +127,7 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
residuals = delta_corrected - delta_predicted;
// angle remapping
- while (residuals(2) > T(M_PI))
- residuals(2) = residuals(2) - T(2. * M_PI);
- while (residuals(2) <= T(-M_PI))
- residuals(2) = residuals(2) + T(2. * M_PI);
+ pi2pi(residuals(2));
// weighted residual
residuals = getMeasurementSquareRootInformationUpper() * residuals;
diff --git a/include/core/factor/factor_odom_2d.h b/include/core/factor/factor_odom_2d.h
index e20f0f50f..1ce2a375c 100644
--- a/include/core/factor/factor_odom_2d.h
+++ b/include/core/factor/factor_odom_2d.h
@@ -4,6 +4,7 @@
//Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
//#include "ceres/jet.h"
@@ -72,10 +73,7 @@ inline bool FactorOdom2d::operator ()(const T* const _p1, const T* const _o1, co
// Error
er = expected_measurement - getMeasurement();
- while (er(2) > T( M_PI ))
- er(2) -= T( 2 * M_PI );
- while (er(2) < T( -M_PI ))
- er(2) += T( 2 * M_PI );
+ er(2) = pi2pi(er(2));
// Residuals
res = getMeasurementSquareRootInformationUpper() * er;
diff --git a/include/core/factor/factor_odom_2d_closeloop.h b/include/core/factor/factor_odom_2d_closeloop.h
index e7d35aefa..85a178544 100644
--- a/include/core/factor/factor_odom_2d_closeloop.h
+++ b/include/core/factor/factor_odom_2d_closeloop.h
@@ -4,6 +4,7 @@
//Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
//#include "ceres/jet.h"
@@ -71,10 +72,7 @@ inline bool FactorOdom2dCloseloop::operator ()(const T* const _p1, const T* cons
// Error
er = expected_measurement - getMeasurement().cast<T>();
- while (er(2) > T( M_PI ))
- er(2) -= T( 2 * M_PI );
- while (er(2) < T( -M_PI ))
- er(2) += T( 2 * M_PI );
+ er(2)=pi2pi(er(2));
// Residuals
res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
diff --git a/include/core/factor/factor_pose_2d.h b/include/core/factor/factor_pose_2d.h
index ce63c8060..f11b674e7 100644
--- a/include/core/factor/factor_pose_2d.h
+++ b/include/core/factor/factor_pose_2d.h
@@ -54,11 +54,7 @@ inline bool FactorPose2d::operator ()(const T* const _p, const T* const _o, T* _
Eigen::Matrix<T,3,1> er;
er(0) = meas(0) - _p[0];
er(1) = meas(1) - _p[1];
- er(2) = meas(2) - _o[0];
- while (er[2] > T(M_PI))
- er(2) = er(2) - T(2*M_PI);
- while (er(2) <= T(-M_PI))
- er(2) = er(2) + T(2*M_PI);
+ er(2) = pi2pi(meas(2) - _o[0]);
// residual
Eigen::Map<Eigen::Matrix<T,3,1>> res(_residuals);
diff --git a/include/core/factor/factor_relative_2d_analytic.h b/include/core/factor/factor_relative_2d_analytic.h
index 5a67da1ff..2dba90bc6 100644
--- a/include/core/factor/factor_relative_2d_analytic.h
+++ b/include/core/factor/factor_relative_2d_analytic.h
@@ -4,6 +4,7 @@
//Wolf includes
#include "core/factor/factor_analytic.h"
#include "core/landmark/landmark_base.h"
+#include "core/math/rotations.h"
#include <Eigen/StdVector>
namespace wolf {
@@ -19,11 +20,11 @@ class FactorRelative2dAnalytic : public FactorAnalytic
**/
FactorRelative2dAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_ptr,
+ const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp, _frame_ptr, nullptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_ptr->getP(), _frame_ptr->getO(), _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
+ FactorAnalytic(_tp, _frame_other_ptr, nullptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_other_ptr->getP(), _frame_other_ptr->getO(), _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
//
}
@@ -45,11 +46,11 @@ class FactorRelative2dAnalytic : public FactorAnalytic
**/
FactorRelative2dAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
- const LandmarkBasePtr& _landmark_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp, nullptr, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _landmark_ptr->getP(), _landmark_ptr->getO())
+ FactorAnalytic(_tp, nullptr, nullptr, nullptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _landmark_other_ptr->getP(), _landmark_other_ptr->getO())
{
//
}
@@ -113,10 +114,7 @@ inline Eigen::VectorXd FactorRelative2dAnalytic::evaluateResiduals(
expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
// Residual
residual = expected_measurement - getMeasurement();
- while (residual(2) > M_PI)
- residual(2) = residual(2) - 2 * M_PI;
- while (residual(2) <= -M_PI)
- residual(2) = residual(2) + 2 * M_PI;
+ residual(2) = pi2pi(residual(2));
residual = getMeasurementSquareRootInformationUpper() * residual;
return residual;
}
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 59e73405f..be13135c9 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -4,6 +4,7 @@
//Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
//#include "ceres/jet.h"
@@ -16,35 +17,37 @@ class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativeP
{
public:
FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
- _frame_other_ptr, nullptr, nullptr, nullptr,
- _processor_ptr,
- _apply_loss_function, _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getCapture()->getSensorP(),
- _ftr_ptr->getCapture()->getSensorO())
+ _frame_other_ptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
{
+ assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
//
}
virtual ~FactorRelativePose2dWithExtrinsics() = default;
- template<typename T>
- bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, const T* const _sp, const T* const _so,
- T* _residuals) const;
-
- public:
- static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
+ virtual std::string getTopology() const override
{
- return std::make_shared<FactorRelativePose2dWithExtrinsics>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
+ return std::string("MOTION");
}
+ template<typename T>
+ bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, const T* const _sp, const T* const _so,
+ T* _residuals) const;
};
template<typename T>
@@ -85,10 +88,7 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1,
// Error
er = expected_measurement - getMeasurement().cast<T>();
- while (er(2) > T( M_PI ))
- er(2) -= T( 2 * M_PI );
- while (er(2) < T( -M_PI ))
- er(2) += T( 2 * M_PI );
+ er(2) = pi2pi(er(2));
// Residuals
res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
diff --git a/include/core/math/rotations.h b/include/core/math/rotations.h
index 7c2ede927..a0a86c6ee 100644
--- a/include/core/math/rotations.h
+++ b/include/core/math/rotations.h
@@ -27,7 +27,7 @@ pi2pi(const T _angle)
T angle = _angle;
while (angle > T( M_PI ))
angle -= T( 2 * M_PI );
- while (angle < T( -M_PI ))
+ while (angle <= T( -M_PI ))
angle += T( 2 * M_PI );
return angle;
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 501965cdf..dc7cfae42 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -177,6 +177,10 @@ target_link_libraries(gtest_factor_block_difference ${PROJECT_NAME})
wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
target_link_libraries(gtest_factor_diff_drive ${PROJECT_NAME})
+# FactorOdom2d class test
+wolf_add_gtest(gtest_factor_odom_2d gtest_factor_odom_2d.cpp)
+target_link_libraries(gtest_factor_odom_2d ${PROJECT_NAME})
+
# FactorOdom3d class test
wolf_add_gtest(gtest_factor_odom_3d gtest_factor_odom_3d.cpp)
target_link_libraries(gtest_factor_odom_3d ${PROJECT_NAME})
@@ -189,6 +193,14 @@ target_link_libraries(gtest_factor_pose_2d ${PROJECT_NAME})
wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
target_link_libraries(gtest_factor_pose_3d ${PROJECT_NAME})
+# FactorRelativePose2dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
+target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PROJECT_NAME})
+
+# FactorRelative2dAnalytic class test
+wolf_add_gtest(gtest_factor_relative_2d_analytic gtest_factor_relative_2d_analytic.cpp)
+target_link_libraries(gtest_factor_relative_2d_analytic ${PROJECT_NAME})
+
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp
new file mode 100644
index 000000000..76d559a76
--- /dev/null
+++ b/test/gtest_factor_odom_2d.cpp
@@ -0,0 +1,109 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/factor/factor_odom_2d.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/math/rotations.h"
+
+#include "core/ceres_wrapper/ceres_manager.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+// Input odometry data and covariance
+Matrix3d data_cov = 0.1*Matrix3d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+CeresManager ceres_mgr(problem_ptr);
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
+
+// Capture, feature and factor from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
+auto fac1 = FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false); // create and add
+
+TEST(FactorOdom2d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorOdom2d, fix_0_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+
+ // Fix frm0, perturb frm1
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(5);
+
+ // solve for frm1
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getState(), x1, 1e-6);
+ }
+}
+
+TEST(FactorOdom2d, fix_1_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+
+ // Fix frm1, perturb frm0
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(5);
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getState(), x0, 1e-6);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_2d_analytic.cpp
new file mode 100644
index 000000000..fd8e2296c
--- /dev/null
+++ b/test/gtest_factor_relative_2d_analytic.cpp
@@ -0,0 +1,109 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/factor/factor_relative_2d_analytic.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/math/rotations.h"
+
+#include "core/ceres_wrapper/ceres_manager.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+// Input odometry data and covariance
+Matrix3d data_cov = 0.1*Matrix3d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+CeresManager ceres_mgr(problem_ptr);
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
+
+// Capture, feature and factor from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
+auto fac1 = FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false); // create and add
+
+TEST(FactorRelative2dAnalytic, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorRelative2dAnalytic, fix_0_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+
+ // Fix frm0, perturb frm1
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(5);
+
+ // solve for frm1
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getState(), x1, 1e-6);
+ }
+}
+
+TEST(FactorRelative2dAnalytic, fix_1_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+
+ // Fix frm1, perturb frm0
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(5);
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getState(), x0, 1e-6);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
new file mode 100644
index 000000000..547b4c9b7
--- /dev/null
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -0,0 +1,219 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/math/rotations.h"
+
+#include "core/ceres_wrapper/ceres_manager.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+// Input odometry data and covariance
+Matrix3d data_cov = 0.1*Matrix3d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+CeresManager ceres_mgr(problem_ptr);
+
+// Sensor
+auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ProcessorParamsOdom2d>());
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
+
+// Capture, feature and factor from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
+auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
+auto fac1 = FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+
+TEST(FactorRelativePose2dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+ sensor_odom2d->setState(xs);
+
+ // Perturb frm1, fix the rest
+ frm0->fix();
+ frm1->unfix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->fix();
+ frm1->perturb(5);
+
+ // solve for frm1
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getState(), x1, 1e-6);
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+ sensor_odom2d->setState(xs);
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->fix();
+ frm0->perturb(5);
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getState(), x0, 1e-6);
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+ sensor_odom2d->setState(xs);
+
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor_odom2d->getP()->unfix();
+ sensor_odom2d->getO()->fix();
+ sensor_odom2d->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(sensor_odom2d->getState(), xs, 1e-6);
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+ sensor_odom2d->setState(xs);
+
+ //std::cout << "Sensor O: " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+
+ // Perturb sensor O, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->unfix();
+ sensor_odom2d->getO()->perturb(.2);
+
+ //std::cout << "Sensor O (perturbed): " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(sensor_odom2d->getState(), xs, 1e-6);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
--
GitLab