New factor block difference

Implementation of a new type of factor defining the euclidean difference between 2 stateblocks. This is a analytic factor (jacobians are trivially +/-Identity). As the factor_block_absolute.h, on which this is inspired, it offers the possibility to constraint only segments of these 2 stateblocks, if these segments are consecutely stored in the stateblock (ex: for a position stateblock, it is possible to constraint only xy or yz but not xz). This could be implemented if necessary.

include/core/factor/factor_block_difference.h

@@ -63,13 +63,17 @@ class FactorBlockDifference : public FactorAnalytic
             sb2_constrained_start_(_start_idx2),
             sb2_constrained_size_(_size2 == -1 ? sb2_size_ : _size2)
         {
+            // Check if constrained portion is in the stateblock vector range 
             assert(sb1_constrained_size_+sb1_constrained_start_ <= sb1_size_);
             assert(sb2_constrained_size_+sb2_constrained_start_ <= sb2_size_);
             assert(sb1_constrained_size_ == sb2_constrained_size_);
 
-            // precompute Jacobian (Identity)
-            J1_ = Eigen::MatrixXd::Identity(sb1_constrained_size_, sb1_size_);
-            J2_ = Eigen::MatrixXd::Identity(sb2_constrained_size_, sb2_size_);
+            // precompute Jacobian (Identity with zero columns if the stateblock part is not constrained)
+            J1_ = Eigen::MatrixXd::Zero(sb1_constrained_size_,sb1_size_);
+            J1_.middleCols(sb1_constrained_start_,sb1_constrained_size_) = - Eigen::MatrixXd::Identity(sb1_constrained_size_,sb1_constrained_size_);
+
+            J2_ = Eigen::MatrixXd::Zero(sb2_constrained_size_,sb2_size_);
+            J2_.middleCols(sb2_constrained_start_,sb1_constrained_size_) =   Eigen::MatrixXd::Identity(sb2_constrained_size_,sb2_constrained_size_);
         }
 
         virtual ~FactorBlockDifference() = default;
@@ -119,12 +123,16 @@ class FactorBlockDifference : public FactorAnalytic
 
 inline Eigen::VectorXd FactorBlockDifference::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
 {
+    // Check measurement and cov sizes 
     assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
-    assert(_st_vector[0].size() >= getMeasurement().size() && "Wrong StateBlock size");
-    assert(_st_vector[1].size() >= getMeasurement().size() && "Wrong StateBlock size");
+    assert(getMeasurement().size() == getMeasurementCovariance().cols());
+    assert(getMeasurement().size() == getMeasurementCovariance().rows());
+    assert(getMeasurement().size() == sb1_constrained_size_);
+    assert(getMeasurement().size() == sb2_constrained_size_);
 
     // residual: meas -> measurement of the difference between partial stateblocks
-    return getMeasurementSquareRootInformationUpper() * ((_st_vector[1] - _st_vector[0]) - getMeasurement());
+    return getMeasurementSquareRootInformationUpper() * ((_st_vector[1].segment(sb1_constrained_start_, sb1_constrained_size_) 
+                                                        - _st_vector[0].segment(sb2_constrained_start_, sb2_constrained_size_)) - getMeasurement());
 }
 
 inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
@@ -140,13 +148,8 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
     assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
 
     // normalized jacobian
-    if (_compute_jacobian.front()){
-        _jacobians[0] = getMeasurementSquareRootInformationUpper() * J1_;
-        _jacobians[1] = getMeasurementSquareRootInformationUpper() * J2_;
-    }
-    else {
-        // TODO?
-    }
+    _jacobians[0] = getMeasurementSquareRootInformationUpper() * J1_;
+    _jacobians[1] = getMeasurementSquareRootInformationUpper() * J2_;
 }
 
 inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
@@ -162,10 +165,8 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
     assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
 
     // normalized jacobian
-    if (_compute_jacobian.front()){
-        _jacobians[0] = getMeasurementSquareRootInformationUpper() * J1_;
-        _jacobians[1] = getMeasurementSquareRootInformationUpper() * J2_;
-    }
+    _jacobians[0] = getMeasurementSquareRootInformationUpper() * J1_;
+    _jacobians[1] = getMeasurementSquareRootInformationUpper() * J2_;
 }
 
 inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const