.gitignore

@@ -4,6 +4,7 @@
 README.txt
 bin/
 build/
+build_release/
 lib/
 .idea/
 ./Wolf.user

src/IMU_tools.h

@@ -34,10 +34,10 @@
  *   - between: Db = D2 (-) D1, so that D2 = D1 (+) Db
  *   - composeOverState: x2 = x1 (+) D
  *   - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D
- *   - lift: got from Delta manifold to tangent space (equivalent to log() in rotations)
- *   - retract: go from tangent space to delta manifold (equivalent to exp() in rotations)
- *   - plus: D2 = D1 (+) retract(d)
- *   - diff: d = lift( D2 (-) D1 )
+ *   - log: got from Delta manifold to tangent space (equivalent to log() in rotations)
+ *   - exp_IMU: go from tangent space to delta manifold (equivalent to exp() in rotations)
+ *   - plus: D2 = D1 (+) exp_IMU(d)
+ *   - diff: d = log_IMU( D2 (-) D1 )
  *   - body2delta: construct a delta from body magnitudes of linAcc and angVel
  */
 
@@ -345,7 +345,7 @@ inline Matrix<typename D1::Scalar, 10, 1> betweenStates(const MatrixBase<D1>& x1
 }
 
 template<typename Derived>
-Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in)
+Matrix<typename Derived::Scalar, 9, 1> log_IMU(const MatrixBase<Derived>& delta_in)
 {
     MatrixSizeCheck<10, 1>::check(delta_in);
 
@@ -366,7 +366,7 @@ Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in)
 }
 
 template<typename Derived>
-Matrix<typename Derived::Scalar, 10, 1> retract(const MatrixBase<Derived>& d_in)
+Matrix<typename Derived::Scalar, 10, 1> exp_IMU(const MatrixBase<Derived>& d_in)
 {
     MatrixSizeCheck<9, 1>::check(d_in);
 

src/three_D_tools.h → src/SE3.h

 /*
- * three_D_tools.h
+ * SE3.h
  *
  *  Created on: Mar 15, 2018
  *      Author: jsola
  */
 
-#ifndef THREE_D_TOOLS_H_
-#define THREE_D_TOOLS_H_
+#ifndef SE3_H_
+#define SE3_H_
 
 
 #include "wolf.h"
@@ -27,10 +27,11 @@
  *   - identity:    I  = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], so that D (+) I = I (+) D = D
  *   - inverse:     so that D (+) D.inv = D.inv (+) D = I
  *   - between:     Db = D2 (-) D1 := D1.inv (+) D2, so that D2 = D1 (+) Db
- *   - lift:        go from Delta manifold to tangent space (equivalent to log() in rotations)
- *   - retract:     go from tangent space to delta manifold (equivalent to exp() in rotations)
- *   - plus:        D2 = D1 (+) retract(d)
- *   - diff:        d  = lift( D2 (-) D1 )
+ *   - log_SE3:     go from Delta manifold to tangent space (equivalent to log() in rotations)
+ *   - exp_SE3:     go from tangent space to delta manifold (equivalent to exp() in rotations)
+ *   - plus:        D2 = D1 * exp_SE3(d)
+ *   - minus:       d  = log_SE3( D1.inv() * D2 )
+ *   - interpolate: dd = D1 * exp ( log( D1.inv() * D2 ) * t ) = D1 (+) ( (D2 (-) D1) * t)
  */
 
 
@@ -220,7 +221,7 @@ inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
 }
 
 template<typename Derived>
-Matrix<typename Derived::Scalar, 6, 1> lift(const MatrixBase<Derived>& delta_in)
+Matrix<typename Derived::Scalar, 6, 1> log_SE3(const MatrixBase<Derived>& delta_in)
 {
     MatrixSizeCheck<7, 1>::check(delta_in);
 
@@ -231,25 +232,32 @@ Matrix<typename Derived::Scalar, 6, 1> lift(const MatrixBase<Derived>& delta_in)
     Map<Matrix<typename Derived::Scalar, 3, 1> >         dp_ret ( & ret(0) );
     Map<Matrix<typename Derived::Scalar, 3, 1> >         do_ret ( & ret(3) );
 
-    dp_ret = dp_in;
-    do_ret = log_q(dq_in);
+    Matrix<typename Derived::Scalar, 3, 3> V_inv;
+
+    do_ret  = log_q(dq_in);
+    V_inv   = jac_SO3_left_inv(do_ret);
+    dp_ret  = V_inv * dp_in;
 
     return ret;
 }
 
 template<typename Derived>
-Matrix<typename Derived::Scalar, 7, 1> retract(const MatrixBase<Derived>& d_in)
+Matrix<typename Derived::Scalar, 7, 1> exp_SE3(const MatrixBase<Derived>& d_in)
 {
     MatrixSizeCheck<6, 1>::check(d_in);
 
     Matrix<typename Derived::Scalar, 7, 1> ret;
 
+    Matrix<typename Derived::Scalar, 3, 3> V;
+
+    V = jac_SO3_left(d_in.template tail<3>());
+
     Map<const Matrix<typename Derived::Scalar, 3, 1> >   dp_in  ( & d_in(0) );
     Map<const Matrix<typename Derived::Scalar, 3, 1> >   do_in  ( & d_in(3) );
     Map<Matrix<typename Derived::Scalar, 3, 1> >         dp     ( &  ret(0) );
     Map<Quaternion<typename Derived::Scalar> >           dq     ( &  ret(3) );
 
-    dp = dp_in;
+    dp = V * dp_in;
     dq = exp_q(do_in);
 
     return ret;
@@ -292,21 +300,21 @@ inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
 }
 
 template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
-                 const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
-                 MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o )
+inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+                  const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
+                  MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o )
 {
-        diff_p = dp2 - dp1;
-        diff_o = log_q(dq1.conjugate() * dq2);
+    diff_p = dp2 - dp1;
+    diff_o = log_q(dq1.conjugate() * dq2);
 }
 
 template<typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
-inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
-                 const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
-                 MatrixBase<D6>& diff_p, MatrixBase<D7>& diff_o,
-                 MatrixBase<D8>& J_do_dq1, MatrixBase<D9>& J_do_dq2)
+inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+                  const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
+                  MatrixBase<D6>& diff_p, MatrixBase<D7>& diff_o,
+                  MatrixBase<D8>& J_do_dq1, MatrixBase<D9>& J_do_dq2)
 {
-    diff(dp1, dq1, dp2, dq2, diff_p, diff_o);
+    minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
 
     J_do_dq1    = - jac_SO3_left_inv(diff_o);
     J_do_dq2    =   jac_SO3_right_inv(diff_o);
@@ -314,9 +322,9 @@ inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
 
 
 template<typename D1, typename D2, typename D3>
-inline void diff(const MatrixBase<D1>& d1,
-                 const MatrixBase<D2>& d2,
-                 MatrixBase<D3>& err)
+inline void minus(const MatrixBase<D1>& d1,
+                  const MatrixBase<D2>& d2,
+                  MatrixBase<D3>& err)
 {
     Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( & d1(0) );
     Map<const Quaternion<typename D1::Scalar> >     dq1    ( & d1(3) );
@@ -325,15 +333,15 @@ inline void diff(const MatrixBase<D1>& d1,
     Map<Matrix<typename D3::Scalar, 3, 1> >         diff_p ( & err(0) );
     Map<Matrix<typename D3::Scalar, 3, 1> >         diff_o ( & err(3) );
 
-    diff(dp1, dq1, dp2, dq2, diff_p, diff_o);
+    minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
 }
 
 template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void diff(const MatrixBase<D1>& d1,
-                 const MatrixBase<D2>& d2,
-                 MatrixBase<D3>& dif,
-                 MatrixBase<D4>& J_diff_d1,
-                 MatrixBase<D5>& J_diff_d2)
+inline void minus(const MatrixBase<D1>& d1,
+                  const MatrixBase<D2>& d2,
+                  MatrixBase<D3>& dif,
+                  MatrixBase<D4>& J_diff_d1,
+                  MatrixBase<D5>& J_diff_d2)
 {
     Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( & d1(0) );
     Map<const Quaternion<typename D1::Scalar> >     dq1    ( & d1(3) );
@@ -347,9 +355,9 @@ inline void diff(const MatrixBase<D1>& d1,
 
     Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
 
-    diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
+    minus(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
 
-    /* d = diff(d1, d2) is
+    /* d = minus(d1, d2) is
      *   dp = dp2 - dp1
      *   do = Log(dq1.conj * dq2)
      *   dv = dv2 - dv1
@@ -367,17 +375,111 @@ inline void diff(const MatrixBase<D1>& d1,
 }
 
 template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 6, 1> diff(const MatrixBase<D1>& d1,
-                                              const MatrixBase<D2>& d2)
+inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1,
+                                               const MatrixBase<D2>& d2)
 {
     Matrix<typename D1::Scalar, 6, 1> ret;
-    diff(d1, d2, ret);
+    minus(d1, d2, ret);
     return ret;
 }
 
+template<typename D1, typename D2, typename D3>
+inline void interpolate(const MatrixBase<D1>& d1,
+                        const MatrixBase<D2>& d2,
+                        const typename D1::Scalar& t,
+                        MatrixBase<D3>& ret)
+{
+    Matrix<typename D1::Scalar, 7, 1> dd = between(d1, d2);
+
+    Matrix<typename D1::Scalar, 6, 1> tau = t * log_SE3(dd);
+
+    ret = compose(d1, exp_SE3(tau));
+}
+
+template<typename D1, typename D2>
+inline void toSE3(const MatrixBase<D1>& pose,
+                        MatrixBase<D2>& SE3)
+{
+    MatrixSizeCheck<4,4>::check(SE3);
+
+    typedef typename D1::Scalar T;
+
+    SE3.template block<3,1>(0,3) = pose.template head<3>();
+    SE3.template block<3,3>(0,0) = q2R(pose.template tail<4>());
+    SE3.template block<1,3>(3,0).setZero();
+    SE3(3,3) = (T)1.0;
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 6, 6> Q_helper(const MatrixBase<D1>& v, const MatrixBase<D1>& w)
+{
+    typedef typename D1::Scalar T;
+
+    Matrix<T, 3, 3> V   = skew(v);
+    Matrix<T, 3, 3> W   = skew(w);
+    Matrix<T, 3, 3> VW  = V * W;
+    Matrix<T, 3, 3> WV  = VW.transpose();       // Note on this change wrt. Barfoot: it happens that V*W = (W*V).transpose() !!!
+    Matrix<T, 3, 3> WVW = WV * W;
+    Matrix<T, 3, 3> VWW = VW * W;
+
+    T th_2      = w.squaredNorm();
+
+    T A(T(0.5)), B, C, D;
+
+    // Small angle approximation
+    if (th_2 <= T(1e-8))
+    {
+        B =  Scalar(1./6.)  + Scalar(1./120.)  * th_2;
+        C = -Scalar(1./24.) + Scalar(1./720.)  * th_2;
+        D = -Scalar(1./60.);
+    }
+    else
+    {
+        T th        = sqrt(th_2);
+        T th_3      = th_2*th;
+        T th_4      = th_2*th_2;
+        T th_5      = th_3*th_2;
+        T sin_th    = sin(th);
+        T cos_th    = cos(th);
+        B           = (th-sin_th)/th_3;
+        C           = (T(1.0) - th_2/T(2.0) - cos_th)/th_4;
+        D           = (th - sin_th - th_3/T(6.0))/th_5;
+    }
+    Matrix<T, 3, 3> Q;
+    Q.noalias() =
+            + A * V
+            + B * (WV + VW + WVW)
+            - C * (VWW - VWW.transpose() - Scalar(3) * WVW)           // Note on this change wrt. Barfoot: it happens that V*W*W = -(W*W*V).transpose() !!!
+            - D * WVW * W;                                            // Note on this change wrt. Barfoot: it happens that W*V*W*W = W*W*V*W !!!
+
+    return Q;
+}
+
+template<typename D1>
+inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_left(const MatrixBase<D1>& tangent)
+{
+    typedef typename D1::Scalar T;
+    Map<Matrix<T, 3, 1>> v(tangent.data() + 0); // linear
+    Map<Matrix<T, 3, 1>> w(tangent.data() + 3); // angular
+
+    Matrix<T, 3, 3> Jl(jac_SO3_left(w));
+    Matrix<T, 3, 3> Q = Q_helper(v,w);
+
+    Matrix<T, 6, 6> Jl_SE3;
+    Jl_SE3.topLeftCorner(3,3)     = Jl;
+    Jl_SE3.bottomRightCorner(3,3) = Jl;
+    Jl_SE3.topRightCorner(3,3)    = Q;
+    Jl_SE3.bottomLeftCorner(3,3)  .setZero();
+}
+
+template<typename D1>
+inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_right(const MatrixBase<D1>& tangent)
+{
+    return jac_SE3_left(-tangent);
+}
 
 } // namespace three_d
 } // namespace wolf
 
 
-#endif /* THREE_D_TOOLS_H_ */
+#endif /* SE3_H_ */

src/capture_base.cpp

@@ -18,7 +18,6 @@ CaptureBase::CaptureBase(const std::string& _type,
     sensor_ptr_(_sensor_ptr),
     state_block_vec_(3),
     calib_size_(0),
-    is_removing_(false),
     capture_id_(++capture_id_count_),
     time_stamp_(_ts)
 {

src/capture_base.h

@@ -29,7 +29,6 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
         SizeEigen calib_size_;           ///< size of the calibration parameters (dynamic or static sensor params that are not fixed)
 
         static unsigned int capture_id_count_;
-        bool is_removing_;          ///< A flag for safely removing nodes from the Wolf tree. See remove().
 
     protected:
         unsigned int capture_id_;
@@ -45,11 +44,13 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
                     StateBlockPtr _intr_ptr     = nullptr);
 
         virtual ~CaptureBase();
-        void remove();
+        virtual void remove();
 
         // Type
         virtual bool isMotion() const { return false; }
 
+        bool process();
+
         unsigned int id();
         TimeStamp getTimeStamp() const;
         void setTimeStamp(const TimeStamp& _ts);
@@ -220,6 +221,14 @@ inline void CaptureBase::setTimeStampToNow()
     time_stamp_.setToNow();
 }
 
+inline bool CaptureBase::process()
+{
+    assert (getSensorPtr() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead.");
+
+    return getSensorPtr()->process(shared_from_this());
+}
+
+
 } // namespace wolf
 
 #endif

src/ceres_wrapper/ceres_manager.h

@@ -53,13 +53,13 @@ class CeresManager : public SolverManager
 
         virtual void computeCovariances(const StateBlockList& st_list) override;
 
-        virtual bool hasConverged();
+        virtual bool hasConverged() override;
 
-        virtual SizeStd iterations();
+        virtual SizeStd iterations() override;
 
-        virtual Scalar initialCost();
+        virtual Scalar initialCost() override;
 
-        virtual Scalar finalCost();
+        virtual Scalar finalCost() override;
 
         ceres::Solver::Options& getSolverOptions();
 

src/constraint_analytic.cpp

@@ -7,9 +7,19 @@ ConstraintAnalytic::ConstraintAnalytic(const std::string&  _tp,
                                        bool _apply_loss_function, ConstraintStatus _status,
                                        StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
                                        StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
-            ConstraintBase(_tp, _apply_loss_function, _status),
-            state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
-                               _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
+        ConstraintBase(_tp, _apply_loss_function, _status),
+        state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
+                           _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
+        state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
+                                   _state1Ptr ? (unsigned int) _state1Ptr->getSize() : 0,
+                                   _state2Ptr ? (unsigned int) _state2Ptr->getSize() : 0,
+                                   _state3Ptr ? (unsigned int) _state3Ptr->getSize() : 0,
+                                   _state4Ptr ? (unsigned int) _state4Ptr->getSize() : 0,
+                                   _state5Ptr ? (unsigned int) _state5Ptr->getSize() : 0,
+                                   _state6Ptr ? (unsigned int) _state6Ptr->getSize() : 0,
+                                   _state7Ptr ? (unsigned int) _state7Ptr->getSize() : 0,
+                                   _state8Ptr ? (unsigned int) _state8Ptr->getSize() : 0,
+                                   _state9Ptr ? (unsigned int) _state9Ptr->getSize() : 0})
 {
     resizeVectors();
 }
@@ -23,9 +33,19 @@ ConstraintAnalytic::ConstraintAnalytic(const std::string&  _tp,
                                        bool _apply_loss_function, ConstraintStatus _status,
                                        StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
                                        StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
-            ConstraintBase(_tp,  _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
-            state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
-                               _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
+        ConstraintBase(_tp,  _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+        state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
+                           _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
+        state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
+                                   _state1Ptr ? (unsigned int) _state1Ptr->getSize() : 0,
+                                   _state2Ptr ? (unsigned int) _state2Ptr->getSize() : 0,
+                                   _state3Ptr ? (unsigned int) _state3Ptr->getSize() : 0,
+                                   _state4Ptr ? (unsigned int) _state4Ptr->getSize() : 0,
+                                   _state5Ptr ? (unsigned int) _state5Ptr->getSize() : 0,
+                                   _state6Ptr ? (unsigned int) _state6Ptr->getSize() : 0,
+                                   _state7Ptr ? (unsigned int) _state7Ptr->getSize() : 0,
+                                   _state8Ptr ? (unsigned int) _state8Ptr->getSize() : 0,
+                                   _state9Ptr ? (unsigned int) _state9Ptr->getSize() : 0})
 {
     resizeVectors();
 }
@@ -58,17 +78,15 @@ JacobianMethod ConstraintAnalytic::getJacobianMethod() const
 
 void ConstraintAnalytic::resizeVectors()
 {
-    for (unsigned int ii = 1; ii<state_ptr_vector_.size(); ii++)
-    {
-        if (state_ptr_vector_.at(ii) != nullptr)
-            state_block_sizes_vector_.push_back(state_ptr_vector_.at(ii)->getSize());
+    assert(state_ptr_vector_[0] != nullptr && "at least one not null state block pointer required");
 
-        else
+    for (unsigned int ii = 1; ii<state_ptr_vector_.size(); ii++)
+        if (state_ptr_vector_.at(ii) == nullptr)
         {
             state_ptr_vector_.resize(ii);
+            state_block_sizes_vector_.resize(ii);
             break;
         }
-    }
 }
 
 } // namespace wolf

src/constraint_analytic.h

@@ -83,7 +83,6 @@ class ConstraintAnalytic: public ConstraintBase
 
         /** \brief Evaluate the constraint given the input parameters and returning the residuals and jacobians
         **/
-        // TODO
         virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override
         {
             // load parameters evaluation value
@@ -118,7 +117,7 @@ class ConstraintAnalytic: public ConstraintBase
             return true;
         };
 
-        /** Returns the residual vetor and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
+        /** Returns the residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
          **/
         // TODO
         virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const override
@@ -132,9 +131,9 @@ class ConstraintAnalytic: public ConstraintBase
          **/
         virtual Eigen::VectorXs evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector) const = 0;
 
-        /** \brief Returns the jacobians evaluated in the states provided
+        /** \brief Returns the normalized jacobians evaluated in the states
          *
-         * Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
+         * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
          * IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
          *
          * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the constraint
@@ -144,11 +143,10 @@ class ConstraintAnalytic: public ConstraintBase
          **/
         virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector, std::vector<Eigen::Map<Eigen::MatrixXs>>& jacobians, const std::vector<bool>& _compute_jacobian) const = 0;
 
-        /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
+        /** \brief Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values
          *
-         * Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
+         * Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values
          *
-         * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the constraint
          * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
          *
          **/

src/constraint_base.cpp

@@ -11,7 +11,6 @@ ConstraintBase::ConstraintBase(const std::string&  _tp,
                                ConstraintStatus _status) :
     NodeBase("CONSTRAINT", _tp),
     feature_ptr_(), // nullptr
-    is_removing_(false),
     constraint_id_(++constraint_id_count_),
     status_(_status),
     apply_loss_function_(_apply_loss_function),
@@ -32,7 +31,6 @@ ConstraintBase::ConstraintBase(const std::string&  _tp,
                                bool _apply_loss_function, ConstraintStatus _status) :
     NodeBase("CONSTRAINT", _tp),
     feature_ptr_(),
-    is_removing_(false),
     constraint_id_(++constraint_id_count_),
     status_(_status),
     apply_loss_function_(_apply_loss_function),

src/constraint_base.h

@@ -42,7 +42,6 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
         FeatureBaseWPtr feature_ptr_;                    ///< FeatureBase pointer (upper node)
 
         static unsigned int constraint_id_count_;
-        bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
 
     protected:
         unsigned int constraint_id_;
@@ -75,7 +74,7 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
 
         virtual ~ConstraintBase() = default;
 
-        void remove();
+        virtual void remove();
 
         unsigned int id() const;
 

src/constraint_block_absolute.h

@@ -9,7 +9,7 @@
 #define CONSTRAINT_BLOCK_ABSOLUTE_H_
 
 //Wolf includes
-#include "constraint_autodiff.h"
+#include "constraint_analytic.h"
 #include "frame_base.h"
 
 
@@ -18,43 +18,122 @@ namespace wolf {
 WOLF_PTR_TYPEDEFS(ConstraintBlockAbsolute);
 
 //class
-class ConstraintBlockAbsolute: public ConstraintAutodiff<ConstraintBlockAbsolute,3,3>
+class ConstraintBlockAbsolute : public ConstraintAnalytic
 {
+    private:
+        SizeEigen sb_size_;              // the size of the state block
+        SizeEigen sb_constrained_start_; // the index of the first state element that is constrained
+        SizeEigen sb_constrained_size_;  // the size of the state segment that is constrained
+        Eigen::MatrixXs J_;              // Jacobian
+
     public:
 
-        ConstraintBlockAbsolute(StateBlockPtr _sb_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-            ConstraintAutodiff<ConstraintBlockAbsolute,3,3>("BLOCK ABS",
-                                                            nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _sb_ptr)
+        /** \brief Constructor
+         *
+         * \param _sb_ptr the constrained state block
+         * \param _start_idx (default=0) the index of the first state element that is constrained
+         * \param _start_idx (default=-1) the size of the state segment that is constrained, -1 = all the
+         *
+         */
+        ConstraintBlockAbsolute(StateBlockPtr _sb_ptr,
+                                unsigned int _start_idx = 0,
+                                int _size = -1,
+                                bool _apply_loss_function = false,
+                                ConstraintStatus _status = CTR_ACTIVE) :
+            ConstraintAnalytic("BLOCK ABS",
+                               _apply_loss_function,
+                               _status,
+                               _sb_ptr),
+            sb_size_(_sb_ptr->getSize()),
+            sb_constrained_start_(_start_idx),
+            sb_constrained_size_(_size == -1 ? sb_size_ : _size)
         {
-            //
+            assert(sb_constrained_size_+sb_constrained_start_ <= sb_size_);
+
+            // precompute Jacobian (Identity)
+            if (sb_constrained_start_ == 0)
+                J_ = Eigen::MatrixXs::Identity(sb_constrained_size_, sb_size_);
+            else
+            {
+                J_ = Eigen::MatrixXs::Zero(sb_constrained_size_,sb_size_);
+                J_.middleCols(sb_constrained_start_,sb_constrained_size_) = Eigen::MatrixXs::Identity(sb_constrained_size_, sb_constrained_size_);
+            }
         }
 
         virtual ~ConstraintBlockAbsolute() = default;
 
-        template<typename T>
-        bool operator ()(const T* const _sb, T* _residuals) const;
-
+        /** \brief Returns the residual evaluated in the states provided
+         *
+         * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXs
+         *
+         **/
+        virtual Eigen::VectorXs evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const;
+
+        /** \brief Returns the normalized jacobians evaluated in the states
+         *
+         * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
+         * IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
+         *
+         * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the constraint
+         * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+         * \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
+         *
+         **/
+        virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
+                                       std::vector<Eigen::Map<Eigen::MatrixXs> >& jacobians,
+                                       const std::vector<bool>& _compute_jacobian) const;
+
+        /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
+         *
+         * Returns the pure jacobians (without measurement noise) evaluated in the current state blocks values
+         *
+         * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+         *
+         **/
+        virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const;
+
+        /** \brief Returns the constraint residual size
+         **/
+        virtual unsigned int getSize() const;
 };
 
-template<typename T>
-inline bool ConstraintBlockAbsolute::operator ()(const T* const _sb, T* _residuals) const
+inline Eigen::VectorXs ConstraintBlockAbsolute::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const
 {
+    assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
+    assert(_st_vector.front().size() >= getMeasurement().size() && "Wrong StateBlock size");
 
-    // states
-    Eigen::Matrix<T, 3, 1>  sb(_sb);
+    // residual
+    if (sb_constrained_size_ == _st_vector.front().size())
+        return getMeasurementSquareRootInformationUpper() * (_st_vector.front() - getMeasurement());
+    else
+        return getMeasurementSquareRootInformationUpper() * (_st_vector.front().segment(sb_constrained_start_,sb_constrained_size_) - getMeasurement());
+}
 
-    // measurements
-    Eigen::Vector3s     measured_state(getMeasurement().data() + 0);
+inline void ConstraintBlockAbsolute::evaluateJacobians(
+        const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
+        std::vector<Eigen::Map<Eigen::MatrixXs> >& jacobians, const std::vector<bool>& _compute_jacobian) const
+{
+    assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
+    assert(jacobians.size() == 1 && "Wrong number of jacobians!");
+    assert(_compute_jacobian.size() == 1 && "Wrong number of _compute_jacobian booleans!");
+    assert(_st_vector.front().size() == sb_size_ && "Wrong StateBlock size");
+    assert(_st_vector.front().size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+
+    // normalized jacobian
+    if (_compute_jacobian.front())
+        jacobians.front() = getMeasurementSquareRootInformationUpper() * J_;
+}
 
-    // error
-    Eigen::Matrix<T, 3, 1> er;
-    er       = measured_state.cast<T>() - sb;
+inline void ConstraintBlockAbsolute::evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const
+{
+    assert(jacobians.size() == 1 && "Wrong number of jacobians!");
 
-    // residual
-    Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
-    res               = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+    jacobians.front() = J_;
+}
 
-    return true;
+inline unsigned int ConstraintBlockAbsolute::getSize() const
+{
+    return sb_constrained_size_;
 }
 
 } // namespace wolf

src/constraint_relative_2D_analytic.h

@@ -85,7 +85,6 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
                                        const std::vector<bool>& _compute_jacobian) const override;
 
         /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
-         * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the constraint
          * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
          *
          **/
@@ -125,11 +124,11 @@ inline void ConstraintRelative2DAnalytic::evaluateJacobians(
             << -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0))
                     + (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)), -(_st_vector[2](0)
             - _st_vector[0](0)) * cos(_st_vector[1](0)) - (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)), -1;
