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.

test/gtest_factor_block_difference.cpp

@@ -15,6 +15,7 @@
 #include "core/capture/capture_base.h"
 #include "core/feature/feature_base.h"
 #include "core/factor/factor_block_difference.h"
+#include "core/factor/factor_block_absolute.h"
 
 
 using namespace Eigen;
@@ -47,8 +48,14 @@ class FixtureFactorBlockDifference : public testing::Test
             TimeStamp t0(0);
             TimeStamp t1(1);
 
+            Vector10d x_origin = problem_->zeroState();
             Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity(); 
-            KF0_ =problem_->setPrior(problem_->zeroState(), cov_prior, t0, 0.1);
+            KF0_ =problem_->setPrior(x_origin, cov_prior, t0, 0.1);
+            
+            CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
+            FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
+            FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
+
             // KF0_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t0);
             KF1_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t1);
 
@@ -68,21 +75,68 @@ TEST_F(FixtureFactorBlockDifference, EqualP)
         Feat_, KF0_->getP(), KF1_->getP()
     );
 
-    KF1_->getO()->fix();
-    KF1_->getV()->fix();
-
     // perturbate
     KF1_->getP()->setState((Vector3d() << 1, 0, 0).finished());
 
-    problem_->print(4,1,1,1);
     std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
-    std::cout << report << std::endl;
-    problem_->print(4,1,1,1);
 
     ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), zero3, 1e-8);
 
 }
 
+TEST_F(FixtureFactorBlockDifference, EqualV)
+{
+    FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+        Feat_, KF0_->getV(), KF1_->getV()
+    );
+
+    // perturbate
+    KF1_->getV()->setState((Vector3d() << 1, 0, 0).finished());
+
+    std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), zero3, 1e-8);
+
+}
+
+
+
+TEST_F(FixtureFactorBlockDifference, DiffP)
+{
+    Vector3d diffP = Vector3d::Random();
+    Feat_->setMeasurement(diffP);
+    FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+        Feat_, KF0_->getP(), KF1_->getP()
+    );
+
+    // perturbate
+    KF1_->getP()->setState((Vector3d() << 1, 0, 0).finished());
+
+    std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), diffP, 1e-8);
+
+}
+
+
+
+TEST_F(FixtureFactorBlockDifference, DiffV)
+{
+    Vector3d diffV = Vector3d::Random();
+    Feat_->setMeasurement(diffV);
+    FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+        Feat_, KF0_->getV(), KF1_->getV()
+    );
+
+    // perturbate
+    KF1_->getV()->setState((Vector3d() << 1, 0, 0).finished());
+
+    std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), diffV, 1e-8);
+
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);