Added new gtests and fixed the partial state constraint case

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

test/gtest_factor_block_difference.cpp

@@ -76,8 +76,9 @@ TEST_F(FixtureFactorBlockDifference, EqualP)
     );
 
     // perturbate
-    KF1_->getP()->setState((Vector3d() << 1, 0, 0).finished());
-
+    KF0_->getP()->setState(Vector3d::Random());
+    KF1_->getP()->setState(Vector3d::Random());
+    
     std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), zero3, 1e-8);
@@ -91,7 +92,8 @@ TEST_F(FixtureFactorBlockDifference, EqualV)
     );
 
     // perturbate
-    KF1_->getV()->setState((Vector3d() << 1, 0, 0).finished());
+    KF0_->getV()->setState(Vector3d::Random());
+    KF1_->getV()->setState(Vector3d::Random());
 
     std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
 
@@ -103,18 +105,19 @@ TEST_F(FixtureFactorBlockDifference, EqualV)
 
 TEST_F(FixtureFactorBlockDifference, DiffP)
 {
-    Vector3d diffP = Vector3d::Random();
-    Feat_->setMeasurement(diffP);
+    Vector3d diff = Vector3d::Random();
+    Feat_->setMeasurement(diff);
     FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
         Feat_, KF0_->getP(), KF1_->getP()
     );
 
     // perturbate
-    KF1_->getP()->setState((Vector3d() << 1, 0, 0).finished());
+    KF0_->getP()->setState(Vector3d::Random());
+    KF1_->getP()->setState(Vector3d::Random());
 
     std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
 
-    ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), diffP, 1e-8);
+    ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), diff, 1e-8);
 
 }
 
@@ -122,19 +125,89 @@ TEST_F(FixtureFactorBlockDifference, DiffP)
 
 TEST_F(FixtureFactorBlockDifference, DiffV)
 {
-    Vector3d diffV = Vector3d::Random();
-    Feat_->setMeasurement(diffV);
+    Vector3d diff = Vector3d::Random();
+    Feat_->setMeasurement(diff);
     FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
         Feat_, KF0_->getV(), KF1_->getV()
     );
 
     // perturbate
-    KF1_->getV()->setState((Vector3d() << 1, 0, 0).finished());
+    KF0_->getV()->setState(Vector3d::Random());
+    KF1_->getV()->setState(Vector3d::Random());
+
+    std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), diff, 1e-8);
+}
+
+
+TEST_F(FixtureFactorBlockDifference, DiffPx)
+{
+    // Vector3d diff = Vector3d::Random();
+    Vector1d diff; diff << 1;  // measurement still of the same size as the constrained state
+    Matrix1d cov_diff = 1e-4 * Matrix1d::Identity();  // measurement still of the same size as the constrained state
+    Feat_->setMeasurement(diff);
+    Feat_->setMeasurementCovariance(cov_diff);
+    FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+        Feat_, KF0_->getP(), KF1_->getP(),
+        0, 1, 0, 1
+    );
+
+    // perturbate
+    KF0_->getP()->setState(Vector3d::Random());
+    KF1_->getP()->setState(Vector3d::Random());
+
+    std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_MATRIX_APPROX(KF1_->getP()->getState().head<1>() - KF0_->getP()->getState().head<1>(), diff.head<1>(), 1e-8);
+}
+
+
+
+
+TEST_F(FixtureFactorBlockDifference, DiffPxy)
+{
+    // Vector3d diff = Vector3d::Random();
+    Vector2d diff; diff << 1, 2;  // measurement still of the same size as the constrained state
+    Matrix2d cov_diff = 1e-4 * Matrix2d::Identity();  // measurement still of the same size as the constrained state
+    Feat_->setMeasurement(diff);
+    Feat_->setMeasurementCovariance(cov_diff);
+    FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+        Feat_, KF0_->getP(), KF1_->getP(),
+        0, 2, 0, 2
+    );
+
+    // perturbate
+    KF0_->getP()->setState(Vector3d::Random());
+    KF1_->getP()->setState(Vector3d::Random());
 
     std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
 
-    ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), diffV, 1e-8);
+    ASSERT_MATRIX_APPROX(KF1_->getP()->getState().head<2>() - KF0_->getP()->getState().head<2>(), diff, 1e-8);
+}
+
+TEST_F(FixtureFactorBlockDifference, DiffPyz)
+{
+    // Vector3d diff = Vector3d::Random();
+    Vector2d diff; diff << 1, 2;  // measurement still of the same size as the constrained state
+    Matrix2d cov_diff = 1e-4 * Matrix2d::Identity();  // measurement still of the same size as the constrained state
+    Feat_->setMeasurement(diff);
+    Feat_->setMeasurementCovariance(cov_diff);
+    FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+        Feat_, KF0_->getP(), KF1_->getP(),
+        1, 2, 1, 2
+    );
+
+    // perturbate
+    KF0_->getP()->setState(Vector3d::Random());
+    KF1_->getP()->setState(Vector3d::Random());
+
+    // 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);
 
+    ASSERT_MATRIX_APPROX(KF1_->getP()->getState().segment<2>(1) - KF0_->getP()->getState().segment<2>(1), diff, 1e-8);
 }
 
 int main(int argc, char **argv)