diff --git a/CMakeLists.txt b/CMakeLists.txt index b9f3ad7358ad2dad416d6cf0c8923f23d5a90ece..4103c38189e7e0892959e7be9a52789920330c03 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -228,6 +228,7 @@ SET(HDRS_FACTOR include/core/factor/factor_relative_pose_2d_with_extrinsics.h include/core/factor/factor_relative_pose_3d.h include/core/factor/factor_pose_3d_with_extrinsics.h + include/core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h ) SET(HDRS_FEATURE include/core/feature/feature_base.h diff --git a/include/core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h b/include/core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h new file mode 100644 index 0000000000000000000000000000000000000000..6f5d3c1bf252e4c801506f18c962f0bcae3e6e7f --- /dev/null +++ b/include/core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h @@ -0,0 +1,163 @@ +#ifndef _FACTOR_APRILTAG_H_ +#define _FACTOR_APRILTAG_H_ + +//Wolf includes +#include "core/common/wolf.h" +#include "core/math/rotations.h" +#include "core/factor/factor_autodiff.h" +#include "core/sensor/sensor_base.h" +#include "core/landmark/landmark_base.h" +#include "core/feature/feature_base.h" + +namespace wolf +{ + +WOLF_PTR_TYPEDEFS(FactorKfLmkPose3dWithExtrinsics); + +class FactorKfLmkPose3dWithExtrinsics : public FactorAutodiff<FactorKfLmkPose3dWithExtrinsics, 6, 3, 4, 3, 4, 3, 4> +{ + public: + + /** \brief Class constructor + */ + FactorKfLmkPose3dWithExtrinsics( + const SensorBasePtr& _sensor_ptr, + const FrameBasePtr& _frame_ptr, + const LandmarkBasePtr& _landmark_other_ptr, + const FeatureBasePtr& _feature_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status); + + /** \brief Class Destructor + */ + ~FactorKfLmkPose3dWithExtrinsics() override; + + /** brief : compute the residual from the state blocks being iterated by the solver. + **/ + template<typename T> + bool operator ()( const T* const _p_camera, + const T* const _o_camera, + const T* const _p_keyframe, + const T* const _o_keyframe, + const T* const _p_landmark, + const T* const _o_landmark, + T* _residuals) const; + + Eigen::Vector6d residual() const; + double cost() const; + + // print function only for double (not Jet) + template<typename T, int Rows, int Cols> + void print(int kf, int lmk, const std::string s, const Eigen::Matrix<T, Rows, Cols> _M) const + { + // jet prints nothing + } + template<int Rows, int Cols> + void print(int kf, int lmk, const std::string s, const Eigen::Matrix<double, Rows, Cols> _M) const + { + // double prints stuff + WOLF_TRACE("KF", kf, " L", lmk, "; ", s, _M); + } +}; + +} // namespace wolf + +// Include here all headers for this class +//#include <YOUR_HEADERS.h> + +namespace wolf +{ + +FactorKfLmkPose3dWithExtrinsics::FactorKfLmkPose3dWithExtrinsics( + const SensorBasePtr& _sensor_ptr, + const FrameBasePtr& _frame_ptr, + const LandmarkBasePtr& _landmark_other_ptr, + const FeatureBasePtr& _feature_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status) : + FactorAutodiff("FactorKfLmkPose3dWithExtrinsics", + TOP_LMK, + _feature_ptr, + nullptr, + nullptr, + nullptr, + _landmark_other_ptr, + _processor_ptr, + _apply_loss_function, + _status, + _sensor_ptr->getP(), _sensor_ptr->getO(), + _frame_ptr->getP(), _frame_ptr->getO(), + _landmark_other_ptr->getP(), _landmark_other_ptr->getO() + ) +{ + + +} + +/** \brief Class Destructor + */ +FactorKfLmkPose3dWithExtrinsics::~FactorKfLmkPose3dWithExtrinsics() +{ + // +} + +template<typename T> bool FactorKfLmkPose3dWithExtrinsics::operator ()( const T* const _p_camera, const T* const _o_camera, const T* const _p_keyframe, const T* const _o_keyframe, const T* const _p_landmark, const T* const _o_landmark, T* _residuals) const +{ + // Maps + Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_c(_p_camera); + Eigen::Map<const Eigen::Quaternion<T>> q_r_c(_o_camera); + Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_keyframe); + Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_keyframe); + Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_l(_p_landmark); + Eigen::Map<const Eigen::Quaternion<T>> q_w_l(_o_landmark); + Eigen::Map<Eigen::Matrix<T,6,1>> residuals(_residuals); + + // Expected measurement + Eigen::Quaternion<T> q_c_w = (q_w_r * q_r_c).conjugate(); + Eigen::Quaternion<T> q_c_l = q_c_w * q_w_l; + Eigen::Matrix<T,3,1> p_c_l = q_c_w * (-(p_w_r + q_w_r * p_r_c) + p_w_l); + + // Measurement + Eigen::Vector3d p_c_l_meas(getMeasurement().head<3>()); + Eigen::Quaterniond q_c_l_meas(getMeasurement().data() + 3 ); + Eigen::Quaternion<T> q_l_c_meas = q_c_l_meas.conjugate().cast<T>(); + //Eigen::Matrix<T,3,1> p_l_c_meas = -q_l_c_meas * p_c_l_meas.cast<T>(); + + // Error + Eigen::Matrix<T, 6, 1> err; + err.head(3) = q_l_c_meas * (p_c_l_meas.cast<T>() - p_c_l); + //err.tail(3) = wolf::log_q(q_l_c_meas * q_c_l); // true error function for which the covariance should be computed + err.tail(3) = T(2)*(q_l_c_meas * q_c_l).vec(); // 1rst order approximation of sin function ( 2*sin(aa/2) ~ aa ) + + // Residual + residuals = getMeasurementSquareRootInformationUpper().cast<T>() * err; + + return true; +} + +Eigen::Vector6d FactorKfLmkPose3dWithExtrinsics::residual() const +{ + Eigen::Vector6d res; + double * p_camera, * o_camera, * p_frame, * o_frame, * p_tag, * o_tag; + p_camera = getCapture()->getSensorP()->getState().data(); + o_camera = getCapture()->getSensorO()->getState().data(); + p_frame = getCapture()->getFrame()->getP()->getState().data(); + o_frame = getCapture()->getFrame()->getO()->getState().data(); + p_tag = getLandmarkOther()->getP()->getState().data(); + o_tag = getLandmarkOther()->getO()->getState().data(); + + operator() (p_camera, o_camera, p_frame, o_frame, p_tag, o_tag, res.data()); + + return res; +} + +double FactorKfLmkPose3dWithExtrinsics::cost() const +{ + return residual().squaredNorm(); +} + +} // namespace wolf + +#endif /* _CONSTRAINT_AUTODIFF_APRILTAG_H_ */ diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 8a5eb8fa3a068dca72974a86bdcdee1fd9453baf..df8050d2ad6e8dd9da6d0bab197cc762d29543be 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -213,6 +213,10 @@ target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAM wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp) target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME}) +# FactorRelativePose3d class test +wolf_add_gtest(gtest_factor_kf_lmk_pose_3d_with_extrinsics gtest_factor_kf_lmk_pose_3d_with_extrinsics.cpp) +target_link_libraries(gtest_factor_kf_lmk_pose_3d_with_extrinsics ${PLUGIN_NAME}) + # MakePosDef function test wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp) target_link_libraries(gtest_make_posdef ${PLUGIN_NAME}) diff --git a/test/gtest_factor_kf_lmk_pose_3d_with_extrinsics.cpp b/test/gtest_factor_kf_lmk_pose_3d_with_extrinsics.cpp new file mode 100644 index 0000000000000000000000000000000000000000..481eae173729b619154b9710dd0287af6ae6325b --- /dev/null +++ b/test/gtest_factor_kf_lmk_pose_3d_with_extrinsics.cpp @@ -0,0 +1,328 @@ +#include <core/utils/utils_gtest.h> + +#include "core/common/wolf.h" +#include "core/utils/logging.h" + +#include "core/state_block/state_quaternion.h" +#include "core/ceres_wrapper/solver_ceres.h" +#include "core/capture/capture_pose.h" +#include "core/feature/feature_pose.h" +#include "core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h" + + + +using namespace Eigen; +using namespace wolf; +using std::static_pointer_cast; + + + +// Use the following in case you want to initialize tests with predefines variables or methods. +class FactorKfLmkPose3dWithExtrinsics_class : public testing::Test{ + public: + Vector3d pos_camera, pos_robot, pos_landmark; + Vector3d euler_camera, euler_robot, euler_landmark; + Quaterniond quat_camera, quat_robot, quat_landmark; + Vector4d vquat_camera, vquat_robot, vquat_landmark; // quaternions as vectors + Vector7d pose_camera, pose_robot, pose_landmark; + + ProblemPtr problem; + SolverCeresPtr solver; + + SensorBasePtr S; + FrameBasePtr F1; + CapturePosePtr c1; + FeaturePosePtr f1; + LandmarkBasePtr lmk1; + FactorKfLmkPose3dWithExtrinsicsPtr fac; + + Eigen::Matrix6d meas_cov; + + void SetUp() override + { + // configuration + + /* We have three poses to take into account: + * - pose of the sensor (extrinsincs) + * - pose of the landmark + * - pose of the robot (Keyframe) + * + * The measurement provides the pose of the landmark wrt sensor's current pose in the world. + * Camera's current pose in World is the composition of the robot pose with the sensor extrinsics. + * + * The robot has a sensor looking forward + * S: pos = (0,0,0), ori = (0, 0, 0) + * + * There is a point at the origin + * P: pos = (0,0,0) + * + * Therefore, P projects exactly at the origin of the sensor, + * f: p = (0,0) + * + * We form a Wolf tree with 1 frames F1, 1 capture C1, + * 1 feature f1 (measurement), 1 landmark l and 1 relative kf landmark constraint c1: + * + * Frame F1, Capture C1, feature f1, landmark l1, constraint c1 + * + * The frame pose F1, and the sensor pose S + * in the robot frame are variables subject to optimization + * + * We perform a number of tests based on this configuration.*/ + + + // sensor is at origin of robot and looking forward + // robot is at (0,0,0) + // landmark is right in front of sensor. Its orientation wrt sensor is identity. + pos_camera << 0,0,0; + pos_robot << 0,0,0; //robot is at origin + pos_landmark << 0,1,0; + euler_camera << 0,0,0; + //euler_camera << -M_PI_2, 0, -M_PI_2; + euler_robot << 0,0,0; + euler_landmark << 0,0,0; + quat_camera = e2q(euler_camera); + quat_robot = e2q(euler_robot); + quat_landmark = e2q(euler_landmark); + vquat_camera = quat_camera.coeffs(); + vquat_robot = quat_robot.coeffs(); + vquat_landmark = quat_landmark.coeffs(); + pose_camera << pos_camera, vquat_camera; + pose_robot << pos_robot, vquat_robot; + pose_landmark << pos_landmark, vquat_landmark; + + // Build problem + problem = Problem::create("PO", 3); + solver = std::make_shared<SolverCeres>(problem); + + // Create sensor to be able to initialize (a camera for instance) + S = SensorBase::emplace<SensorBase>(problem->getHardware(), "SensorPose", + std::make_shared<StateBlock>(pos_camera, true), + std::make_shared<StateQuaternion>(vquat_camera, true), + std::make_shared<StateBlock>(Vector4d::Zero(), false), // fixed + Vector1d::Zero()); + + // ParamsSensorCameraPtr params_camera = std::make_shared<ParamsSensorCamera>(); + // S = problem->installSensor("SensorCamera", "canonical", pose_camera, params_camera); + // camera = std::static_pointer_cast<SensorCamera>(S); + + + // F1 is be origin KF + VectorComposite x0(pose_robot, "PO", {3,4}); + VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)}); + F1 = problem->setPriorFactor(x0, s0, 0.0, 0.1); + + + // the sensor is at origin as well as the robot. The measurement matches with the pose of the tag wrt sensor (and also wrt robot and world) + meas_cov = Eigen::Matrix6d::Identity(); + meas_cov.topLeftCorner(3,3) *= 1e-2; + meas_cov.bottomRightCorner(3,3) *= 1e-3; + + //emplace feature and landmark + c1 = std::static_pointer_cast<CapturePose>(CaptureBase::emplace<CapturePose>(F1, 0, nullptr, pose_landmark, meas_cov)); + f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, pose_landmark, meas_cov)); + lmk1 = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkPose", + std::make_shared<StateBlock>(pos_landmark), + std::make_shared<StateQuaternion>(Quaterniond(vquat_landmark))); + } +}; + + +TEST_F(FactorKfLmkPose3dWithExtrinsics_class, Constructor) +{ + FactorKfLmkPose3dWithExtrinsicsPtr factor = std::make_shared<FactorKfLmkPose3dWithExtrinsics>( + S, + F1, + lmk1, + f1, + nullptr, + false, + FAC_ACTIVE + ); + + ASSERT_TRUE(factor->getType() == "FactorKfLmkPose3dWithExtrinsics"); +} + +///////////////////////////////////////////// +// Tree not ok with this gtest implementation +///////////////////////////////////////////// +// TEST_F(FactorKfLmkPose3dWithExtrinsics_class, Check_tree) +// { +// auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1, +// S, +// F1, +// lmk1, +// f1, +// nullptr, +// false, +// FAC_ACTIVE); +// ASSERT_TRUE(problem->check(1)); +// } + +TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_F1_P_perturbated) +{ + auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1, + S, + F1, + lmk1, + f1, + nullptr, + false, + FAC_ACTIVE); + + // unfix F1, perturbate state + F1->unfix(); + F1->getP()->perturb(); + + std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport + ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); +} + +TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_F1_O_perturbated) +{ + auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1, + S, + F1, + lmk1, + f1, + nullptr, + false, + FAC_ACTIVE); + + // unfix F1, perturbate state + F1->unfix(); + F1->getO()->perturb(); + + std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport + ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); +} + +TEST_F(FactorKfLmkPose3dWithExtrinsics_class, Check_initialization) +{ + auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1, + S, + F1, + lmk1, + f1, + nullptr, + false, + FAC_ACTIVE); + + ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); + ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6); + ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); + +} + +TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_P_perturbated) +{ + auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1, + S, + F1, + lmk1, + f1, + nullptr, + false, + FAC_ACTIVE); + + // unfix lmk1, perturbate state + lmk1->unfix(); + lmk1->getP()->perturb(); + + std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport + ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); + ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); +} + +TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_O_perturbated) +{ + auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1, + S, + F1, + lmk1, + f1, + nullptr, + false, + FAC_ACTIVE); + + // unfix F1, perturbate state + lmk1->unfix(); + lmk1->getO()->perturb(); + + std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport + ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); + ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); + +} + +TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_PO_perturbated) +{ + // Change setup + Vector3d p_w_r, p_r_c, p_c_l, p_w_l; + Quaterniond q_w_r, q_r_c, q_c_l, q_w_l; + p_w_r << 1, 2, 3; + p_r_c << 4, 5, 6; + p_c_l << 7, 8, 9; + q_w_r.coeffs() << 1, 2, 3, 4; q_w_r.normalize(); + q_r_c.coeffs() << 4, 5, 6, 7; q_r_c.normalize(); + q_c_l.coeffs() << 7, 8, 9, 0; q_c_l.normalize(); + + q_w_l = q_w_r * q_r_c * q_c_l; + p_w_l = p_w_r + q_w_r * (p_r_c + q_r_c * p_c_l); + + // Change feature (remove and emplace) + Vector7d meas; + meas << p_c_l, q_c_l.coeffs(); + f1->remove(); + f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, meas, meas_cov)); + + // emplace factor + auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1, + S, + F1, + lmk1, + f1, + nullptr, + false, + FAC_ACTIVE); + + // Change Landmark + lmk1->getP()->setState(p_w_l); + lmk1->getO()->setState(q_w_l.coeffs()); + ASSERT_TRUE(lmk1->getP()->stateUpdated()); + ASSERT_TRUE(lmk1->getO()->stateUpdated()); + + // Change Frame + F1->getP()->setState(p_w_r); + F1->getO()->setState(q_w_r.coeffs()); + F1->fix(); + ASSERT_TRUE(F1->getP()->stateUpdated()); + ASSERT_TRUE(F1->getO()->stateUpdated()); + + // Change sensor extrinsics + S->getP()->setState(p_r_c); + S->getO()->setState(q_r_c.coeffs()); + ASSERT_TRUE(S->getP()->stateUpdated()); + ASSERT_TRUE(S->getO()->stateUpdated()); + + Vector7d t_w_r, t_w_l; + t_w_r << p_w_r, q_w_r.coeffs(); + t_w_l << p_w_l, q_w_l.coeffs(); + ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), t_w_r, 1e-6); + ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), t_w_l, 1e-6); + + // unfix LMK, perturbate state + lmk1->unfix(); + lmk1->perturb(); + + std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport + ASSERT_MATRIX_APPROX(F1->getState().vector("PO").transpose(), t_w_r.transpose(), 1e-6); + ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO").transpose(), t_w_l.transpose(), 1e-6); + +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +