Modified factor block diff name and constructor to add the possibility to link...

Modified factor block diff name and constructor to add the possibility to link Frame/Capt.. other to the factor

include/core/factor/factor_block_absolute.h

@@ -41,7 +41,7 @@ class FactorBlockAbsolute : public FactorAnalytic
                             ProcessorBasePtr _processor_ptr = nullptr,
                             bool _apply_loss_function = false,
                             FactorStatus _status = FAC_ACTIVE) :
-            FactorAnalytic("BLOCK ABS",
+            FactorAnalytic("FactorBlockAbsolute",
                            nullptr,
                            nullptr,
                            nullptr,

include/core/factor/factor_block_difference.h

@@ -39,6 +39,10 @@ class FactorBlockDifference : public FactorAnalytic
         FactorBlockDifference(
                             StateBlockPtr _sb1_ptr,
                             StateBlockPtr _sb2_ptr,
+                            FrameBasePtr _frame_other = nullptr,
+                            CaptureBasePtr _cap_other = nullptr,
+                            FeatureBasePtr _feat_other = nullptr,
+                            LandmarkBasePtr _lmk_other = nullptr,
                             unsigned int _start_idx1 = 0,
                             int _size1 = -1,
                             unsigned int _start_idx2 = 0,
@@ -46,11 +50,11 @@ class FactorBlockDifference : public FactorAnalytic
                             ProcessorBasePtr _processor_ptr = nullptr,
                             bool _apply_loss_function = false,
                             FactorStatus _status = FAC_ACTIVE) :
-            FactorAnalytic("BLOCK ABS",
-                           nullptr,
-                           nullptr,
-                           nullptr,
-                           nullptr,
+            FactorAnalytic("FactorBlockDifference",
+                           _frame_other,
+                           _cap_other,
+                           _feat_other,
+                           _lmk_other,
                            _processor_ptr,
                            _apply_loss_function,
                            _status,
@@ -146,6 +150,8 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
     assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
     assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
     assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+    assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+    assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
 
     // normalized jacobian
     _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
@@ -163,6 +169,8 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
     assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
     assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
     assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+    assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+    assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
 
     // normalized jacobian
     _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;

test/gtest_factor_block_difference.cpp

@@ -50,18 +50,17 @@ class FixtureFactorBlockDifference : public testing::Test
 
             Vector10d x_origin = problem_->zeroState();
             Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity(); 
-            KF0_ =problem_->setPrior(x_origin, 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);
 
-            Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "DIFF", t1);
+            Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
             Eigen::Matrix3d cov = 0.2 * Eigen::Matrix3d::Identity();
-            Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "DIFF", zero3, cov);
+            Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, cov);
         }
 
         virtual void TearDown() override {}
@@ -150,6 +149,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPx)
     Feat_->setMeasurementCovariance(cov_diff);
     FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
         Feat_, KF0_->getP(), KF1_->getP(),
+        nullptr, nullptr, nullptr, nullptr,
         0, 1, 0, 1
     );
 
@@ -174,6 +174,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPxy)
     Feat_->setMeasurementCovariance(cov_diff);
     FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
         Feat_, KF0_->getP(), KF1_->getP(),
+        nullptr, nullptr, nullptr, nullptr,
         0, 2, 0, 2
     );
 
@@ -195,6 +196,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz)
     Feat_->setMeasurementCovariance(cov_diff);
     FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
         Feat_, KF0_->getP(), KF1_->getP(),
+        nullptr, nullptr, nullptr, nullptr,
         1, 2, 1, 2
     );