Skip to content
Snippets Groups Projects
Commit 44be7ce1 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Merge branch 'devel' into 281-new-class-ismotion

parents 1180e69d 4d01724a
No related branches found
No related tags found
1 merge request!338Resolve "New class isMotion"
...@@ -235,6 +235,7 @@ SET(HDRS_FACTOR ...@@ -235,6 +235,7 @@ SET(HDRS_FACTOR
include/core/factor/factor_autodiff_distance_3D.h include/core/factor/factor_autodiff_distance_3D.h
include/core/factor/factor_base.h include/core/factor/factor_base.h
include/core/factor/factor_block_absolute.h include/core/factor/factor_block_absolute.h
include/core/factor/factor_block_difference.h
include/core/factor/factor_diff_drive.h include/core/factor/factor_diff_drive.h
include/core/factor/factor_odom_2D.h include/core/factor/factor_odom_2D.h
include/core/factor/factor_odom_2D_closeloop.h include/core/factor/factor_odom_2D_closeloop.h
......
/**
* \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 J_res_sb1_; // Jacobian
Eigen::MatrixXd J_res_sb2_; // 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)
{
// 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 with zero columns if the stateblock part is not constrained)
J_res_sb1_ = Eigen::MatrixXd::Zero(sb1_constrained_size_,sb1_size_);
J_res_sb1_.middleCols(sb1_constrained_start_,sb1_constrained_size_) = Eigen::MatrixXd::Identity(sb1_constrained_size_,sb1_constrained_size_);
J_res_sb2_ = Eigen::MatrixXd::Zero(sb2_constrained_size_,sb2_size_);
J_res_sb2_.middleCols(sb2_constrained_start_,sb1_constrained_size_) = - Eigen::MatrixXd::Identity(sb2_constrained_size_,sb2_constrained_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
{
// Check measurement and cov sizes
assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
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() * (getMeasurement() - (_st_vector[1].segment(sb2_constrained_start_, sb2_constrained_size_)
- _st_vector[0].segment(sb1_constrained_start_, sb1_constrained_size_)));
}
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
_jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
_jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
}
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
_jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
_jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
}
inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& _pure_jacobians) const
{
assert(_pure_jacobians.size() == 1 && "Wrong number of jacobians!");
_pure_jacobians[0] = J_res_sb1_;
_pure_jacobians[1] = J_res_sb2_;
}
inline unsigned int FactorBlockDifference::getSize() const
{
return sb1_constrained_size_;
}
} // namespace wolf
#endif
...@@ -53,7 +53,10 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature ...@@ -53,7 +53,10 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
* \param _measurement the measurement * \param _measurement the measurement
* \param _meas_covariance the noise of 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 ~FeatureBase();
virtual void remove(bool viral_remove_empty_parent=false); virtual void remove(bool viral_remove_empty_parent=false);
......
...@@ -25,8 +25,8 @@ void HasStateBlocks::removeStateBlocks(ProblemPtr _problem) ...@@ -25,8 +25,8 @@ void HasStateBlocks::removeStateBlocks(ProblemPtr _problem)
{ {
_problem->notifyStateBlock(sbp,REMOVE); _problem->notifyStateBlock(sbp,REMOVE);
} }
removeStateBlock(key);
} }
removeStateBlock(key);
} }
} }
......
...@@ -51,6 +51,10 @@ target_link_libraries(gtest_capture_base ${PROJECT_NAME}) ...@@ -51,6 +51,10 @@ target_link_libraries(gtest_capture_base ${PROJECT_NAME})
#wolf_add_gtest(gtest_factor_sparse gtest_factor_sparse.cpp) #wolf_add_gtest(gtest_factor_sparse gtest_factor_sparse.cpp)
#target_link_libraries(gtest_factor_sparse ${PROJECT_NAME}) #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 # FactorAutodiff class test
wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp) wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME}) target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME})
......
/**
* \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"
#include "core/factor/factor_block_absolute.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);
Vector10d x_origin = problem_->zeroState();
Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
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);
Eigen::Matrix3d cov = 0.2 * Eigen::Matrix3d::Identity();
Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "DIFF", zero3, cov);
}
virtual void TearDown() override {}
};
TEST_F(FixtureFactorBlockDifference, EqualP)
{
// Feat_->setMeasurement()
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getP(), KF1_->getP()
);
// 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() - KF0_->getP()->getState(), zero3, 1e-8);
}
TEST_F(FixtureFactorBlockDifference, EqualV)
{
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getV(), KF1_->getV()
);
// perturbate
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(), zero3, 1e-8);
}
TEST_F(FixtureFactorBlockDifference, DiffP)
{
Vector3d diff = Vector3d::Random();
Feat_->setMeasurement(diff);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getP(), KF1_->getP()
);
// 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() - KF0_->getP()->getState(), diff, 1e-8);
}
TEST_F(FixtureFactorBlockDifference, DiffV)
{
Vector3d diff = Vector3d::Random();
Feat_->setMeasurement(diff);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getV(), KF1_->getV()
);
// perturbate
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_->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)
{
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();
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment