diff --git a/include/core/factor/factor_pose_3d_with_extrinsics.h b/include/core/factor/factor_pose_3d_with_extrinsics.h
index cecb1c32f5db2dff405b874e4a7306beece32b1a..f726291a9a0480dd2edc71d137511c543d4d3886 100644
--- a/include/core/factor/factor_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_pose_3d_with_extrinsics.h
@@ -76,12 +76,6 @@ inline bool FactorPose3dWithExtrinsics::operator ()(const T* const _p,
err.head(3) = w_p_wm - (w_p_wb + w_q_b*b_p_bm);
err.tail(3) = q2v((w_q_b * b_q_m).conjugate() * w_q_m.cast<T>());
- std::cout << "\n\n\n\nFAC" << std::endl;
- std::cout << "w_p_wm: " << w_p_wm.transpose() << std::endl;
- std::cout << "(w_p_wb + w_q_b*b_p_bm): " << (w_p_wb + w_q_b*b_p_bm).transpose() << std::endl;
- std::cout << "b_p_bm: " << b_p_bm.transpose() << std::endl;
- std::cout << "err: " << err.transpose() << std::endl;
-
// Residuals
Eigen::Map<Eigen::Matrix<T,6,1> > res(_residuals);
res = getMeasurementSquareRootInformationUpper().cast<T>() * err;
diff --git a/test/gtest_factor_pose_3d_with_extrinsics.cpp b/test/gtest_factor_pose_3d_with_extrinsics.cpp
index aeeba81efd6feb7674f6efe47289b831ccfcd297..5858f75073201190eca2757e50d7d84df1821996 100644
--- a/test/gtest_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_pose_3d_with_extrinsics.cpp
@@ -7,10 +7,12 @@
#include "core/utils/utils_gtest.h"
#include "core/ceres_wrapper/solver_ceres.h"
-#include "core/factor/factor_relative_pose_3d.h"
+#include "core/capture/capture_base.h"
+#include "core/capture/capture_pose.h"
#include "core/sensor/sensor_pose.h"
-#include "core/capture/capture_odom_3d.h"
#include "core/processor/processor_pose.h"
+#include "core/factor/factor_relative_pose_3d.h"
+#include "core/capture/capture_odom_3d.h"
#include "core/factor/factor_pose_3d_with_extrinsics.h"
@@ -21,111 +23,145 @@ using std::endl;
const Vector3d zero3 = Vector3d::Zero();
-
-// 2 random 3d positions
-Vector3d w_p_wb1 = Vector3d::Random();
-Quaterniond w_q_b1 = Quaterniond::UnitRandom();
-Vector3d w_p_wb2 = Vector3d::Random();
-Quaterniond w_q_b2 = Quaterniond::UnitRandom();
-
-// Vector3d w_p_wb1 = Vector3d::Zero();
-// Quaterniond w_q_b1 = Quaterniond::Identity();
-// Vector3d w_p_wb2 = Vector3d::Ones();
-// Quaterniond w_q_b2 = Quaterniond::Identity();
-// Quaterniond w_q_b2(0,0,0,1);
-// Quaterniond w_q_b2 = Quaterniond::UnitRandom();
-
-// pose vectors
-Vector7d pose1((Vector7d() << w_p_wb1, w_q_b1.coeffs()).finished());
-Vector7d pose2((Vector7d() << w_p_wb2, w_q_b2.coeffs()).finished());
-// unitary covariance
-Matrix6d cov_odom = pow(0.01,2)*Matrix6d::Identity();
-Matrix6d cov_mocap = pow(0.0001,2)*Matrix6d::Identity();
-
-/////////////////////////////////////////
-// extrinsics ground truth and mocap data
-// Vector3d b_p_bm = Vector3d::Zero();
-// Vector3d b_p_bm = 0.1*Vector3d::Ones();
-Vector3d b_p_bm((Vector3d() << 0.1,0,0).finished());
-// Vector3d b_p_bm = Vector3d::Random();
-// Quaterniond b_q_m = Quaterniond::Identity();
-Quaterniond b_q_m(0,0,0,1);
-// Quaterniond b_q_m = Quaterniond::UnitRandom();
-
-Vector3d w_p_wb1_meas = w_p_wb1 + w_q_b1 * b_p_bm;
-Vector3d w_p_wb2_meas = w_p_wb2 + w_q_b2 * b_p_bm;
-Quaterniond w_q_b1_meas = w_q_b1 * b_q_m;
-Quaterniond w_q_b2_meas = w_q_b2 * b_q_m;
-Vector7d pose1_meas((Vector7d() << w_p_wb1_meas, w_q_b1_meas.coeffs()).finished());
-Vector7d pose2_meas((Vector7d() << w_p_wb2_meas, w_q_b2_meas.coeffs()).finished());
-
-/////////////////////
-// relative odom data
-Vector3d b1_p_b1b2 = w_q_b1.conjugate()*(w_p_wb2 - w_p_wb1);
-Quaterniond b1_q_b2 = w_q_b1.conjugate()*w_q_b2;
-Vector7d pose_12((Vector7d() << b1_p_b1b2, b1_q_b2.coeffs()).finished());
-
-// Problem and solver
-ProblemPtr problem = Problem::create("PO", 3);
-SolverCeres solver(problem);
-
-// pose sensor and proc (to get extrinsics in the prb)
-auto intr_sp = std::make_shared<ParamsSensorPose>();
-Vector7d extr = (Vector7d() << b_p_bm, b_q_m.coeffs()).finished();
-auto sensor_pose = problem->installSensor("SensorPose", "pose", extr, intr_sp);
-auto params_proc = std::make_shared<ParamsProcessorPose>();
-auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc);
-
-// Two frames
-FrameBasePtr KF1 = problem->emplaceFrame(1, pose1);
-FrameBasePtr KF2 = problem->emplaceFrame(2, pose2);
-
-
-///////////////////
-// Create factor graph
-// Odometry capture, feature and factor
-auto cap_odom = CaptureBase::emplace<CapturePose>(KF2, 2, nullptr, pose_12, cov_odom);
-auto fea_odom = FeatureBase::emplace<FeatureBase>(cap_odom, "odom", pose_12, cov_odom);
-FactorRelativePose3dPtr fac_odom = FactorBase::emplace<FactorRelativePose3d>(fea_odom, fea_odom, KF1, nullptr, false, TOP_MOTION);
-
-// Captures mocap
-auto cap_mocap1 = CaptureBase::emplace<CapturePose>(KF1, 1, sensor_pose, pose1_meas, cov_mocap);
-auto fea_mocap1 = FeatureBase::emplace<FeatureBase>(cap_mocap1, "pose", pose1_meas, cov_mocap);
-FactorPose3dWithExtrinsicsPtr fac_mocap1 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap1, fea_mocap1, nullptr, false, TOP_MOTION);
-
-auto cap_mocap2 = CaptureBase::emplace<CapturePose>(KF2, 2, sensor_pose, pose2_meas, cov_mocap);
-auto fea_mocap2 = FeatureBase::emplace<FeatureBase>(cap_mocap2, "pose", pose2_meas, cov_mocap);
-FactorPose3dWithExtrinsicsPtr fac_mocap2 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap2, fea_mocap2, nullptr, false, TOP_MOTION);
-
-
-
-TEST(FactorPose3dWithExtrinsics, check_tree)
+/**
+Factor graph implemented for the test
+2 KF is not enough since the SensorPose extrinsics translation would be unobservable along the first KF1-KF2 transformation axis of rotation
+ (KF1)----|Odom3d|----(KF2)----|Odom3d|----(KF3)
+ | | |
+ | | |
+ |Pose3dWE| |Pose3dWE| |Pose3dWE|
+ \ /
+ \ /
+ \____________(Extrinsics)_______________/
+*/
+
+class FactorPose3dWithExtrinsics_Test : public testing::Test
+{
+ public:
+
+ ProblemPtr problem_;
+ SolverCeresPtr solver_;
+ SensorBasePtr sensor_pose_;
+ FrameBasePtr KF1_;
+ FrameBasePtr KF2_;
+ FrameBasePtr KF3_;
+
+ Vector7d pose1_;
+ Vector7d pose2_;
+ Vector7d pose3_;
+ Vector3d b_p_bm_;
+ Quaterniond b_q_m_;
+
+ void SetUp() override
+ {
+ // 2 random 3d positions
+ Vector3d w_p_wb1 = Vector3d::Random();
+ Quaterniond w_q_b1 = Quaterniond::UnitRandom();
+ Vector3d w_p_wb2 = Vector3d::Random();
+ Quaterniond w_q_b2 = Quaterniond::UnitRandom();
+ Vector3d w_p_wb3 = Vector3d::Random();
+ Quaterniond w_q_b3 = Quaterniond::UnitRandom();
+
+ // pose vectors
+ pose1_ << w_p_wb1, w_q_b1.coeffs();
+ pose2_ << w_p_wb2, w_q_b2.coeffs();
+ pose3_ << w_p_wb3, w_q_b3.coeffs();
+ // unitary covariance
+ Matrix6d cov_odom = pow(0.1,2)*Matrix6d::Identity();
+ Matrix6d cov_mocap = pow(0.001,2)*Matrix6d::Identity();
+
+ /////////////////////////////////////////
+ // extrinsics ground truth and mocap data
+ b_p_bm_ << 0.1,0.2,0.3;
+ b_q_m_ = Quaterniond::UnitRandom();
+
+ Vector3d w_p_wb1_meas = w_p_wb1 + w_q_b1 * b_p_bm_;
+ Vector3d w_p_wb2_meas = w_p_wb2 + w_q_b2 * b_p_bm_;
+ Vector3d w_p_wb3_meas = w_p_wb3 + w_q_b3 * b_p_bm_;
+ Quaterniond w_q_b1_meas = w_q_b1 * b_q_m_;
+ Quaterniond w_q_b2_meas = w_q_b2 * b_q_m_;
+ Quaterniond w_q_b3_meas = w_q_b3 * b_q_m_;
+ Vector7d pose1__meas; pose1__meas << w_p_wb1_meas, w_q_b1_meas.coeffs();
+ Vector7d pose2__meas; pose2__meas << w_p_wb2_meas, w_q_b2_meas.coeffs();
+ Vector7d pose3__meas; pose3__meas << w_p_wb3_meas, w_q_b3_meas.coeffs();
+
+ /////////////////////
+ // relative odom data
+ Vector3d b1_p_b1b2 = w_q_b1.conjugate()*(w_p_wb2 - w_p_wb1);
+ Quaterniond b1_q_b2 = w_q_b1.conjugate()*w_q_b2;
+ Vector7d pose_12; pose_12 << b1_p_b1b2, b1_q_b2.coeffs();
+
+ Vector3d b2_p_b2b3 = w_q_b2.conjugate()*(w_p_wb3 - w_p_wb2);
+ Quaterniond b2_q_b3 = w_q_b2.conjugate()*w_q_b3;
+ Vector7d pose_23; pose_23 << b2_p_b2b3, b2_q_b3.coeffs();
+
+ // Problem and solver_
+ problem_ = Problem::create("PO", 3);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+
+ // pose sensor and proc (to get extrinsics in the prb)
+ auto intr_sp = std::make_shared<ParamsSensorPose>();
+ Vector7d extr; extr << b_p_bm_, b_q_m_.coeffs();
+ sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
+ auto params_proc = std::make_shared<ParamsProcessorPose>();
+ auto proc_pose = problem_->installProcessor("ProcessorPose", "pose", sensor_pose_, params_proc);
+
+ // Two frames
+ KF1_ = problem_->emplaceFrame(1, pose1_);
+ KF2_ = problem_->emplaceFrame(2, pose2_);
+ KF3_ = problem_->emplaceFrame(3, pose3_);
+
+ ///////////////////
+ // Create factor graph
+ // Odometry capture, feature and factor
+ auto cap_odom1 = CaptureBase::emplace<CapturePose>(KF2_, 2, nullptr, pose_12, cov_odom);
+ auto fea_odom1 = FeatureBase::emplace<FeatureBase>(cap_odom1, "odom", pose_12, cov_odom);
+ FactorRelativePose3dPtr fac_odomA = FactorBase::emplace<FactorRelativePose3d>(fea_odom1, fea_odom1, KF1_, nullptr, false, TOP_MOTION);
+
+ auto cap_odom2 = CaptureBase::emplace<CapturePose>(KF3_, 3, nullptr, pose_23, cov_odom);
+ auto fea_odom2 = FeatureBase::emplace<FeatureBase>(cap_odom2, "odom", pose_23, cov_odom);
+ FactorRelativePose3dPtr fac_odom2 = FactorBase::emplace<FactorRelativePose3d>(fea_odom2, fea_odom2, KF2_, nullptr, false, TOP_MOTION);
+
+ // Captures mocap
+ auto cap_mocap1 = CaptureBase::emplace<CapturePose>(KF1_, 1, sensor_pose_, pose1__meas, cov_mocap);
+ auto fea_mocap1 = FeatureBase::emplace<FeatureBase>(cap_mocap1, "pose", pose1__meas, cov_mocap);
+ FactorPose3dWithExtrinsicsPtr fac_mocap1 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap1, fea_mocap1, nullptr, false, TOP_MOTION);
+
+ auto cap_mocap2 = CaptureBase::emplace<CapturePose>(KF2_, 2, sensor_pose_, pose2__meas, cov_mocap);
+ auto fea_mocap2 = FeatureBase::emplace<FeatureBase>(cap_mocap2, "pose", pose2__meas, cov_mocap);
+ FactorPose3dWithExtrinsicsPtr fac_mocap2 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap2, fea_mocap2, nullptr, false, TOP_MOTION);
+
+ auto cap_mocap3 = CaptureBase::emplace<CapturePose>(KF3_, 3, sensor_pose_, pose3__meas, cov_mocap);
+ auto fea_mocap3 = FeatureBase::emplace<FeatureBase>(cap_mocap3, "pose", pose3__meas, cov_mocap);
+ FactorPose3dWithExtrinsicsPtr fac_mocap3 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap3, fea_mocap3, nullptr, false, TOP_MOTION);
+
+ }
+
+ void TearDown() override{};
+};
+
+
+TEST_F(FactorPose3dWithExtrinsics_Test, check_tree)
{
- ASSERT_TRUE(problem->check(0));
+ ASSERT_TRUE(problem_->check(0));
}
-TEST(FactorPose3dWithExtrinsics, solve)
+TEST_F(FactorPose3dWithExtrinsics_Test, solve)
{
+ // somehow by default the sensor extrinsics is fixed
+ sensor_pose_->unfixExtrinsics();
- // Fix frame 0, perturb frm1
- sensor_pose->unfixExtrinsics();
- // sensor_pose->fixExtrinsics();
- // frm0->setState(x0);
-
-
- problem->print(4, true, true, true);
- problem->perturb();
-
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ problem_->perturb();
- std::cout << report << std::endl;
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
- problem->print(4, true, true, true);
+ // problem_->print(4,1,1,1);
- // ASSERT_MATRIX_APPROX(KF1->getStateVector(), pose1, 1e-6);
- ASSERT_MATRIX_APPROX(KF2->getStateVector(), pose2, 1e-6);
- ASSERT_MATRIX_APPROX(sensor_pose->getP()->getState(), b_p_bm, 1e-6);
- ASSERT_MATRIX_APPROX(sensor_pose->getO()->getState(), b_q_m.coeffs(), 1e-6);
+ SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
+ ASSERT_MATRIX_APPROX(KF1_->getStateVector(), pose1_, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
}