diff --git a/demos/solver/test_SPQR.cpp b/demos/solver/test_SPQR.cpp index b5f159c731741dabe4ed00bbe8eaa0e7968be7f3..56df88821b3441b9e1526f713741ffbfc49acedc 100644 --- a/demos/solver/test_SPQR.cpp +++ b/demos/solver/test_SPQR.cpp @@ -27,8 +27,8 @@ */ #include <iostream> -#include <eigen3/Eigen/SPQRSupport> -#include <eigen3/Eigen/CholmodSupport> +#include <Eigen/SPQRSupport> +#include <Eigen/CholmodSupport> #include "SuiteSparseQR.hpp" using namespace Eigen; diff --git a/demos/solver/test_ccolamd.cpp b/demos/solver/test_ccolamd.cpp index 98f496f26b5de691a38384d42b3349d62fc8b20e..126fa7f869f203188a3c589bf68fced8f4e593ff 100644 --- a/demos/solver/test_ccolamd.cpp +++ b/demos/solver/test_ccolamd.cpp @@ -43,9 +43,9 @@ #include "solver/ccolamd_ordering.h" // eigen includes -#include <eigen3/Eigen/OrderingMethods> -#include <eigen3/Eigen/CholmodSupport> -#include <eigen3/Eigen/SparseLU> +#include <Eigen/OrderingMethods> +#include <Eigen/CholmodSupport> +#include <Eigen/SparseLU> using namespace Eigen; using namespace wolf; diff --git a/demos/solver/test_ccolamd_blocks.cpp b/demos/solver/test_ccolamd_blocks.cpp index 14bb174286235f063a375cca4e28f1e4dfeca537..2d82633432d7620f108b8ef8532c4d08f0a17b9f 100644 --- a/demos/solver/test_ccolamd_blocks.cpp +++ b/demos/solver/test_ccolamd_blocks.cpp @@ -37,9 +37,9 @@ #include <queue> // eigen includes -#include <eigen3/Eigen/OrderingMethods> -#include <eigen3/Eigen/CholmodSupport> -#include <eigen3/Eigen/SparseLU> +#include <Eigen/OrderingMethods> +#include <Eigen/CholmodSupport> +#include <Eigen/SparseLU> // ccolamd #include "solver/ccolamd_ordering.h" diff --git a/demos/solver/test_iQR.cpp b/demos/solver/test_iQR.cpp index 53a37cef1a62ec1ffb281a6fdbd741fc73cb59f8..ba4ae194a00c6e9c8a78affc1a54fc02b1f52bb1 100644 --- a/demos/solver/test_iQR.cpp +++ b/demos/solver/test_iQR.cpp @@ -44,9 +44,9 @@ #include <queue> // eigen includes -#include <eigen3/Eigen/OrderingMethods> -#include <eigen3/Eigen/SparseQR> -#include <eigen3/Eigen/SPQRSupport> +#include <Eigen/OrderingMethods> +#include <Eigen/SparseQR> +#include <Eigen/SPQRSupport> // ccolamd #include "solver/ccolamd_ordering.h" diff --git a/demos/solver/test_iQR_wolf.cpp b/demos/solver/test_iQR_wolf.cpp index a8df237237669b54aa837d32f35bf8c156fa33eb..4ffc4a0bffada8742a20dd9d00b220c7544be156 100644 --- a/demos/solver/test_iQR_wolf.cpp +++ b/demos/solver/test_iQR_wolf.cpp @@ -38,8 +38,8 @@ #include <queue> // eigen includes -#include <eigen3/Eigen/OrderingMethods> -#include <eigen3/Eigen/SparseQR> +#include <Eigen/OrderingMethods> +#include <Eigen/SparseQR> #include <Eigen/SPQRSupport> // ccolamd diff --git a/demos/solver/test_incremental_ccolamd_blocks.cpp b/demos/solver/test_incremental_ccolamd_blocks.cpp index 740407a9a560f0efdbdb8267ee7925c22fbf12bf..f0eabf66cde8d920265f3af6a5b4e8a994fa5858 100644 --- a/demos/solver/test_incremental_ccolamd_blocks.cpp +++ b/demos/solver/test_incremental_ccolamd_blocks.cpp @@ -37,9 +37,9 @@ #include <queue> // eigen includes -#include <eigen3/Eigen/OrderingMethods> -#include <eigen3/Eigen/CholmodSupport> -#include <eigen3/Eigen/SparseLU> +#include <Eigen/OrderingMethods> +#include <Eigen/CholmodSupport> +#include <Eigen/SparseLU> // ccolamd #include "solver/ccolamd_ordering.h" diff --git a/demos/solver/test_permutations.cpp b/demos/solver/test_permutations.cpp index 0765d8ae77996a72b027a62380804243aa0aedfa..6724ea83f167bbacbfceed245a01b7c5cfdeea60 100644 --- a/demos/solver/test_permutations.cpp +++ b/demos/solver/test_permutations.cpp @@ -37,7 +37,7 @@ #include <queue> // eigen includes -#include <eigen3/Eigen/OrderingMethods> +#include <Eigen/OrderingMethods> using namespace Eigen; diff --git a/include/core/ceres_wrapper/sparse_utils.h b/include/core/ceres_wrapper/sparse_utils.h index 9c2e6422e84a8747ec8a468f7ba7c1fca1089394..314a31bb92f722eed3743778ceaa388ace44f1a4 100644 --- a/include/core/ceres_wrapper/sparse_utils.h +++ b/include/core/ceres_wrapper/sparse_utils.h @@ -22,7 +22,7 @@ #pragma once // eigen includes -//#include <eigen3/Eigen/Sparse> +//#include <Eigen/Sparse> namespace wolf { diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 8a366ebccad93c0f26f0368238e84f2e93b6ce51..9a782ab6e936f6dd79746061c5d68fb7bb1ffa72 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -148,17 +148,17 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha bool isConstrainedBy(const FactorBaseConstPtr& _factor) const; template <class C> - CaptureBaseConstPtr getCaptureOfType() const; + std::shared_ptr<const C> getCaptureOfType() const; template <class C> - CaptureBasePtr getCaptureOfType(); + std::shared_ptr<C> getCaptureOfType(); CaptureBaseConstPtr getCaptureOfType(const std::string& type) const; CaptureBasePtr getCaptureOfType(const std::string& type); template <class C> - CaptureBaseConstPtrList getCapturesOfType() const; + std::list<std::shared_ptr<const C>> getCapturesOfType() const; template <class C> - CaptureBasePtrList getCapturesOfType(); + std::list<std::shared_ptr<C>> getCapturesOfType(); CaptureBaseConstPtrList getCapturesOfType(const std::string& type) const; CaptureBasePtrList getCapturesOfType(const std::string& type); @@ -299,40 +299,52 @@ inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr) } template <class C> -inline CaptureBaseConstPtr FrameBase::getCaptureOfType() const +inline std::shared_ptr<const C> FrameBase::getCaptureOfType() const { for (auto capture_ptr : getCaptureList()) - if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr) - return capture_ptr; + { + auto cap_derived = std::dynamic_pointer_cast<const C>(capture_ptr); + if (cap_derived) + return cap_derived; + } return nullptr; } template <class C> -inline CaptureBasePtr FrameBase::getCaptureOfType() +inline std::shared_ptr<C> FrameBase::getCaptureOfType() { for (auto capture_ptr : getCaptureList()) - if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr) - return capture_ptr; + { + auto cap_derived = std::dynamic_pointer_cast<C>(capture_ptr); + if (cap_derived) + return cap_derived; + } return nullptr; } template <class C> -inline CaptureBaseConstPtrList FrameBase::getCapturesOfType() const +inline std::list<std::shared_ptr<const C>> FrameBase::getCapturesOfType() const { - CaptureBaseConstPtrList captures; + std::list<std::shared_ptr<const C>> captures; for (auto capture_ptr : getCaptureList()) - if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr) - captures.push_back(capture_ptr); + { + auto cap_derived = std::dynamic_pointer_cast<const C>(capture_ptr); + if (cap_derived) + captures.push_back(cap_derived); + } return captures; } template <class C> -inline CaptureBasePtrList FrameBase::getCapturesOfType() +inline std::list<std::shared_ptr<C>> FrameBase::getCapturesOfType() { - CaptureBasePtrList captures; + std::list<std::shared_ptr<C>> captures; for (auto capture_ptr : getCaptureList()) - if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr) - captures.push_back(capture_ptr); + { + auto cap_derived = std::dynamic_pointer_cast<C>(capture_ptr); + if (cap_derived) + captures.push_back(cap_derived); + } return captures; } diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h index 343247a93f07350eacb9296bbfdbf201962db4a9..7c1365a1ec0a8d82c2aef1e3390613ac10ee8851 100644 --- a/include/core/processor/motion_buffer.h +++ b/include/core/processor/motion_buffer.h @@ -92,7 +92,7 @@ class MotionBuffer : public std::list<Motion> const Motion& getMotion(const TimeStamp& _ts) const; void getMotion(const TimeStamp& _ts, Motion& _motion) const; void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer); - void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0); + void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0, bool show_covs = 0); }; } // namespace wolf \ No newline at end of file diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h index 8b81af6a87015310996aa535239a38b9c3f5a103..725c33ef81b950811a9a4f60cf7aec58f63afe4e 100644 --- a/include/core/processor/processor_diff_drive.h +++ b/include/core/processor/processor_diff_drive.h @@ -55,30 +55,30 @@ class ProcessorDiffDrive : public ProcessorOdom2d protected: // Motion integration - void computeCurrentDelta(const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - const Eigen::VectorXd& _calib, - const double _dt, - Eigen::VectorXd& _delta, - Eigen::MatrixXd& _delta_cov, - Eigen::MatrixXd& _jacobian_calib) const override; - CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, - const SensorBasePtr& _sensor, - const TimeStamp& _ts, - const VectorXd& _data, - const MatrixXd& _data_cov, - const VectorXd& _calib, - const VectorXd& _calib_preint, - const CaptureBasePtr& _capture_origin) override; - FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override; - FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, - CaptureBasePtr _capture_origin) override; - VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override; - void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override; + void computeCurrentDelta(const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + const Eigen::VectorXd& _calib, + const double _dt, + Eigen::VectorXd& _delta, + Eigen::MatrixXd& _delta_cov, + Eigen::MatrixXd& _jacobian_calib) const override; + + CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, + const SensorBasePtr& _sensor, + const TimeStamp& _ts, + const VectorXd& _data, + const MatrixXd& _data_cov, + const VectorXd& _calib, + const VectorXd& _calib_preint, + const CaptureBasePtr& _capture_origin) override; + void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override; + + VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override; + void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override; protected: - ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_; - double radians_per_tick_; + ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_; + double radians_per_tick_; }; diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index dbc86ff01abcb16fa27605781326fb66e30a62b3..0d68a216211c1169e62c0daedf8056952524bb24 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -468,16 +468,17 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider const VectorXd& _calib_preint, const CaptureBasePtr& _capture_origin_ptr) = 0; - /** \brief emplace a feature corresponding to given capture and add the feature to this capture - * \param _capture_motion: the parent capture - */ - virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) = 0; - /** \brief emplace a factor and link it in the wolf tree - * \param _feature_motion: the parent feature - * \param _frame_origin: the frame constrained by this motion factor + /** \brief emplace the features and factors corresponding to given capture and link them to the capture + * \param _capture_own: the parent capture + * \param _capture_origin: the capture constrained by this motion factor + * + * Typical factors to add for a ProcessorMotionDerived can be: + * - A preintegrated motion factor -- this is the main factor + * - A calibration drift factor -- only for dynamic sensor calibration parameters + * - An instantaneous factor for observing non-typical states of the Frame -- useful for complex dynamic robot models */ - virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0; + virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) = 0; virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0; diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h index e5dd0d822f34922f8afeba03ee71ab85d7b8d68c..f1b04fe357035477f51f51c26b39b5f9eb88b599 100644 --- a/include/core/processor/processor_odom_2d.h +++ b/include/core/processor/processor_odom_2d.h @@ -62,44 +62,42 @@ class ProcessorOdom2d : public ProcessorMotion bool voteForKeyFrame() const override; protected: - void computeCurrentDelta(const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - const Eigen::VectorXd& _calib, - const double _dt, - Eigen::VectorXd& _delta, - Eigen::MatrixXd& _delta_cov, - Eigen::MatrixXd& _jacobian_calib) const override; - void deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _Dt2, - Eigen::VectorXd& _delta1_plus_delta2) const override; - void deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _Dt2, - Eigen::VectorXd& _delta1_plus_delta2, - Eigen::MatrixXd& _jacobian1, - Eigen::MatrixXd& _jacobian2) const override; - void statePlusDelta(const VectorComposite& _x, - const Eigen::VectorXd& _delta, - const double _Dt, - VectorComposite& _x_plus_delta) const override; - Eigen::VectorXd deltaZero() const override; - Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint, - const Eigen::VectorXd& delta_step) const override; - - CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, - const SensorBasePtr& _sensor, - const TimeStamp& _ts, - const VectorXd& _data, - const MatrixXd& _data_cov, - const VectorXd& _calib, - const VectorXd& _calib_preint, - const CaptureBasePtr& _capture_origin_ptr) override; - FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override; - FactorBasePtr emplaceFactor(FeatureBasePtr _feature, - CaptureBasePtr _capture_origin) override; - VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override; - void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override; + void computeCurrentDelta(const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + const Eigen::VectorXd& _calib, + const double _dt, + Eigen::VectorXd& _delta, + Eigen::MatrixXd& _delta_cov, + Eigen::MatrixXd& _jacobian_calib) const override; + void deltaPlusDelta(const Eigen::VectorXd& _delta1, + const Eigen::VectorXd& _delta2, + const double _Dt2, + Eigen::VectorXd& _delta1_plus_delta2) const override; + void deltaPlusDelta(const Eigen::VectorXd& _delta1, + const Eigen::VectorXd& _delta2, + const double _Dt2, + Eigen::VectorXd& _delta1_plus_delta2, + Eigen::MatrixXd& _jacobian1, + Eigen::MatrixXd& _jacobian2) const override; + void statePlusDelta(const VectorComposite& _x, + const Eigen::VectorXd& _delta, + const double _Dt, + VectorComposite& _x_plus_delta) const override; + Eigen::VectorXd deltaZero() const override; + Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint, + const Eigen::VectorXd& delta_step) const override; + + CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, + const SensorBasePtr& _sensor, + const TimeStamp& _ts, + const VectorXd& _data, + const MatrixXd& _data_cov, + const VectorXd& _calib, + const VectorXd& _calib_preint, + const CaptureBasePtr& _capture_origin_ptr) override; + void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override; + VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override; + void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override; protected: ParamsProcessorOdom2dPtr params_odom_2d_; diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h index 8118344916d696f262fc3227dab735e2b56a4cc7..a06085407e85a45268e9336890d6ec73c8e8760f 100644 --- a/include/core/processor/processor_odom_3d.h +++ b/include/core/processor/processor_odom_3d.h @@ -81,47 +81,45 @@ class ProcessorOdom3d : public ProcessorMotion public: // Motion integration - void computeCurrentDelta(const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - const Eigen::VectorXd& _calib, - const double _dt, - Eigen::VectorXd& _delta, - Eigen::MatrixXd& _delta_cov, - Eigen::MatrixXd& _jacobian_calib) const override; - void deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _Dt2, - Eigen::VectorXd& _delta1_plus_delta2) const override; - void deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _Dt2, - Eigen::VectorXd& _delta1_plus_delta2, - Eigen::MatrixXd& _jacobian1, - Eigen::MatrixXd& _jacobian2) const override; - void statePlusDelta(const VectorComposite& _x, - const Eigen::VectorXd& _delta, - const double _Dt, - VectorComposite& _x_plus_delta) const override; - Eigen::VectorXd deltaZero() const override; - Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint, - const Eigen::VectorXd& delta_step) const override; + void computeCurrentDelta(const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + const Eigen::VectorXd& _calib, + const double _dt, + Eigen::VectorXd& _delta, + Eigen::MatrixXd& _delta_cov, + Eigen::MatrixXd& _jacobian_calib) const override; + void deltaPlusDelta(const Eigen::VectorXd& _delta1, + const Eigen::VectorXd& _delta2, + const double _Dt2, + Eigen::VectorXd& _delta1_plus_delta2) const override; + void deltaPlusDelta(const Eigen::VectorXd& _delta1, + const Eigen::VectorXd& _delta2, + const double _Dt2, + Eigen::VectorXd& _delta1_plus_delta2, + Eigen::MatrixXd& _jacobian1, + Eigen::MatrixXd& _jacobian2) const override; + void statePlusDelta(const VectorComposite& _x, + const Eigen::VectorXd& _delta, + const double _Dt, + VectorComposite& _x_plus_delta) const override; + Eigen::VectorXd deltaZero() const override; + Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint, + const Eigen::VectorXd& delta_step) const override; - bool voteForKeyFrame() const override; - CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, - const SensorBasePtr& _sensor, - const TimeStamp& _ts, - const VectorXd& _data, - const MatrixXd& _data_cov, - const VectorXd& _calib, - const VectorXd& _calib_preint, - const CaptureBasePtr& _capture_origin) override; + bool voteForKeyFrame() const override; - FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override; + CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, + const SensorBasePtr& _sensor, + const TimeStamp& _ts, + const VectorXd& _data, + const MatrixXd& _data_cov, + const VectorXd& _calib, + const VectorXd& _calib_preint, + const CaptureBasePtr& _capture_origin) override; + void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override; - FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, - CaptureBasePtr _capture_origin) override; - VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override; - void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override; + VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override; + void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override; protected: ParamsProcessorOdom3dPtr params_odom_3d_; diff --git a/include/core/solver_suitesparse/ccolamd_ordering.h b/include/core/solver_suitesparse/ccolamd_ordering.h index eee05794ae2aa545f1ccdd628ce628d99d2b005c..e1187b9d9580c0e9516b473723f1d8364ee2f040 100644 --- a/include/core/solver_suitesparse/ccolamd_ordering.h +++ b/include/core/solver_suitesparse/ccolamd_ordering.h @@ -25,9 +25,9 @@ #include <iostream> // Eigen includes -#include <eigen3/Eigen/OrderingMethods> -#include <eigen3/Eigen/CholmodSupport> -#include <eigen3/Eigen/SparseLU> +#include <Eigen/OrderingMethods> +#include <Eigen/CholmodSupport> +#include <Eigen/SparseLU> // ccolamd #include "ccolamd.h" diff --git a/include/core/solver_suitesparse/qr_solver.h b/include/core/solver_suitesparse/qr_solver.h index f715761d8514009682066365cf67c59c72e227e3..06b87b641fcec83e1f7d3f29f5e62ecae9cb1171 100644 --- a/include/core/solver_suitesparse/qr_solver.h +++ b/include/core/solver_suitesparse/qr_solver.h @@ -38,8 +38,8 @@ #include "solver/cost_function_sparse.h" // eigen includes -#include <eigen3/Eigen/OrderingMethods> -#include <eigen3/Eigen/SparseQR> +#include <Eigen/OrderingMethods> +#include <Eigen/SparseQR> #include <Eigen/StdVector> #include "core/factor/factor_pose_2d.h" diff --git a/include/core/solver_suitesparse/sparse_utils.h b/include/core/solver_suitesparse/sparse_utils.h index 4c0f01633c0a591d62c25daafd3029e10caccfee..fffb34f3abf69e82923d0037d29a58e7c0b503ee 100644 --- a/include/core/solver_suitesparse/sparse_utils.h +++ b/include/core/solver_suitesparse/sparse_utils.h @@ -22,7 +22,7 @@ #pragma once // eigen includes -#include <eigen3/Eigen/Sparse> +#include <Eigen/Sparse> namespace wolf { diff --git a/prova.txt b/prova.txt deleted file mode 100644 index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000 diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp index f61c1b1f9ef7ba4d4f3609fed41fedef53e3cbb8..6afd1c5c76c9c838e3072711b69ca199da0a48b9 100644 --- a/src/processor/motion_buffer.cpp +++ b/src/processor/motion_buffer.cpp @@ -144,14 +144,14 @@ void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before } -void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, bool show_jacs) +void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, bool show_jacs, bool show_covs) { using std::cout; using std::endl; - if (!show_data && !show_delta && !show_delta_int && !show_jacs) + if (!show_data && !show_delta && !show_delta_int && !show_jacs && !show_covs) { - cout << "Buffer state [" << this->size() << "] : <"; + cout << "Buffer size: " << this->size() << " ; time stamps: <"; for (Motion mot : *this) cout << " " << mot.ts_; cout << " >" << endl; @@ -165,17 +165,20 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b if (show_data) { cout << " data: " << mot.data_.transpose() << endl; - cout << " data cov: \n" << mot.data_cov_ << endl; + if (show_covs) + cout << " data cov: \n" << mot.data_cov_ << endl; } if (show_delta) { cout << " delta: " << mot.delta_.transpose() << endl; - cout << " delta cov: \n" << mot.delta_cov_ << endl; + if (show_covs) + cout << " delta cov: \n" << mot.delta_cov_ << endl; } if (show_delta_int) { cout << " delta integrated: " << mot.delta_integr_.transpose() << endl; - cout << " delta integrated cov: \n" << mot.delta_integr_cov_ << endl; + if (show_covs) + cout << " delta integrated cov: \n" << mot.delta_integr_cov_ << endl; } if (show_jacs) { diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp index 3e1ea99bb600d4a94fa67e58db8a8146ed3fd4ce..9c4a4e6926e1699f3faf4686952636873046783f 100644 --- a/src/processor/processor_diff_drive.cpp +++ b/src/processor/processor_diff_drive.cpp @@ -167,28 +167,22 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o return cap_motion; } -FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_motion) +void ProcessorDiffDrive::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) { - auto key_feature_ptr = FeatureBase::emplace<FeatureDiffDrive>(_capture_motion, - _capture_motion->getBuffer().back().delta_integr_, - _capture_motion->getBuffer().back().delta_integr_cov_, - _capture_motion->getCalibrationPreint(), - _capture_motion->getBuffer().back().jacobian_calib_); - - return key_feature_ptr; + auto feature = FeatureBase::emplace<FeatureDiffDrive>(_capture_own, + _capture_own->getBuffer().back().delta_integr_, + _capture_own->getBuffer().back().delta_integr_cov_, + _capture_own->getCalibrationPreint(), + _capture_own->getBuffer().back().jacobian_calib_); + + auto ftr_motion = std::static_pointer_cast<FeatureMotion>(feature); + FactorBase::emplace<FactorDiffDrive>(ftr_motion, + ftr_motion, + _capture_origin, + shared_from_this(), + params_prc_diff_drv_selfcal_->apply_loss_function); } -FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature, - CaptureBasePtr _capture_origin) -{ - auto ftr_motion = std::static_pointer_cast<FeatureMotion>(_feature); - auto fac_odom = FactorBase::emplace<FactorDiffDrive>(ftr_motion, - ftr_motion, - _capture_origin, - shared_from_this(), - params_prc_diff_drv_selfcal_->apply_loss_function); - return fac_odom; -} } /* namespace wolf */ diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index d47ea9b91149949c0f94dfa757746deec525982c..8549943aa71439543fdb74e0a66922342f8b80d0 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -58,11 +58,13 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, calib_preint_(_calib_size), jacobian_delta_preint_(delta_cov_size_, delta_cov_size_), jacobian_delta_(delta_cov_size_, delta_cov_size_), - jacobian_calib_(delta_cov_size_, calib_size_) + jacobian_calib_(delta_cov_size_, calib_size_), + jacobian_delta_calib_(delta_cov_size_, calib_size_) { jacobian_delta_preint_ .setIdentity(delta_cov_size_,delta_cov_size_); // dDp'/dDp, dDv'/dDv, all zeros jacobian_delta_ .setIdentity(delta_cov_size_,delta_cov_size_); // jacobian_calib_ .setZero(delta_cov_size_,calib_size_); + jacobian_delta_calib_ .setZero(delta_cov_size_,calib_size_); unmeasured_perturbation_cov_ = params_motion_->unmeasured_perturbation_std * params_motion_->unmeasured_perturbation_std @@ -109,8 +111,7 @@ void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev, // emplace new feature and factor (if origin has frame) if (cap_prev->getOriginCapture() and cap_prev->getOriginCapture()->getFrame()) { - auto new_feature = emplaceFeature(cap_target); - emplaceFactor(new_feature, cap_prev->getOriginCapture()); + emplaceFeaturesAndFactors(cap_prev->getOriginCapture(), cap_target); } } @@ -302,11 +303,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) // and give the part of the buffer before the new keyframe to the capture for the KF callback splitBuffer(capture_existing, timestamp_from_callback, capture_for_keyframe_callback); - // create motion feature and add it to the capture - auto feature_new = emplaceFeature(capture_for_keyframe_callback); - - // create motion factor and add it to the feature, and constrain to the other capture (origin) - emplaceFactor(feature_new, capture_origin ); + // // create motion feature and add it to the capture + // // create motion factor and add it to the feature, and constrain to the other capture (origin) + emplaceFeaturesAndFactors(capture_origin, capture_for_keyframe_callback); // modify existing feature and factor (if they exist in the existing capture) if (!capture_existing->getFeatureList().empty()) @@ -316,9 +315,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) assert(capture_existing->getFeatureList().empty());// there was only one feature! - auto new_feature_existing = emplaceFeature(capture_existing); - - emplaceFactor(new_feature_existing, capture_for_keyframe_callback); + emplaceFeaturesAndFactors(capture_for_keyframe_callback, capture_existing); } break; @@ -396,10 +393,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) splitBuffer(capture_existing, timestamp_from_callback, capture_for_keyframe_callback); // create motion feature and add it to the capture - auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback); - // create motion factor and add it to the feature, and constrain to the other capture (origin) - emplaceFactor(feature_for_keyframe_callback, capture_origin ); + emplaceFeaturesAndFactors(capture_origin, capture_for_keyframe_callback); // reset processor origin origin_ptr_ = capture_for_keyframe_callback; @@ -467,10 +462,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) keyframe ->link(getProblem()); // create motion feature and add it to the key_capture - auto key_feature = emplaceFeature(last_ptr_); - // create motion factor and link it to parent feature and other frame (which is origin's frame) - auto factor = emplaceFactor(key_feature, origin_ptr_); + emplaceFeaturesAndFactors(origin_ptr_, last_ptr_); // create a new frame auto frame_new = std::make_shared<FrameBase>(getTimeStamp(), @@ -985,23 +978,23 @@ FrameBasePtr ProcessorMotion::computeProcessingStep() if (checkTimeTolerance(keyframe_from_callback, incoming_ptr_)) { - WOLF_DEBUG("First time with a KF compatible.") + WOLF_DEBUG("PM ", getName(), ": First time with a KF compatible.") processing_step_ = FIRST_TIME_WITH_KF_ON_INCOMING; } else if (keyframe_from_callback->getTimeStamp() < incoming_ptr_->getTimeStamp()) { - WOLF_DEBUG("First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.") + WOLF_DEBUG("PM ", getName(), ": First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.") processing_step_ = FIRST_TIME_WITH_KF_BEFORE_INCOMING; } else { - WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'") + WOLF_DEBUG("PM ", getName(), ": First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'") processing_step_ = FIRST_TIME_WITH_KF_AFTER_INCOMING; } } else { - WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'") + WOLF_DEBUG("PM ", getName(), ": First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'") processing_step_ = FIRST_TIME_WITHOUT_KF; } @@ -1047,7 +1040,7 @@ TimeStamp ProcessorMotion::getTimeStamp ( ) const origin_ptr_->isRemoving() or not last_ptr_) { - WOLF_DEBUG("Processor has no time stamp. Returning a non-valid timestamp equal to 0"); + WOLF_DEBUG("PM proc \"", getName(), "\" has no time stamp. Returning a non-valid timestamp equal to 0"); return TimeStamp::Invalid(); } diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp index c6c0e34c4ae47a507a235d990810f52745a3db04..84c916b90d68f4aef67a502ec795118e26c10808 100644 --- a/src/processor/processor_odom_2d.cpp +++ b/src/processor/processor_odom_2d.cpp @@ -167,29 +167,17 @@ CaptureMotionPtr ProcessorOdom2d::emplaceCapture(const FrameBasePtr& _frame_own, return cap_motion; } -FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin) +void ProcessorOdom2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) { - auto fac_odom = FactorBase::emplace<FactorRelativePose2d>(_feature, - _feature, - _capture_origin->getFrame(), - shared_from_this(), - params_->apply_loss_function, - TOP_MOTION); - return fac_odom; -} - -FeatureBasePtr ProcessorOdom2d::emplaceFeature(CaptureMotionPtr _capture_motion) -{ - Eigen::MatrixXd covariance = _capture_motion->getBuffer().back().delta_integr_cov_; + Eigen::MatrixXd covariance = _capture_own->getBuffer().back().delta_integr_cov_; makePosDef(covariance); - FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion, - "ProcessorOdom2d", - _capture_motion->getBuffer().back().delta_integr_, - covariance); - return key_feature_ptr; -} + FeatureBasePtr feature = FeatureBase::emplace<FeatureBase>( + _capture_own, "ProcessorOdom2d", _capture_own->getBuffer().back().delta_integr_, covariance); + FactorBase::emplace<FactorRelativePose2d>( + feature, feature, _capture_origin->getFrame(), shared_from_this(), params_->apply_loss_function, TOP_MOTION); +} } /* namespace wolf */ diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp index 26a2a8d19b8c1156443638263bc92c68c9220ff4..b9a53c2a3ae875a068ad926bb5e680378cf5f726 100644 --- a/src/processor/processor_odom_3d.cpp +++ b/src/processor/processor_odom_3d.cpp @@ -58,9 +58,8 @@ void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data, if (_data.size() == (long int)6) { // rotation in vector form - _delta.head<3>() = _data.head<3>(); - Eigen::Map<Eigen::Quaterniond> q(_delta.data() + 3); - q = v2q(_data.tail<3>()); + _delta.head<3>() = _data.head<3>(); + _delta.tail<4>() = v2q(_data.tail<3>()).coeffs(); } else { @@ -217,15 +216,6 @@ CaptureMotionPtr ProcessorOdom3d::emplaceCapture(const FrameBasePtr& _frame_own, return cap_motion; } -FeatureBasePtr ProcessorOdom3d::emplaceFeature(CaptureMotionPtr _capture_motion) -{ - FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion, - "ProcessorOdom3d", - _capture_motion->getBuffer().back().delta_integr_, - _capture_motion->getBuffer().back().delta_integr_cov_); - return key_feature_ptr; -} - Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_preint, const Eigen::VectorXd& delta_step) const { @@ -234,15 +224,16 @@ Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_prei return delta_corrected; } -FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) + +void ProcessorOdom3d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) { - auto fac_odom = FactorBase::emplace<FactorRelativePose3d>(_feature_motion, - _feature_motion, - _capture_origin->getFrame(), - shared_from_this(), - params_->apply_loss_function, - TOP_MOTION); - return fac_odom; + Eigen::MatrixXd covariance = _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_; + + FeatureBasePtr feature = FeatureBase::emplace<FeatureBase>( + _capture_own, "ProcessorOdom3d", _capture_own->getBuffer().back().delta_integr_, covariance); + + FactorBase::emplace<FactorRelativePose3d>( + feature, feature, _capture_origin->getFrame(), shared_from_this(), params_->apply_loss_function, TOP_MOTION); } VectorXd ProcessorOdom3d::getCalibration (const CaptureBaseConstPtr _capture) const diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 0892276de37c33ad0390f2a01b38e14400375f29..94e30fd1f20d1526a9580f7b1a2a5a94083dfede 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -112,7 +112,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // Process info // TrackerFeature: We only process new features in Last, here last = nullptr, so we do not have anything to do. // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them. - processKnown(); + if (not getProblem()->getMap()->getLandmarkList().empty()) + processKnown(); // Issue KF callback with new KF getProblem()->keyFrameCallback(keyframe, shared_from_this()); @@ -148,14 +149,16 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_KEYFRAME" ); // Make a NON KEY Frame to hold incoming capture - FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), - getProblem()->getFrameTypes(), - getProblem()->getState()); - incoming_ptr_->link(frame); + FrameBasePtr keyframe = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), + getProblem()->getFrameTypes(), + getProblem()->getState()); + incoming_ptr_->link(keyframe); // Process info // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them. - processKnown(); + if (not getProblem()->getMap()->getLandmarkList().empty()) + processKnown(); + // Both Trackers: We have a last_ Capture with not enough features, so populate it. processNew(params_tracker_->max_new_features); diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 0db3b19e06f4c0c359009afaeaed21c29dc99e85..820f1b9968a890d028763d69f2210fee5fed3fb4 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -554,8 +554,18 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st { auto sb = getStateBlockDynamic(key); if (sb) - _stream << _tabs << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " - << std::setprecision(3) << sb->getState().transpose() << " )" << " @ " << sb << std::endl; + { + _stream << _tabs << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << ", " + << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "] = ( " << std::setprecision(3) + << sb->getState().transpose() << " )"; + if (params_prior_map_.count(key)) + { + auto ftr = params_prior_map_.at(key); + _stream << " ; mean = ( " << ftr->getMeasurement().transpose() << " )"; + _stream << " , std = ( " << ftr->getMeasurementCovariance().diagonal().array().sqrt().transpose() << " )"; + } + _stream << " @ " << sb << std::endl; + } } } else if (_metric) @@ -570,7 +580,7 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st { auto sb = getStateBlockDynamic(key); if (sb) - _stream << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb; + _stream << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << ", " << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "] @ " << sb; } _stream << std::endl; } diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp index ed4aeede54f33e274dbd6cc9c52837f22be2869c..411a057635d27c7ac164536525a34ae28caf8268 100644 --- a/test/gtest_factor_diff_drive.cpp +++ b/test/gtest_factor_diff_drive.cpp @@ -108,16 +108,16 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin); } - FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override - { - return Base::emplaceFeature(_capture_own); - } - - FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, - CaptureBasePtr _capture_origin) override - { - return Base::emplaceFactor(_feature_motion, _capture_origin); - } + // FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override + // { + // return Base::emplaceFeature(_capture_own); + // } + + // FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, + // CaptureBasePtr _capture_origin) override + // { + // return Base::emplaceFactor(_feature_motion, _capture_origin); + // } ParamsProcessorDiffDrivePtr getParams() { diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp index 47cc6069c7bcb90dc4f1ea2307cef2d95b0622ff..345818253dfeede026481d1665f0dcef680c35e2 100644 --- a/test/gtest_processor_diff_drive.cpp +++ b/test/gtest_processor_diff_drive.cpp @@ -98,16 +98,16 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin); } - FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override - { - return Base::emplaceFeature(_capture_own); - } - - FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, - CaptureBasePtr _capture_origin) override - { - return Base::emplaceFactor(_feature_motion, _capture_origin); - } + // FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override + // { + // return Base::emplaceFeature(_capture_own); + // } + + // FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, + // CaptureBasePtr _capture_origin) override + // { + // return Base::emplaceFactor(_feature_motion, _capture_origin); + // } ParamsProcessorDiffDrivePtr getParams() { diff --git a/test/gtest_yaml_conversions.cpp b/test/gtest_yaml_conversions.cpp index 73f873272b402fcf226b4ae3b2eaff746bd3072a..16fb0d7c554372b4a0a98d71f75dbe474819c695 100644 --- a/test/gtest_yaml_conversions.cpp +++ b/test/gtest_yaml_conversions.cpp @@ -24,7 +24,7 @@ #include "core/utils/utils_gtest.h" #include <yaml-schema-cpp/yaml_conversion.hpp> #include <yaml-cpp/yaml.h> -#include <eigen3/Eigen/Dense> +#include <Eigen/Dense> #include <iostream> using namespace Eigen;