Implemented factor and gtest

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

include/core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h

+#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_ */

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})

test/gtest_factor_kf_lmk_pose_3d_with_extrinsics.cpp

+#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();
+}
+