From e9d7053792627c6f17098cdbc7684c7a1b4174ff Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Sat, 29 Feb 2020 14:37:43 +0100
Subject: [PATCH] New factor enforcing euclidean difference measurement between
2 states blocks using analytic jacobians
---
include/core/factor/factor_block_difference.h | 186 ++++++++++++++++++
include/core/feature/feature_base.h | 5 +-
test/CMakeLists.txt | 4 +
test/gtest_factor_block_difference.cpp | 92 +++++++++
4 files changed, 286 insertions(+), 1 deletion(-)
create mode 100644 include/core/factor/factor_block_difference.h
create mode 100644 test/gtest_factor_block_difference.cpp
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
new file mode 100644
index 000000000..8fd52090b
--- /dev/null
+++ b/include/core/factor/factor_block_difference.h
@@ -0,0 +1,186 @@
+/**
+ * \file factor_block_difference.h
+ *
+ * Created on: Feb 28, 2020
+ * \author: mfourmy
+ */
+
+#ifndef FACTOR_BLOCK_DIFFERENCE_H_
+#define FACTOR_BLOCK_DIFFERENCE_H_
+
+//Wolf includes
+#include "core/factor/factor_analytic.h"
+#include "core/frame/frame_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorBlockDifference);
+
+//class
+class FactorBlockDifference : public FactorAnalytic
+{
+ private:
+ SizeEigen sb1_size_; // the size of the state blocks
+ SizeEigen sb2_size_; // the size of the state blocks
+ SizeEigen sb1_constrained_start_; // the index of the first state element of sb1 that is constrained
+ SizeEigen sb1_constrained_size_; // the size of sb1 the state segment that is constrained
+ SizeEigen sb2_constrained_start_; // the index of the first state element of sb2 that is constrained
+ SizeEigen sb2_constrained_size_; // the size of sb2 the state segment that is constrained
+ Eigen::MatrixXd J1_; // Jacobian
+ Eigen::MatrixXd J2_; // Jacobian
+
+ public:
+
+ /** \brief Constructor
+ *
+ * \param _sb_ptr the constrained state block
+ *
+ */
+ FactorBlockDifference(
+ StateBlockPtr _sb1_ptr,
+ StateBlockPtr _sb2_ptr,
+ unsigned int _start_idx1 = 0,
+ int _size1 = -1,
+ unsigned int _start_idx2 = 0,
+ int _size2 = -1,
+ ProcessorBasePtr _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("BLOCK ABS",
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _sb1_ptr,
+ _sb2_ptr),
+ sb1_size_(_sb1_ptr->getSize()),
+ sb2_size_(_sb2_ptr->getSize()),
+ sb1_constrained_start_(_start_idx1),
+ sb1_constrained_size_(_size1 == -1 ? sb1_size_ : _size1),
+ sb2_constrained_start_(_start_idx2),
+ sb2_constrained_size_(_size2 == -1 ? sb2_size_ : _size2)
+ {
+ 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_);
+ }
+
+ virtual ~FactorBlockDifference() = default;
+
+ virtual std::string getTopology() const override
+ {
+ return std::string("DIFF");
+ }
+
+ /** \brief Returns the residual evaluated in the states provided
+ *
+ * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
+ *
+ **/
+ virtual Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
+
+ /** \brief Returns the normalized jacobians evaluated in the states
+ *
+ * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
+ * IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
+ *
+ * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
+ * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+ * \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
+ *
+ **/
+ virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
+ virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
+
+ /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
+ *
+ * Returns the pure jacobians (without measurement noise) evaluated in the current state blocks values
+ *
+ * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+ *
+ **/
+ virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
+
+ /** \brief Returns the factor residual size
+ **/
+ virtual unsigned int getSize() const override;
+};
+
+inline Eigen::VectorXd FactorBlockDifference::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
+{
+ 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");
+
+ // residual: meas -> measurement of the difference between partial stateblocks
+ return getMeasurementSquareRootInformationUpper() * ((_st_vector[1] - _st_vector[0]) - getMeasurement());
+}
+
+inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& _jacobians,
+ const std::vector<bool>& _compute_jacobian) const
+{
+ assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
+ assert(_jacobians.size() == 2 && "Wrong number of jacobians!");
+ assert(_compute_jacobian.size() == 2 && "Wrong number of _compute_jacobian booleans!");
+ assert(_st_vector[0].size() == sb1_size_ && "Wrong StateBlock size");
+ 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");
+
+ // normalized jacobian
+ if (_compute_jacobian.front()){
+ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J1_;
+ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J2_;
+ }
+ else {
+ // TODO?
+ }
+}
+
+inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& _jacobians,
+ const std::vector<bool>& _compute_jacobian) const
+{
+ assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
+ assert(_jacobians.size() == 2 && "Wrong number of jacobians!");
+ assert(_compute_jacobian.size() == 2 && "Wrong number of _compute_jacobian booleans!");
+ assert(_st_vector[0].size() == sb1_size_ && "Wrong StateBlock size");
+ 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");
+
+ // normalized jacobian
+ if (_compute_jacobian.front()){
+ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J1_;
+ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J2_;
+ }
+}
+
+inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
+{
+ assert(jacobians.size() == 1 && "Wrong number of jacobians!");
+
+ jacobians[0] = J1_;
+ jacobians[1] = J2_;
+}
+
+inline unsigned int FactorBlockDifference::getSize() const
+{
+ return sb1_constrained_size_;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 1eb9bd338..720342591 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -53,7 +53,10 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
* \param _measurement the measurement
* \param _meas_covariance the noise of the measurement
*/
- FeatureBase(const std::string& _type, const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_uncertainty, UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
+ FeatureBase(const std::string& _type,
+ const Eigen::VectorXd& _measurement,
+ const Eigen::MatrixXd& _meas_uncertainty,
+ UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
virtual ~FeatureBase();
virtual void remove(bool viral_remove_empty_parent=false);
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 49f5dfbb1..79fc2c60a 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -51,6 +51,10 @@ target_link_libraries(gtest_capture_base ${PROJECT_NAME})
#wolf_add_gtest(gtest_factor_sparse gtest_factor_sparse.cpp)
#target_link_libraries(gtest_factor_sparse ${PROJECT_NAME})
+# FactorBlockDifference class test
+wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
+target_link_libraries(gtest_factor_block_difference ${PROJECT_NAME})
+
# FactorAutodiff class test
wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME})
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
new file mode 100644
index 000000000..806914051
--- /dev/null
+++ b/test/gtest_factor_block_difference.cpp
@@ -0,0 +1,92 @@
+/**
+ * \file gtest_factor_block_difference.cpp
+ *
+ * Created on: Feb 29, 2020
+ * \author: mfourmy
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include <Eigen/Dense>
+#include "core/problem/problem.h"
+#include <core/ceres_wrapper/ceres_manager.h>
+
+#include "core/frame/frame_base.h"
+#include "core/capture/capture_base.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_block_difference.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+
+
+const Vector6d zero6 = Vector6d::Zero();
+const Vector3d zero3 = zero6.head<3>();
+
+
+class FixtureFactorBlockDifference : public testing::Test
+{
+ public:
+ ProblemPtr problem_;
+ CeresManagerPtr ceres_manager_;
+ FrameBasePtr KF0_;
+ FrameBasePtr KF1_;
+ CaptureBasePtr Cap_;
+ FeatureBasePtr Feat_;
+
+ protected:
+
+ virtual void SetUp() override
+ {
+ // Problem and solver
+ problem_ = Problem::create("POV", 3);
+ ceres::Solver::Options ceres_options;
+ ceres_manager_ = std::make_shared<CeresManager>(problem_, ceres_options);
+
+ TimeStamp t0(0);
+ TimeStamp t1(1);
+
+ Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
+ KF0_ =problem_->setPrior(problem_->zeroState(), cov_prior, t0, 0.1);
+ // KF0_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t0);
+ KF1_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t1);
+
+ Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "DIFF", t1);
+ Eigen::Matrix3d cov = 0.2 * Eigen::Matrix3d::Identity();
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "DIFF", zero3, cov);
+ }
+
+ virtual void TearDown(){}
+};
+
+
+TEST_F(FixtureFactorBlockDifference, EqualP)
+{
+ // Feat_->setMeasurement()
+ FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+ 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);
+
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ //::testing::GTEST_FLAG(filter) = "TestTest.DummyTestExample"; // Test only this one
+ //::testing::GTEST_FLAG(filter) = "TestTest.*"; // Test only the tests in this group
+ return RUN_ALL_TESTS();
+}
--
GitLab