diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index 8fd52090ba8b012fecb3484c3659d0e37e3b9ed7..22ec994a4338e9ff15e5b7457ae5aafce52e9f0e 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/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
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 0945b328de7d8094d49624aad6a89b0af5708788..4585edaab87c4a94d23712cdafe9a7856c04510a 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/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)