Merge branch 'devel' into 19-building-a-new-visual-odometry-system

.gitlab-ci.yml

@@ -114,8 +114,6 @@ license_headers:
     - key: wolf-focal
       paths:
       - ci_deps/wolf/
-  except:
-    - master
   before_script:
     - *preliminaries_definition
     - *install_wolf_definition
@@ -133,8 +131,6 @@ build_and_test:bionic:
     - key: visionutils-bionic
       paths:
       - ci_deps/vision_utils/
-  except:
-    - master
   before_script:
     - *preliminaries_definition
     - *install_wolf_definition
@@ -154,8 +150,6 @@ build_and_test:focal:
     - key: visionutils-focal
       paths:
       - ci_deps/vision_utils/
-  except:
-    - master
   before_script:
     - *preliminaries_definition
     - *install_wolf_definition

cmake_modules/wolfvisionConfig.cmake

@@ -48,7 +48,11 @@ macro(wolf_report_not_found REASON_MSG)
   # FindPackage() use the camelcase library name, not uppercase.
   if (wolfvision_FIND_QUIETLY)
     message(STATUS "Failed to find wolfvision- " ${REASON_MSG} ${ARGN})
+<<<<<<< HEAD
   elseif(wolfvision_FIND_REQUIRED)
+=======
+  elseif (wolfvision_FIND_REQUIRED)
+>>>>>>> devel
     message(FATAL_ERROR "Failed to find wolfvision - " ${REASON_MSG} ${ARGN})
   else()
     # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error

demos/demo_processor_bundle_adjustment.cpp

@@ -173,9 +173,9 @@ int main(int argc, char** argv)
         camera->process(image);
 
         // solve only when new KFs are added
-        if (problem->getTrajectory()->getFrameMap().size() > number_of_KFs)
+        if (problem->getTrajectory()->size() > number_of_KFs)
         {
-            number_of_KFs = problem->getTrajectory()->getFrameMap().size();
+            number_of_KFs = problem->getTrajectory()->size();
             std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
             std::cout << report << std::endl;
             if (number_of_KFs > 5)

include/vision/capture/capture_image.h

@@ -124,7 +124,7 @@ class CaptureImage : public CaptureBase
         const TracksMap& getTracksOrigin() const {return tracks_origin_;}
         void setTracksOrigin(const TracksMap& _tracks){tracks_origin_ = _tracks;}
 
-        bool getLastWasRepopulated(){return last_was_repopulated_;}
+        bool getLastWasRepopulated() const {return last_was_repopulated_;}
         void setLastWasRepopulated(bool _repopulated){last_was_repopulated_ = _repopulated;}
 
         void addKeyPoint(const WKeyPoint& _wkp);

include/vision/factor/factor_ahp.h

@@ -103,9 +103,9 @@ inline FactorAhp::FactorAhp(const FeatureBasePtr&   _ftr_ptr,
 
 inline Eigen::VectorXd FactorAhp::expectation() const
 {
-    FrameBasePtr frm_current = getFeature()->getCapture()->getFrame();
-    FrameBasePtr frm_anchor  = getFrameOther();
-    LandmarkBasePtr lmk      = getLandmarkOther();
+    auto frm_current = getFeature()->getCapture()->getFrame();
+    auto frm_anchor  = getFrameOther();
+    auto lmk         = getLandmarkOther();
 
     const Eigen::MatrixXd frame_current_pos = frm_current->getP()->getState();
     const Eigen::MatrixXd frame_current_ori = frm_current->getO()->getState();
@@ -151,7 +151,7 @@ inline void FactorAhp::expectation(const T* const _current_frame_p,
     TransformType       T_R0_C0 = t_r0_c0 * q_r0_c0;
 
     // current robot to current camera transform
-    CaptureBasePtr      current_capture = this->getFeature()->getCapture();
+    auto                current_capture = this->getFeature()->getCapture();
     Translation<T, 3>   t_r1_c1  (current_capture->getSensorP()->getState().cast<T>());
     Quaterniond         q_r1_c1_s(Eigen::Vector4d(current_capture->getSensorO()->getState()));
     Quaternion<T>       q_r1_c1 = q_r1_c1_s.cast<T>();

include/vision/factor/factor_pixel_hp.h

@@ -99,9 +99,9 @@ inline FactorPixelHp::FactorPixelHp(const FeatureBasePtr&   _ftr_ptr,
 
 inline Eigen::VectorXd FactorPixelHp::expectation() const
 {
-    FrameBasePtr    frm = getFeature()->getCapture()->getFrame();
-    SensorBasePtr   sen = getFeature()->getCapture()->getSensor();
-    LandmarkBasePtr lmk = getLandmarkOther();
+    auto frm  = getFeature()->getCapture()->getFrame();
+    auto sen  = getFeature()->getCapture()->getSensor();
+    auto lmk  = getLandmarkOther();
 
     const Eigen::MatrixXd frame_pos     = frm->getP()->getState();
     const Eigen::MatrixXd frame_ori     = frm->getO()->getState();

include/vision/factor/factor_trifocal.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef _FACTOR_TRIFOCAL_H_
+#define _FACTOR_TRIFOCAL_H_
+
+//Wolf includes
+//#include "core/common/wolf.h"
+#include "core/factor/factor_autodiff.h"
+#include "vision/sensor/sensor_camera.h"
+
+#include <vision_utils/common_class/trifocaltensor.h>
+#include <vision_utils/vision_utils.h>
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorTrifocal);
+
+using namespace Eigen;
+
+class FactorTrifocal : public FactorAutodiff<FactorTrifocal, 3, 3, 4, 3, 4, 3, 4, 3, 4>
+{
+    public:
+
+        /** \brief Class constructor
+         */
+        FactorTrifocal(const FeatureBasePtr& _feature_1_ptr,
+                       const FeatureBasePtr& _feature_2_ptr,
+                       const FeatureBasePtr& _feature_own_ptr,
+                       const ProcessorBasePtr& _processor_ptr,
+                       bool _apply_loss_function,
+                       FactorStatus _status);
+
+        /** \brief Class Destructor
+         */
+        ~FactorTrifocal() override;
+
+        FeatureBaseConstPtr getFeaturePrev() const;
+        FeatureBasePtr getFeaturePrev();
+
+        const Vector3d& getPixelCanonical3() const
+        {
+            return pixel_canonical_3_;
+        }
+
+        void setPixelCanonical3(const Vector3d& pixelCanonical3)
+        {
+            pixel_canonical_3_ = pixelCanonical3;
+        }
+
+        const Vector3d& getPixelCanonical2() const
+        {
+            return pixel_canonical_2_;
+        }
+
+        void setPixelCanonical2(const Vector3d& pixelCanonical2)
+        {
+            pixel_canonical_2_ = pixelCanonical2;
+        }
+
+        const Vector3d& getPixelCanonical1() const
+        {
+            return pixel_canonical_1_;
+        }
+
+        void setPixelCanonical1(const Vector3d& pixelCanonical1)
+        {
+            pixel_canonical_1_ = pixelCanonical1;
+        }
+
+        const Matrix3d& getSqrtInformationUpper() const
+        {
+            return sqrt_information_upper;
+        }
+
+        /** brief : compute the residual from the state blocks being iterated by the solver.
+         **/
+        template<typename T>
+        bool operator ()( const T* const _pos1,
+                          const T* const _quat1,
+                          const T* const _pos2,
+                          const T* const _quat2,
+                          const T* const _pos3,
+                          const T* const _quat3,
+                          const T* const _sen_pos,
+                          const T* const _sen_quat,
+                          T*             _residuals) const;
+
+    public:
+        template<typename D1, typename D2, class T, typename D3>
+        void expectation(const MatrixBase<D1>&     _wtr1,
+                         const QuaternionBase<D2>& _wqr1,
+                         const MatrixBase<D1>&     _wtr2,
+                         const QuaternionBase<D2>& _wqr2,
+                         const MatrixBase<D1>&     _wtr3,
+                         const QuaternionBase<D2>& _wqr3,
+                         const MatrixBase<D1>&     _rtc,
+                         const QuaternionBase<D2>& _rqc,
+                         vision_utils::TrifocalTensorBase<T>& _tensor,
+                         MatrixBase<D3>&           _c2Ec1) const;
+
+        template<typename T, typename D1>
+        Matrix<T, 3, 1> residual(const vision_utils::TrifocalTensorBase<T>& _tensor,
+                                 const MatrixBase<D1>& _c2Ec1) const;
+
+        // Helper functions to be used by the above
+        template<class T, typename D1, typename D2, typename D3, typename D4>
+        Matrix<T, 3, 1> error_jacobians(const vision_utils::TrifocalTensorBase<T>& _tensor,
+                                        const MatrixBase<D1>& _c2Ec1,
+                                        MatrixBase<D2>& _J_e_m1,
+                                        MatrixBase<D3>& _J_e_m2,
+                                        MatrixBase<D4>& _J_e_m3);
+
+    private:
+        FeatureBaseWPtr feature_prev_ptr_;  // To look for measurements
+        SensorCameraPtr camera_ptr_;        // To look for intrinsics
+        Vector3d pixel_canonical_1_, pixel_canonical_2_, pixel_canonical_3_;
+
+        Matrix3d sqrt_information_upper;
+
+        //Print function specialized for doubles (avoid jets)
+        template <class T, int ROWS, int COLS>
+        void print_matrix(const Eigen::Matrix<T, ROWS, COLS>& _mat) const;
+
+        template <int ROWS, int COLS>
+        void print_matrix(const Eigen::Matrix<double, ROWS, COLS>& _mat) const;
+
+        template<class T>
+        void print_scalar(const T& _val) const;
+
+        void print_scalar(const double& _val) const;
+};
+
+} // namespace wolf
+
+// Includes for implentation
+#include "core/math/rotations.h"
+
+namespace wolf
+{
+
+using namespace Eigen;
+
+// Constructor
+FactorTrifocal::FactorTrifocal(const FeatureBasePtr& _feature_1_ptr,
+                               const FeatureBasePtr& _feature_2_ptr,
+                               const FeatureBasePtr& _feature_own_ptr,
+                               const ProcessorBasePtr& _processor_ptr,
+                               bool _apply_loss_function,
+                               FactorStatus _status) :
+        FactorAutodiff( "TRIFOCAL PLP",
+                        TOP_GEOM,
+                        _feature_own_ptr,
+                        FrameBasePtrList(),
+                        CaptureBasePtrList(),
+                        FeatureBasePtrList({_feature_2_ptr, _feature_1_ptr}), //< this sets feature 2 (the one between the oldest and the newest)
+                        LandmarkBasePtrList(),
+                        _processor_ptr,
+                        _apply_loss_function,
+                        _status,
+                        _feature_1_ptr->getFrame()->getP(),
+                        _feature_1_ptr->getFrame()->getO(),
+                        _feature_2_ptr->getFrame()->getP(),
+                        _feature_2_ptr->getFrame()->getO(),
+                        _feature_own_ptr->getFrame()->getP(),
+                        _feature_own_ptr->getFrame()->getO(),
+                        _feature_own_ptr->getCapture()->getSensorP(),
+                        _feature_own_ptr->getCapture()->getSensorO() ),
+        feature_prev_ptr_(_feature_1_ptr),
+        camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensor())),
+        sqrt_information_upper(Matrix3d::Zero())
+{
+    // First add feature_1_ptr to the list of features (because the constructor FeatureAutodiff did not do so)
+    //if (_feature_1_ptr) feature_other_list_.push_back(_feature_1_ptr);
+
+    // Store some geometry elements
+    Matrix3d K_inv          = camera_ptr_->getIntrinsicMatrix().inverse();
+    pixel_canonical_1_      = K_inv * Vector3d(_feature_1_ptr->getMeasurement(0), _feature_1_ptr->getMeasurement(1), 1.0);
+    pixel_canonical_2_      = K_inv * Vector3d(_feature_2_ptr->getMeasurement(0), _feature_2_ptr->getMeasurement(1), 1.0);
+    pixel_canonical_3_      = K_inv * Vector3d(_feature_own_ptr->getMeasurement(0), _feature_own_ptr->getMeasurement(1), 1.0);
+    Matrix<double,3,2> J_m_u = K_inv.block(0,0,3,2);
+
+    // extract relevant states
+    Vector3d    wtr1 =             _feature_1_ptr  ->getFrame()  ->getP()      ->getState();
+    Quaterniond wqr1 = Quaterniond(_feature_1_ptr  ->getFrame()  ->getO()      ->getState().data() );
+    Vector3d    wtr2 =             _feature_2_ptr  ->getFrame()  ->getP()      ->getState();
+    Quaterniond wqr2 = Quaterniond(_feature_2_ptr  ->getFrame()  ->getO()      ->getState().data() );
+    Vector3d    wtr3 =             _feature_own_ptr->getFrame()  ->getP()      ->getState();
+    Quaterniond wqr3 = Quaterniond(_feature_own_ptr->getFrame()  ->getO()      ->getState().data() );
+    Vector3d    rtc  =             _feature_own_ptr->getCapture()->getSensorP()->getState();
+    Quaterniond rqc  = Quaterniond(_feature_own_ptr->getCapture()->getSensorO()->getState().data() );
+
+    // expectation // canonical units
+    vision_utils::TrifocalTensorBase<double> tensor;
+    Matrix3d    c2Ec1;
+    expectation(wtr1, wqr1,
+                wtr2, wqr2,
+                wtr3, wqr3,
+                rtc, rqc,
+                tensor, c2Ec1);
+
+    // error Jacobians // canonical units
+    Matrix<double,3,3> J_e_m1, J_e_m2, J_e_m3;
+    error_jacobians(tensor, c2Ec1, J_e_m1, J_e_m2, J_e_m3);
+
+    // chain rule to go from homogeneous pixel to Euclidean pixel
+    Matrix<double,3,2> J_e_u1 = J_e_m1 * J_m_u;
+    Matrix<double,3,2> J_e_u2 = J_e_m2 * J_m_u;
+    Matrix<double,3,2> J_e_u3 = J_e_m3 * J_m_u;
+
+    // Error covariances induced by each of the measurement covariance // canonical units
+    Matrix3d Q1 = J_e_u1 * _feature_1_ptr   ->getMeasurementCovariance() * J_e_u1.transpose();
+    Matrix3d Q2 = J_e_u2 * _feature_2_ptr   ->getMeasurementCovariance() * J_e_u2.transpose();
+    Matrix3d Q3 = J_e_u3 * _feature_own_ptr ->getMeasurementCovariance() * J_e_u3.transpose();
+
+    // Total error covariance // canonical units
+    Matrix3d Q = Q1 + Q2 + Q3;
+
+    // Sqrt of information matrix // canonical units
+    Eigen::LLT<Eigen::MatrixXd> llt_of_info(Q.inverse()); // Cholesky decomposition
+    sqrt_information_upper = llt_of_info.matrixU();
+
+    // Re-write info matrix (for debug only)
+    //    double pix_noise = 1.0;
+    //    sqrt_information_upper = pow(1.0/pix_noise, 2) * Matrix3d::Identity(); // one PLP (2d) and one epipolar (1d) factor
+}
+
+// Destructor
+FactorTrifocal::~FactorTrifocal()
+{
+}
+
+inline FeatureBasePtr FactorTrifocal::getFeaturePrev()
+{
+    return feature_prev_ptr_.lock();
+}
+
+template<typename T>
+bool FactorTrifocal::operator ()( const T* const _pos1,
+                                          const T* const _quat1,
+                                          const T* const _pos2,
+                                          const T* const _quat2,
+                                          const T* const _pos3,
+                                          const T* const _quat3,
+                                          const T* const _sen_pos,
+                                          const T* const _sen_quat,
+                                          T* _residuals) const
+{
+
+    // MAPS
+    Map<const Matrix<T,3,1> > wtr1(_pos1);
+    Map<const Quaternion<T> > wqr1(_quat1);
+    Map<const Matrix<T,3,1> > wtr2(_pos2);
+    Map<const Quaternion<T> > wqr2(_quat2);
+    Map<const Matrix<T,3,1> > wtr3(_pos3);
+    Map<const Quaternion<T> > wqr3(_quat3);
+    Map<const Matrix<T,3,1> > rtc (_sen_pos);
+    Map<const Quaternion<T> > rqc (_sen_quat);
+    Map<Matrix<T,3,1> >       res (_residuals);
+
+    vision_utils::TrifocalTensorBase<T> tensor;
+    Matrix<T, 3, 3> c2Ec1;
+    expectation(wtr1, wqr1, wtr2, wqr2, wtr3, wqr3, rtc, rqc, tensor, c2Ec1);
+    res = residual(tensor, c2Ec1);
+    return true;
+}
+
+template<typename D1, typename D2, class T, typename D3>
+inline void FactorTrifocal::expectation(const MatrixBase<D1>&     _wtr1,
+                                                const QuaternionBase<D2>& _wqr1,
+                                                const MatrixBase<D1>&     _wtr2,
+                                                const QuaternionBase<D2>& _wqr2,
+                                                const MatrixBase<D1>&     _wtr3,
+                                                const QuaternionBase<D2>& _wqr3,
+                                                const MatrixBase<D1>&     _rtc,
+                                                const QuaternionBase<D2>& _rqc,
+                                                vision_utils::TrifocalTensorBase<T>& _tensor,
+                                                MatrixBase<D3>&     _c2Ec1) const
+{
+
+        typedef Translation<T, 3> TranslationType;
+        typedef Eigen::Transform<T, 3, Eigen::Isometry> TransformType;
+
+        // All input Transforms
+        TransformType wHr1 = TranslationType(_wtr1) * _wqr1;
+        TransformType wHr2 = TranslationType(_wtr2) * _wqr2;
+        TransformType wHr3 = TranslationType(_wtr3) * _wqr3;
+        TransformType rHc  = TranslationType(_rtc)  * _rqc ;
+
+        // Relative camera transforms
+        TransformType c1Hc2 = rHc.inverse() * wHr1.inverse() * wHr2 * rHc;
+        TransformType c1Hc3 = rHc.inverse() * wHr1.inverse() * wHr3 * rHc;
+
+        // Projection matrices
+        Matrix<T,3,4> c2Pc1 = c1Hc2.inverse().affine();
+        Matrix<T,3,4> c3Pc1 = c1Hc3.inverse().affine();
+
+        // Trifocal tensor
+        _tensor.computeTensorFromProjectionMat(c2Pc1, c3Pc1);
+
+        /* Essential matrix convention disambiguation
+         *
+         * C1 is the origin frame or reference
+         * C2 is another cam
+         * C2 is specified by R and T wrt C1 so that
+         *   T is the position    of C2 wrt C1
+         *   R is the orientation of C2 wrt C1
+         * There is a 3d point P, noted P1 when expressed in C1 and P2 when expressed in C2:
+         *   P1 = T + R * P2
+         *
+         * Coplanarity condition: a' * (b x c) = 0 with {a,b,c} three coplanar vectors.
+         *
+         * The three vectors are:
+         *
+         *   baseline: b  = T
+         *   ray 1   : r1 = P1
+         *   ray 2   : r2 = P1 - T = R*P2;
+         *
+         * so,
+         *
+         *   (r1)' * (b x r2) = 0 , which develops as:
+         *
+         *   P1' * (T x (R*P2))  = 0
+         *   P1' * [T]x * R * P2 = 0
+         *   P1' * c1Ec2 * P2    = 0 <--- Epipolar factor
+         *
+         * therefore:
+         *   c1Ec2 = [T]x * R        <--- Essential matrix
+         *
+         * or, if we prefer the factor P2' * c2Ec1 * P1 = 0,
+         *   c2Ec1 = c1Ec2' = R' * [T]x (we obviate the sign change)
+         */
+        Matrix<T,3,3> Rtr = c1Hc2.matrix().block(0,0,3,3).transpose();
+        Matrix<T,3,1> t   = c1Hc2.matrix().block(0,3,3,1);
+        _c2Ec1 = Rtr * skew(t);
+//        _c2Ec1 =  c1Hc2.rotation().transpose() * skew(c1Hc2.translation()) ;
+}
+
+template<typename T, typename D1>
+inline Matrix<T, 3, 1> FactorTrifocal::residual(const vision_utils::TrifocalTensorBase<T>& _tensor,
+                                                        const MatrixBase<D1>& _c2Ec1) const
+{
+    // 1. COMMON COMPUTATIONS
+
+    // m1, m2, m3: canonical pixels in cams 1,2,3 -- canonical means m = K.inv * u, with _u_ a homogeneous pixel [ux; uy; 1].
+    Matrix<T,3,1> m1(pixel_canonical_1_.cast<T>());
+    Matrix<T,3,1> m2(pixel_canonical_2_.cast<T>());
+    Matrix<T,3,1> m3(pixel_canonical_3_.cast<T>());
+
+    // 2. TRILINEARITY PLP
+
+    Matrix<T,2,1> e1;
+    vision_utils::errorTrifocalPLP(m1, m2, m3, _tensor, _c2Ec1, e1);
+
+    // 3. EPIPOLAR
+    T e2;
+    vision_utils::errorEpipolar(m1, m2, _c2Ec1, e2);
+
+    // 4. RESIDUAL
+
+    Matrix<T,3,1> errors, residual;
+    errors  << e1, e2;
+    residual = sqrt_information_upper.cast<T>() * errors;
+
+    return residual;
+}
+
+// Helper functions to be used by the above
+template<class T, typename D1, typename D2, typename D3, typename D4>
+inline Matrix<T, 3, 1> FactorTrifocal::error_jacobians(const vision_utils::TrifocalTensorBase<T>& _tensor,
+                                                               const MatrixBase<D1>& _c2Ec1,
+                                                               MatrixBase<D2>& _J_e_m1,
+                                                               MatrixBase<D3>& _J_e_m2,
+                                                               MatrixBase<D4>& _J_e_m3)
+{
+    // 1. COMMON COMPUTATIONS
+
+    // m1, m2, m3: canonical pixels in cams 1,2,3 -- canonical means m = K.inv * u, with _u_ a homogeneous pixel [ux; uy; 1].
+    Matrix<T,3,1> m1(pixel_canonical_1_.cast<T>());
+    Matrix<T,3,1> m2(pixel_canonical_2_.cast<T>());
+    Matrix<T,3,1> m3(pixel_canonical_3_.cast<T>());
+
+    // 2. TRILINEARITY PLP
+
+    Matrix<T,2,3> J_e1_m1, J_e1_m2, J_e1_m3;
+    Matrix<T,2,1> e1;
+
+    vision_utils::errorTrifocalPLP(m1, m2, m3, _tensor, _c2Ec1, e1, J_e1_m1, J_e1_m2, J_e1_m3);
+
+    // 3. EPIPOLAR
+
+    T e2;
+    Matrix<T,1,3> J_e2_m1, J_e2_m2, J_e2_m3;
+
+    vision_utils::errorEpipolar(m1, m2, _c2Ec1, e2, J_e2_m1, J_e2_m2);
+
+    J_e2_m3.setZero(); // Not involved in epipolar c1->c2
+
+    // Compact Jacobians
+    _J_e_m1.topRows(2) = J_e1_m1;
+    _J_e_m1.row(2)     = J_e2_m1;
+    _J_e_m2.topRows(2) = J_e1_m2;
+    _J_e_m2.row(2)     = J_e2_m2;
+    _J_e_m3.topRows(2) = J_e1_m3;
+    _J_e_m3.row(2)     = J_e2_m3;
+
+    // 4. ERRORS
+
+    Matrix<T,3,1> errors;
+    errors  << e1, e2;
+
+    return errors;
+
+}
+
+// Print function
+template<class T, int ROWS, int COLS>
+void FactorTrifocal::print_matrix(const Eigen::Matrix<T, ROWS, COLS>& _mat) const
+{}
+
+template<int ROWS, int COLS>
+void FactorTrifocal::print_matrix(const Eigen::Matrix<double, ROWS, COLS>& _mat) const
+{
+    std::cout << _mat << std::endl;
+}
+
+template<class T>
+void FactorTrifocal::print_scalar(const T& _val) const
+{}
+
+void FactorTrifocal::print_scalar(const double& _val) const
+{
+    std::cout << _val << std::endl;
+}
+
+}    // namespace wolf
+
+#endif /* _FACTOR_AUTODIFF_TRIFOCAL_H_ */

include/vision/landmark/landmark_ahp.h

@@ -52,8 +52,10 @@ class LandmarkAhp : public LandmarkBase
         const cv::Mat& getCvDescriptor() const;
         void setCvDescriptor(const cv::Mat& _descriptor);
 
-        const FrameBasePtr  getAnchorFrame () const;
-        const SensorBasePtr getAnchorSensor() const;
+        FrameBaseConstPtr  getAnchorFrame () const;
+        FrameBasePtr  getAnchorFrame ();
+        SensorBaseConstPtr getAnchorSensor() const;
+        SensorBasePtr getAnchorSensor();
 
         void setAnchorFrame  (FrameBasePtr  _anchor_frame );
         void setAnchorSensor (SensorBasePtr _anchor_sensor);
@@ -80,7 +82,12 @@ inline void LandmarkAhp::setCvDescriptor(const cv::Mat& _descriptor)
     cv_descriptor_ = _descriptor;
 }
 
-inline const FrameBasePtr LandmarkAhp::getAnchorFrame() const
+inline FrameBaseConstPtr LandmarkAhp::getAnchorFrame() const
+{
+    return anchor_frame_;
+}
+
+inline FrameBasePtr LandmarkAhp::getAnchorFrame() 
 {
     return anchor_frame_;
 }
@@ -90,7 +97,12 @@ inline void LandmarkAhp::setAnchorFrame(FrameBasePtr _anchor_frame)
     anchor_frame_ = _anchor_frame;
 }
 
-inline const SensorBasePtr LandmarkAhp::getAnchorSensor() const
+inline SensorBaseConstPtr LandmarkAhp::getAnchorSensor() const
+{
+    return anchor_sensor_;
+}
+
+inline SensorBasePtr LandmarkAhp::getAnchorSensor()
 {