Skip to content
Snippets Groups Projects
Commit 8f4d2f70 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

changed names and format/API generic

parent ac04fac1
No related branches found
No related tags found
2 merge requests!440New tag,!416Resolve "New factor FactorKfLmkPose3dWithExtrinsicsPtr"
Pipeline #7139 failed
Stage: build
    Hide whitespace changes
    Inline Side-by-side
    include/core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h deleted 100644 → 0
    + 0
    − 163
    View file @ ac04fac1
    #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_ */
    include/core/factor/factor_relative_pose_2d_with_extrinsics.h
    + 82
    − 39
    View file @ 8f4d2f70
    ......@@ -16,61 +16,104 @@ WOLF_PTR_TYPEDEFS(FactorRelativePose2dWithExtrinsics);
    class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>
    {
    public:
    /** \brief Class constructor Frame-Frame
    */
    FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
    const FrameBasePtr& _frame_other_ptr,
    const ProcessorBasePtr& _processor_ptr,
    bool _apply_loss_function,
    const FactorTopology& _top,
    FactorStatus _status = FAC_ACTIVE) :
    FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
    _top,
    _ftr_ptr,
    _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!");
    //
    }
    FactorStatus _status = FAC_ACTIVE);
    /** \brief Class constructor Frame-Landmark
    */
    FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
    const LandmarkBasePtr& _lmk_other_ptr,
    const ProcessorBasePtr& _processor_ptr,
    bool _apply_loss_function,
    const FactorTopology& _top = TOP_LMK,
    FactorStatus _status = FAC_ACTIVE);
    ~FactorRelativePose2dWithExtrinsics() override = 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,
    bool operator ()(const T* const _p_ref,
    const T* const _o_ref,
    const T* const _p_target,
    const T* const _o_target,
    const T* const _p_sensor,
    const T* const _o_sensor,
    T* _residuals) const;
    };
    FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
    const FrameBasePtr& _frame_other_ptr,
    const ProcessorBasePtr& _processor_ptr,
    bool _apply_loss_function,
    const FactorTopology& _top,
    FactorStatus _status = FAC_ACTIVE) :
    FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
    _top,
    _ftr_ptr,
    _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!");
    //
    }
    FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
    const LandmarkBasePtr& _lmk_other_ptr,
    const ProcessorBasePtr& _processor_ptr,
    bool _apply_loss_function,
    const FactorTopology& _top,
    FactorStatus _status = FAC_ACTIVE) :
    FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
    _top,
    _ftr_ptr,
    _lmk_other_ptr, nullptr, nullptr, nullptr,
    _processor_ptr,
    _apply_loss_function,
    _status,
    _ftr_ptr->getFrame()->getP(),
    _ftr_ptr->getFrame()->getO(),
    _lmk_other_ptr->getP(),
    _lmk_other_ptr->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!");
    //
    }
    template<typename T>
    inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1,
    const T* const _o1,
    const T* const _p2,
    const T* const _o2,
    const T* const _ps,
    const T* const _os,
    inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_ref,
    const T* const _o_ref,
    const T* const _p_target,
    const T* const _o_target,
    const T* const _p_sensor,
    const T* const _o_sensor,
    T* _residuals) const
    {
    // MAPS
    Eigen::Map<Eigen::Matrix<T,3,1> > res(_residuals);
    Eigen::Map<const Eigen::Matrix<T,2,1> > p1(_p1);
    Eigen::Map<const Eigen::Matrix<T,2,1> > p2(_p2);
    Eigen::Map<const Eigen::Matrix<T,2,1> > ps(_ps);
    T o1 = _o1[0];
    T o2 = _o2[0];
    T os = _os[0];
    Eigen::Map<const Eigen::Matrix<T,2,1> > p_ref(_p_ref);
    Eigen::Map<const Eigen::Matrix<T,2,1> > p_target(_p_target);
    Eigen::Map<const Eigen::Matrix<T,2,1> > p_sensor(_p_sensor);
    T o_ref = _o_ref[0];
    T o_target = _o_target[0];
    T o_sensor = _o_sensor[0];
    Eigen::Matrix<T, 3, 1> expected_measurement;
    Eigen::Matrix<T, 3, 1> er; // error
    ......@@ -91,8 +134,8 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1,
    // s1_p_s1_s2 = s1_R_r1*(r1_p_s1_r1 + r1_R_w*(w_p_r1_w + w_p_w_r2 + w_R_r2*r2_p_r2_s2))
    expected_measurement.head(2) = Eigen::Rotation2D<T>(-os)*(-ps + Eigen::Rotation2D<T>(-o1)*(-p1 + p2 + Eigen::Rotation2D<T>(o2)*ps));
    expected_measurement(2) = o2 - o1;
    expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target + Eigen::Rotation2D<T>(o_target)*p_sensor));
    expected_measurement(2) = o_target - o_ref;
    // Error
    er = expected_measurement - getMeasurement().cast<T>();
    ......
    include/core/factor/factor_relative_pose_3d_with_extrinsics.h 0 → 100644
    + 194
    − 0
    View file @ 8f4d2f70
    #ifndef _FACTOR_TELATIVE_POSE_3D_WITH_EXTRINSICS
    #define _FACTOR_TELATIVE_POSE_3D_WITH_EXTRINSICS
    //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(FactorRelativePose3dWithExtrinsics);
    class FactorRelativePose3dWithExtrinsics : public FactorAutodiff<FactorRelativePose3dWithExtrinsics, 6, 3, 4, 3, 4, 3, 4>
    {
    public:
    /** \brief Class constructor Frame-Frame
    */
    FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
    const FrameBasePtr& _frame_other_ptr,
    const ProcessorBasePtr& _processor_ptr,
    bool _apply_loss_function,
    const FactorTopology& _top,
    FactorStatus _status = FAC_ACTIVE);
    /** \brief Class constructor Frame-Landmark
    */
    FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
    const LandmarkBasePtr& _landmark_other_ptr,
    const ProcessorBasePtr& _processor_ptr,
    bool _apply_loss_function,
    const FactorTopology& _top = TOP_LMK,
    FactorStatus _status = FAC_ACTIVE);
    /** \brief Class Destructor
    */
    ~FactorRelativePose3dWithExtrinsics() override;
    /** brief : compute the residual from the state blocks being iterated by the solver.
    **/
    template<typename T>
    bool operator ()(const T* const _p_ref,
    const T* const _o_ref,
    const T* const _p_target,
    const T* const _o_target,
    const T* const _p_sensor,
    const T* const _o_sensor,
    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
    namespace wolf
    {
    FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
    const FrameBasePtr& _frame_other_ptr,
    const ProcessorBasePtr& _processor_ptr,
    bool _apply_loss_function,
    const FactorTopology& _top,
    FactorStatus _status) :
    FactorAutodiff("FactorKfLmkPose3dWithExtrinsics",
    _top,
    _feature_ptr,
    _frame_other_ptr,
    nullptr,
    nullptr,
    nullptr,
    _processor_ptr,
    _apply_loss_function,
    _status,
    _frame_other_ptr->getP(),
    _frame_other_ptr->getO(),
    _feature_ptr->getFrame()->getP(),
    _feature_ptr->getFrame()->getO(),
    _feature_ptr->getCapture()->getSensorP(),
    _feature_ptr->getCapture()->getSensorO())
    {
    }
    FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
    const LandmarkBasePtr& _landmark_other_ptr,
    const ProcessorBasePtr& _processor_ptr,
    bool _apply_loss_function,
    const FactorTopology& _top,
    FactorStatus _status) :
    FactorAutodiff("FactorKfLmkPose3dWithExtrinsics",
    _top,
    _feature_ptr,
    nullptr,
    nullptr,
    nullptr,
    _landmark_other_ptr,
    _processor_ptr,
    _apply_loss_function,
    _status,
    _feature_ptr->getFrame()->getP(),
    _feature_ptr->getFrame()->getO(),
    _landmark_other_ptr->getP(),
    _landmark_other_ptr->getO(),
    _feature_ptr->getCapture()->getSensorP(),
    _feature_ptr->getCapture()->getSensorO())
    {
    }
    FactorRelativePose3dWithExtrinsics::~FactorRelativePose3dWithExtrinsics()
    {
    //
    }
    template<typename T>
    bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_ref,
    const T* const _o_ref,
    const T* const _p_target,
    const T* const _o_target,
    const T* const _p_sensor,
    const T* const _o_sensor,
    T* _residuals) const
    {
    // Maps
    Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_c(_p_sensor);
    Eigen::Map<const Eigen::Quaternion<T>> q_r_c(_o_sensor);
    Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_ref);
    Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref);
    Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_l(_p_target);
    Eigen::Map<const Eigen::Quaternion<T>> q_w_l(_o_target);
    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 FactorRelativePose3dWithExtrinsics::residual() const
    {
    Eigen::Vector6d res;
    double * p_camera, * o_camera, * p_ref, * o_ref, * p_target, * o_target;
    p_camera = getCapture()->getSensorP()->getState().data();
    o_camera = getCapture()->getSensorO()->getState().data();
    p_ref = getCapture()->getFrame()->getP()->getState().data();
    o_ref = getCapture()->getFrame()->getO()->getState().data();
    p_target = getLandmarkOther()->getP()->getState().data();
    o_target = getLandmarkOther()->getO()->getState().data();
    operator() (p_camera, o_camera, p_ref, o_ref, p_target, o_target, res.data());
    return res;
    }
    double FactorRelativePose3dWithExtrinsics::cost() const
    {
    return residual().squaredNorm();
    }
    } // namespace wolf
    #endif /* _FACTOR_TELATIVE_POSE_3D_WITH_EXTRINSICS */
    test/CMakeLists.txt
    + 3
    − 3
    View file @ 8f4d2f70
    ......@@ -213,9 +213,9 @@ 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})
    # FactorRelativePose3dWithExtrinsics class test
    wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
    target_link_libraries(gtest_factor_relative_pose_3d_with_extrinsics ${PLUGIN_NAME})
    # MakePosDef function test
    wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
    ......
    test/gtest_factor_kf_lmk_pose_3d_with_extrinsics.cpp → test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
    + 56
    − 82
    View file @ 8f4d2f70
    #include <core/factor/factor_relative_pose_3d_with_extrinsics.h>
    #include <core/utils/utils_gtest.h>
    #include "core/common/wolf.h"
    ......@@ -7,7 +8,6 @@
    #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"
    ......@@ -18,7 +18,7 @@ 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{
    class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
    public:
    Vector3d pos_camera, pos_robot, pos_landmark;
    Vector3d euler_camera, euler_robot, euler_landmark;
    ......@@ -34,7 +34,7 @@ class FactorKfLmkPose3dWithExtrinsics_class : public testing::Test{
    CapturePosePtr c1;
    FeaturePosePtr f1;
    LandmarkBasePtr lmk1;
    FactorKfLmkPose3dWithExtrinsicsPtr fac;
    FactorRelativePose3dWithExtrinsicsPtr fac;
    Eigen::Matrix6d meas_cov;
    ......@@ -127,47 +127,36 @@ class FactorKfLmkPose3dWithExtrinsics_class : public testing::Test{
    };
    TEST_F(FactorKfLmkPose3dWithExtrinsics_class, Constructor)
    TEST_F(FactorRelativePose3dWithExtrinsics_class, Constructor)
    {
    FactorKfLmkPose3dWithExtrinsicsPtr factor = std::make_shared<FactorKfLmkPose3dWithExtrinsics>(
    S,
    F1,
    lmk1,
    f1,
    nullptr,
    false,
    FAC_ACTIVE
    );
    ASSERT_TRUE(factor->getType() == "FactorKfLmkPose3dWithExtrinsics");
    auto factor = std::make_shared<FactorRelativePose3dWithExtrinsics>(f1,
    lmk1,
    nullptr,
    false);
    ASSERT_TRUE(factor->getType() == "FactorRelativePose3dWithExtrinsics");
    }
    /////////////////////////////////////////////
    // 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)
    TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_tree)
    {
    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
    f1,
    lmk1,
    nullptr,
    false);
    ASSERT_TRUE(problem->check(1));
    }
    TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_P_perturbated)
    {
    auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1,
    S,
    F1,
    lmk1,
    f1,
    nullptr,
    false,
    FAC_ACTIVE);
    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
    f1,
    lmk1,
    nullptr,
    false);
    // unfix F1, perturbate state
    F1->unfix();
    ......@@ -177,16 +166,13 @@ TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_F1_P_perturbated)
    ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
    }
    TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_F1_O_perturbated)
    TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated)
    {
    auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1,
    S,
    F1,
    lmk1,
    f1,
    nullptr,
    false,
    FAC_ACTIVE);
    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
    f1,
    lmk1,
    nullptr,
    false);
    // unfix F1, perturbate state
    F1->unfix();
    ......@@ -196,16 +182,13 @@ TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_F1_O_perturbated)
    ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
    }
    TEST_F(FactorKfLmkPose3dWithExtrinsics_class, Check_initialization)
    TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization)
    {
    auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1,
    S,
    F1,
    lmk1,
    f1,
    nullptr,
    false,
    FAC_ACTIVE);
    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
    f1,
    lmk1,
    nullptr,
    false);
    ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
    ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6);
    ......@@ -213,16 +196,13 @@ TEST_F(FactorKfLmkPose3dWithExtrinsics_class, Check_initialization)
    }
    TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_P_perturbated)
    TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_P_perturbated)
    {
    auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1,
    S,
    F1,
    lmk1,
    f1,
    nullptr,
    false,
    FAC_ACTIVE);
    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
    f1,
    lmk1,
    nullptr,
    false);
    // unfix lmk1, perturbate state
    lmk1->unfix();
    ......@@ -233,16 +213,13 @@ TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_P_perturbated)
    ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
    }
    TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_O_perturbated)
    TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated)
    {
    auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1,
    S,
    F1,
    lmk1,
    f1,
    nullptr,
    false,
    FAC_ACTIVE);
    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
    f1,
    lmk1,
    nullptr,
    false);
    // unfix F1, perturbate state
    lmk1->unfix();
    ......@@ -254,7 +231,7 @@ TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_O_perturbated)
    }
    TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_PO_perturbated)
    TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_PO_perturbated)
    {
    // Change setup
    Vector3d p_w_r, p_r_c, p_c_l, p_w_l;
    ......@@ -276,14 +253,11 @@ TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_PO_perturbated)
    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);
    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
    f1,
    lmk1,
    nullptr,
    false);
    // Change Landmark
    lmk1->getP()->setState(p_w_l);
    ......
    0% or .
    You are about to add 0 people to the discussion. Proceed with caution.
    Finish editing this message first!
    Please register or to comment