diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp index a9146cc7e3f49d70a208439c83b709646828d924..a81b36918ce96fa8d47d6387cdae5a24a6a95800 100644 --- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp +++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp @@ -117,6 +117,8 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test 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); + // somehow by default the sensor extrinsics is fixed + sensor_pose_->unfixExtrinsics(); } void TearDown() override{}; @@ -258,15 +260,9 @@ TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, check_tree) TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, solve) { - // somehow by default the sensor extrinsics is fixed - // sensor_pose_->unfixExtrinsics(); - problem_->perturb(); - std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); - problem_->print(4,1,1,1); - 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); @@ -283,11 +279,7 @@ TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, check TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, solve) { - // somehow by default the sensor extrinsics is fixed - sensor_pose_->unfixExtrinsics(); - problem_->perturb(); - std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_); @@ -306,11 +298,7 @@ TEST_F(FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test, check_tree) TEST_F(FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test, solve) { - // somehow by default the sensor extrinsics is fixed - sensor_pose_->unfixExtrinsics(); - problem_->perturb(); - std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_); diff --git a/test/gtest_processor_pose.cpp b/test/gtest_processor_pose.cpp deleted file mode 100644 index 5858f75073201190eca2757e50d7d84df1821996..0000000000000000000000000000000000000000 --- a/test/gtest_processor_pose.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/** - * \file gtest_factor_pose_with_extrinsics.cpp - * - * Created on: Feb a9, 2020 - * \author: mfourmy - */ - -#include "core/utils/utils_gtest.h" -#include "core/ceres_wrapper/solver_ceres.h" -#include "core/capture/capture_base.h" -#include "core/capture/capture_pose.h" -#include "core/sensor/sensor_pose.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" - - -using namespace Eigen; -using namespace wolf; -using std::cout; -using std::endl; - -const Vector3d zero3 = Vector3d::Zero(); - -/** -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)); -} - -TEST_F(FactorPose3dWithExtrinsics_Test, solve) -{ - // somehow by default the sensor extrinsics is fixed - sensor_pose_->unfixExtrinsics(); - - problem_->perturb(); - - std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); - - // problem_->print(4,1,1,1); - - 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); - -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file