From 8d742451891c237ff5835c5d897410765d1176c8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Fri, 27 Mar 2020 14:44:17 +0100
Subject: [PATCH] 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 | 2 +-
include/core/factor/factor_block_difference.h | 18 +++++++++++++-----
test/gtest_factor_block_difference.cpp | 10 ++++++----
3 files changed, 20 insertions(+), 10 deletions(-)
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index eb55faae7..0743548b7 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/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,
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index 8f62b6383..2ece75d51 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/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_;
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 20bbe2c42..1e06c7a80 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/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
);
--
GitLab