-    jacobians[1] = getMeasurementSquareRootInformationUpper() * jacobians[0];
+    jacobians[1] = getMeasurementSquareRootInformationUpper() * jacobians[1];
     jacobians[2] << cos(_st_vector[1](0)), sin(_st_vector[1](0)), -sin(_st_vector[1](0)), cos(_st_vector[1](0)), 0, 0;
-    jacobians[2] = getMeasurementSquareRootInformationUpper() * jacobians[0];
+    jacobians[2] = getMeasurementSquareRootInformationUpper() * jacobians[2];
     jacobians[3] << 0, 0, 1;
-    jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[0];
+    jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[3];
 }
 
 inline void ConstraintRelative2DAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const

src/examples/test_wolf_factories.cpp

@@ -65,7 +65,7 @@ int main(void)
     IntrinsicsBasePtr intr_cam_ptr = IntrinsicsFactory::get().create("CAMERA", wolf_config + "/camera_params_ueye_sim.yaml");
     ProcessorParamsBasePtr params_ptr = ProcessorParamsFactory::get().create("IMAGE FEATURE", wolf_config + "/processor_image_feature.yaml");
 
-    cout << "CAMERA with intrinsics      : " << (static_pointer_cast<IntrinsicsCamera>(intr_cam_ptr))->pinhole_model.transpose() << endl;
+    cout << "CAMERA with intrinsics      : " << (static_pointer_cast<IntrinsicsCamera>(intr_cam_ptr))->pinhole_model_raw.transpose() << endl;
 //    cout << "Processor IMAGE image width : " << (static_pointer_cast<ProcessorParamsImage>(params_ptr))->image.width << endl;
 
     cout << "\n==================== Install Sensors ====================" << endl;

