Skip to content
Snippets Groups Projects
Commit 4d02d6d1 authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Added new gtests and fixed the partial state constraint case

parent 3b2db45f
No related branches found
No related tags found
1 merge request!336New factor block difference
Pipeline #4889 passed
This commit is part of merge request !336. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
include/core/factor/factor_block_difference.h
+ 18
− 17
View file @ 4d02d6d1
...@@ -63,13 +63,17 @@ class FactorBlockDifference : public FactorAnalytic ...@@ -63,13 +63,17 @@ class FactorBlockDifference : public FactorAnalytic
sb2_constrained_start_(_start_idx2), sb2_constrained_start_(_start_idx2),
sb2_constrained_size_(_size2 == -1 ? sb2_size_ : _size2) 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(sb1_constrained_size_+sb1_constrained_start_ <= sb1_size_);
assert(sb2_constrained_size_+sb2_constrained_start_ <= sb2_size_); assert(sb2_constrained_size_+sb2_constrained_start_ <= sb2_size_);
assert(sb1_constrained_size_ == sb2_constrained_size_); assert(sb1_constrained_size_ == sb2_constrained_size_);
// precompute Jacobian (Identity) // precompute Jacobian (Identity with zero columns if the stateblock part is not constrained)
J1_ = Eigen::MatrixXd::Identity(sb1_constrained_size_, sb1_size_); J1_ = Eigen::MatrixXd::Zero(sb1_constrained_size_,sb1_size_);
J2_ = Eigen::MatrixXd::Identity(sb2_constrained_size_, sb2_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; virtual ~FactorBlockDifference() = default;
...@@ -119,12 +123,16 @@ class FactorBlockDifference : public FactorAnalytic ...@@ -119,12 +123,16 @@ class FactorBlockDifference : public FactorAnalytic
inline Eigen::VectorXd FactorBlockDifference::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const 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.size() == 2 && "Wrong number of state blocks!");
assert(_st_vector[0].size() >= getMeasurement().size() && "Wrong StateBlock size"); assert(getMeasurement().size() == getMeasurementCovariance().cols());
assert(_st_vector[1].size() >= getMeasurement().size() && "Wrong StateBlock size"); 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 // 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, 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 ...@@ -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"); assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
// normalized jacobian // normalized jacobian
if (_compute_jacobian.front()){ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J1_;
_jacobians[0] = getMeasurementSquareRootInformationUpper() * J1_; _jacobians[1] = getMeasurementSquareRootInformationUpper() * J2_;
_jacobians[1] = getMeasurementSquareRootInformationUpper() * J2_;
}
else {
// TODO?
}
} }
inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector, 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 ...@@ -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"); assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
// normalized jacobian // normalized jacobian
if (_compute_jacobian.front()){ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J1_;
_jacobians[0] = getMeasurementSquareRootInformationUpper() * J1_; _jacobians[1] = getMeasurementSquareRootInformationUpper() * J2_;
_jacobians[1] = getMeasurementSquareRootInformationUpper() * J2_;
}
} }
inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
......
test/gtest_factor_block_difference.cpp
+ 84
− 11
View file @ 4d02d6d1
...@@ -76,8 +76,9 @@ TEST_F(FixtureFactorBlockDifference, EqualP) ...@@ -76,8 +76,9 @@ TEST_F(FixtureFactorBlockDifference, EqualP)
); );
// perturbate // 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); std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), zero3, 1e-8); ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), zero3, 1e-8);
...@@ -91,7 +92,8 @@ TEST_F(FixtureFactorBlockDifference, EqualV) ...@@ -91,7 +92,8 @@ TEST_F(FixtureFactorBlockDifference, EqualV)
); );
// perturbate // 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); std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
...@@ -103,18 +105,19 @@ TEST_F(FixtureFactorBlockDifference, EqualV) ...@@ -103,18 +105,19 @@ TEST_F(FixtureFactorBlockDifference, EqualV)
TEST_F(FixtureFactorBlockDifference, DiffP) TEST_F(FixtureFactorBlockDifference, DiffP)
{ {
Vector3d diffP = Vector3d::Random(); Vector3d diff = Vector3d::Random();
Feat_->setMeasurement(diffP); Feat_->setMeasurement(diff);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>( FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getP(), KF1_->getP() Feat_, KF0_->getP(), KF1_->getP()
); );
// perturbate // 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); 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) ...@@ -122,19 +125,89 @@ TEST_F(FixtureFactorBlockDifference, DiffP)
TEST_F(FixtureFactorBlockDifference, DiffV) TEST_F(FixtureFactorBlockDifference, DiffV)
{ {
Vector3d diffV = Vector3d::Random(); Vector3d diff = Vector3d::Random();
Feat_->setMeasurement(diffV); Feat_->setMeasurement(diff);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>( FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getV(), KF1_->getV() Feat_, KF0_->getV(), KF1_->getV()
); );
// perturbate // 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); 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) int main(int argc, char **argv)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment