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

@@ -26,8 +26,8 @@ class FactorBlockDifference : public FactorAnalytic
         SizeEigen sb1_constrained_size_;  // the size of sb1 the state segment that is constrained
         SizeEigen sb2_constrained_start_; // the index of the first state element of sb2 that is constrained
         SizeEigen sb2_constrained_size_;  // the size of sb2 the state segment that is constrained
-        Eigen::MatrixXd J1_;              // Jacobian
-        Eigen::MatrixXd J2_;              // Jacobian
+        Eigen::MatrixXd J_res_sb1_;              // Jacobian
+        Eigen::MatrixXd J_res_sb2_;              // Jacobian
 
     public:
 
@@ -69,11 +69,11 @@ class FactorBlockDifference : public FactorAnalytic
             assert(sb1_constrained_size_ == sb2_constrained_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_);
+            J_res_sb1_ = Eigen::MatrixXd::Zero(sb1_constrained_size_,sb1_size_);
+            J_res_sb1_.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_);
+            J_res_sb2_ = Eigen::MatrixXd::Zero(sb2_constrained_size_,sb2_size_);
+            J_res_sb2_.middleCols(sb2_constrained_start_,sb1_constrained_size_) = - Eigen::MatrixXd::Identity(sb2_constrained_size_,sb2_constrained_size_);
         }
 
         virtual ~FactorBlockDifference() = default;
@@ -131,8 +131,8 @@ inline Eigen::VectorXd FactorBlockDifference::evaluateResiduals(const std::vecto
     assert(getMeasurement().size() == sb2_constrained_size_);
 
     // residual: meas -> measurement of the difference between partial stateblocks
-    return getMeasurementSquareRootInformationUpper() * ((_st_vector[1].segment(sb1_constrained_start_, sb1_constrained_size_) 
-                                                        - _st_vector[0].segment(sb2_constrained_start_, sb2_constrained_size_)) - getMeasurement());
+    return getMeasurementSquareRootInformationUpper() * (getMeasurement() - (_st_vector[1].segment(sb2_constrained_start_, sb2_constrained_size_) 
+                                                                           - _st_vector[0].segment(sb1_constrained_start_, sb1_constrained_size_)));
 }
 
 inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
@@ -148,8 +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
-    _jacobians[0] = getMeasurementSquareRootInformationUpper() * J1_;
-    _jacobians[1] = getMeasurementSquareRootInformationUpper() * J2_;
+    _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+    _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
 }
 
 inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
@@ -165,16 +165,16 @@ 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
-    _jacobians[0] = getMeasurementSquareRootInformationUpper() * J1_;
-    _jacobians[1] = getMeasurementSquareRootInformationUpper() * J2_;
+    _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+    _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
 }
 
-inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
+inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& _pure_jacobians) const
 {
-    assert(jacobians.size() == 1 && "Wrong number of jacobians!");
+    assert(_pure_jacobians.size() == 1 && "Wrong number of jacobians!");
 
-    jacobians[0] = J1_;
-    jacobians[1] = J2_;
+    _pure_jacobians[0] = J_res_sb1_;
+    _pure_jacobians[1] = J_res_sb2_;
 }
 
 inline unsigned int FactorBlockDifference::getSize() const