Small correction: oposite residual, jacobians changed accordingly

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

test/gtest_factor_block_difference.cpp

@@ -202,10 +202,10 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz)
     KF0_->getP()->setState(Vector3d::Random());
     KF1_->getP()->setState(Vector3d::Random());
 
-    // problem_->print(4,true,true,true);
+    problem_->print(4,true,true,true);
     std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
-    // std::cout << report << std::endl;
-    // problem_->print(4,true,true,true);
+    std::cout << report << std::endl;
+    problem_->print(4,true,true,true);
 
     ASSERT_MATRIX_APPROX(KF1_->getP()->getState().segment<2>(1) - KF0_->getP()->getState().segment<2>(1), diff, 1e-8);
 }