src/feature_base.cpp

@@ -9,7 +9,6 @@ unsigned int FeatureBase::feature_id_count_ = 0;
 FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
 	NodeBase("FEATURE", _type),
     capture_ptr_(),
-    is_removing_(false),
     feature_id_(++feature_id_count_),
     track_id_(0),
     landmark_id_(0),

src/feature_base.h

@@ -25,7 +25,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
         ConstraintBaseList constrained_by_list_;
 
         static unsigned int feature_id_count_;
-        bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
 
     protected:
         unsigned int feature_id_;
@@ -46,7 +45,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
         FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
 
         virtual ~FeatureBase();
-        void remove();
+        virtual void remove();
 
         virtual void setProblem(ProblemPtr _problem) final;
 

src/frame_base.cpp

@@ -15,7 +15,6 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
             NodeBase("FRAME", "Base"),
             trajectory_ptr_(),
             state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
-            is_removing_(false),
             frame_id_(++frame_id_count_),
             type_(NON_KEY_FRAME),
             time_stamp_(_ts)
@@ -29,7 +28,6 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
             NodeBase("FRAME", "Base"),
             trajectory_ptr_(),
             state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
-            is_removing_(false),
             frame_id_(++frame_id_count_),
             type_(_tp),
             time_stamp_(_ts)

src/frame_base.h

@@ -37,7 +37,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity.
 
         static unsigned int frame_id_count_;
-        bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
 
     protected:
         unsigned int frame_id_;
@@ -67,7 +66,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
 
         virtual ~FrameBase();
-        void remove();
+        virtual void remove();
 
 
 

src/landmark_base.cpp

@@ -13,7 +13,6 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
             NodeBase("LANDMARK", _type),
             map_ptr_(),
             state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed.
-            is_removing_(false),
             landmark_id_(++landmark_id_count_)
 {
     state_block_vec_[0] = _p_ptr;

src/landmark_base.h

@@ -29,7 +29,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
         std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O.
 
         static unsigned int landmark_id_count_;
-        bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
 
     protected:
         unsigned int landmark_id_; ///< landmark unique id
@@ -48,7 +47,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
          **/
         LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
         virtual ~LandmarkBase();
-        void remove();
+        virtual void remove();
         virtual YAML::Node saveToYaml() const;
 
         // Properties

src/node_base.h

@@ -67,6 +67,8 @@ class NodeBase
         std::string node_type_;  ///< Text label identifying the type or subcategory of node ("Pin Hole", "Point 2D", etc)
         std::string node_name_;  ///< Text label identifying each specific object ("left camera", "LIDAR 1", "PointGrey", "Andrew", etc)
 
+        bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
+
     public: 
 
         NodeBase(const std::string& _category, const std::string& _type = "Undefined", const std::string& _name = "");
@@ -76,6 +78,7 @@ class NodeBase
         std::string getCategory() const;
         std::string getType()  const;
         std::string getName()  const;
+        bool isRemoving() const;
 
         void setType(const std::string& _type);
         void setName(const std::string& _name);
@@ -95,7 +98,8 @@ inline NodeBase::NodeBase(const std::string& _category, const std::string& _type
         node_id_(++node_id_count_),
         node_category_(_category),
         node_type_(_type),
-        node_name_(_name)
+        node_name_(_name),
+        is_removing_(false)
 {
     //
 }
@@ -120,6 +124,11 @@ inline std::string NodeBase::getName() const
     return node_name_;
 }
 
+inline bool NodeBase::isRemoving() const
+{
+    return is_removing_;
+}
+
 inline void NodeBase::setType(const std::string& _type)
 {
     node_type_ = _type;