diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0910fe579821a1fc1ab7e2df324c754212584123..1f1f99f4c4bfb71389afc7fe1695355ad801f3c0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -63,7 +63,6 @@ if(UNIX)
set(CMAKE_CXX_FLAGS
"${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
endif(UNIX)
-
IF(NOT BUILD_TESTS)
OPTION(BUILD_TESTS "Build Unit tests" ON)
ENDIF(NOT BUILD_TESTS)
@@ -147,8 +146,8 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
ENDIF()
# Configure config.h
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-message("WOLF CONFIG ${WOLF_CONFIG_DIR}/config.h")
-message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
+message(STATUS "WOLF CONFIG DIRECTORY ${WOLF_CONFIG_DIR}")
+message(STATUS "WOLF CONFIG FILE ${WOLF_CONFIG_DIR}/config.h")
include_directories("${PROJECT_BINARY_DIR}/conf")
# include spdlog (logging library)
@@ -217,6 +216,7 @@ SET(HDRS_STATE_BLOCK
include/core/state_block/local_parametrization_quaternion.h
include/core/state_block/state_angle.h
include/core/state_block/state_block.h
+ include/core/state_block/state_composite.h
include/core/state_block/state_homogeneous_3d.h
include/core/state_block/state_quaternion.h
)
@@ -227,7 +227,6 @@ SET(HDRS_CAPTURE
include/core/capture/capture_odom_2d.h
include/core/capture/capture_odom_3d.h
include/core/capture/capture_pose.h
- include/core/capture/capture_velocity.h
include/core/capture/capture_void.h
include/core/capture/capture_diff_drive.h
)
@@ -318,6 +317,7 @@ SET(SRCS_STATE_BLOCK
src/state_block/local_parametrization_homogeneous.cpp
src/state_block/local_parametrization_quaternion.cpp
src/state_block/state_block.cpp
+ src/state_block/state_composite.cpp
)
SET(SRCS_COMMON
src/common/node_base.cpp
@@ -338,7 +338,6 @@ SET(SRCS_CAPTURE
src/capture/capture_odom_2d.cpp
src/capture/capture_odom_3d.cpp
src/capture/capture_pose.cpp
- src/capture/capture_velocity.cpp
src/capture/capture_void.cpp
src/capture/capture_diff_drive.cpp
)
@@ -444,6 +443,18 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_WRAPPER}
${SRCS_YAML}
)
+
+# Set compiler options
+# ====================
+if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+ message(STATUS "Using C++ compiler clang")
+ target_compile_options(${PROJECT_NAME} PRIVATE -Winconsistent-missing-override)
+ # using Clang
+elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+ message(STATUS "Using C++ compiler gnu")
+ target_compile_options(${PROJECT_NAME} PRIVATE -Wsuggest-override)
+ # using GCC
+endif()
#Link the created libraries
#=============================================================
diff --git a/demos/hello_wolf/factor_bearing.h b/demos/hello_wolf/factor_bearing.h
index 6975a798db580e1c127b0215609f49904b29ae40..f5c5dd95887add0562377c360e7087a8f975665d 100644
--- a/demos/hello_wolf/factor_bearing.h
+++ b/demos/hello_wolf/factor_bearing.h
@@ -68,9 +68,8 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
// 3. Get the expected bearing of the point
T bearing = atan2(point_r(1), point_r(0));
- // 4. Get the measured range-and-bearing to the point, and its covariance
+ // 4. Get the measured range-and-bearing to the point
Matrix<T, 2, 1> range_bearing = getMeasurement();
- Matrix<T, 2, 2> range_bearing_cov = getMeasurementCovariance();
// 5. Get the bearing error by comparing against the bearing measurement
T er = range_bearing(1) - bearing;
@@ -80,8 +79,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
er -= T(2*M_PI);
// 6. Compute the residual by scaling according to the standard deviation of the bearing part
- T sigma = sqrt(range_bearing_cov(1,1));
- *_res = er / sigma;
+ *_res = er * T(getMeasurementSquareRootInformationUpper()(1,1));
return true;
}
diff --git a/demos/hello_wolf/factor_range_bearing.h b/demos/hello_wolf/factor_range_bearing.h
index 436b79e4a0eee67aa8ddd97de34ec14127251d01..226a6335e7966a0d3bdd7042c017913bc64b419b 100644
--- a/demos/hello_wolf/factor_range_bearing.h
+++ b/demos/hello_wolf/factor_range_bearing.h
@@ -21,12 +21,14 @@ class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2,
{
public:
FactorRangeBearing(const CaptureBasePtr& _capture_own, // own capture's pointer
+ const FeatureBasePtr& _feature_ptr,
const LandmarkBasePtr& _landmark_other_ptr, // other landmark's pointer
const ProcessorBasePtr& _processor_ptr, // processor having created this
bool _apply_loss_function, // apply loss function to residual?
FactorStatus _status) : // active factor?
FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2, 1, 2>( // sizes of: residual, rob pos, rob ori, sen pos, sen ori, lmk pos
"RANGE BEARING", // factor type enum (see wolf.h)
+ _feature_ptr,
nullptr, // other frame's pointer
nullptr, // other capture's pointer
nullptr, // other feature's pointer
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 1089e4ae64f96db351e5f2aee74423b26642f0ed..a0af310caa518d2a956d5bd80e587a2ec9d39314 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -139,8 +139,10 @@ int main()
// initialize
TimeStamp t(0.0); // t : 0.0
- Vector3d x(0,0,0);
- Matrix3d P = Matrix3d::Identity() * 0.1;
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P = Matrix3d::Identity() * 0.1;
+ VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
FrameBasePtr KF1 = problem->setPriorFactor(x, P, t, 0.5); // KF1 : (0,0,0)
std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1);
diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
index 9510807d443da2458299fe3522fa8c5f49cf0bf5..08e77f76c8d7d2ec70c9b1ac6689e724a67e5c7c 100644
--- a/demos/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -91,6 +91,7 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
auto prc = shared_from_this();
auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr,
capture_rb,
+ ftr,
lmk,
prc,
false,
@@ -125,14 +126,14 @@ ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2
Eigen::Vector2d ProcessorRangeBearing::fromSensor(const Eigen::Vector2d& lmk_s) const
{
- Trf H_w_r = transform(getProblem()->getCurrentState());
+ Trf H_w_r = transform(getProblem()->getState().vector("PO"));
return H_w_r * H_r_s * lmk_s;
}
Eigen::Vector2d ProcessorRangeBearing::toSensor(const Eigen::Vector2d& lmk_w) const
{
// Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState());
- Trf H_w_r = transform(getProblem()->getCurrentState());
+ Trf H_w_r = transform(getProblem()->getState().vector("PO"));
return (H_w_r * H_r_s).inverse() * lmk_w;
}
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index 90e1c1616c1d32955bc50ccf04eae73e0ef01159..f2e4f8db31f97ef8059b4d59483a747456e6d19c 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -11,8 +11,14 @@ config:
dimension: 2 # space is 2d
prior:
mode: "factor"
- state: [0,0,0]
- cov: [[3,3],.01,0,0,0,.01,0,0,0,.01]
+ # state: [0,0,0]
+ $state:
+ P: [0,0]
+ O: [0]
+ # cov: [[3,3],.01,0,0,0,.01,0,0,0,.01]
+ $sigma:
+ P: [0.31, 0.31]
+ O: [0.31]
time_tolerance: 0.1
sensors:
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index dbabbdefe4ee96de47685fcd519abe28127ed120..413aaed7d3fbac71f4b9142c3b139acc2b34cbb9 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -29,7 +29,6 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
FeatureBasePtrList feature_list_;
FactorBasePtrList constrained_by_list_;
SensorBaseWPtr sensor_ptr_; ///< Pointer to sensor
- SizeEigen calib_size_; ///< size of the calibration parameters (dynamic or static sensor params that are not fixed)
static unsigned int capture_id_count_;
@@ -83,7 +82,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
// State blocks
- const std::string& getStructure() const;
+ const StateStructure& getStructure() const;
StateBlockPtr getStateBlock(const std::string& _key) const;
StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1, _key)); }
StateBlockPtr getSensorP() const;
@@ -93,10 +92,6 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
virtual void fix() override;
virtual void unfix() override;
- bool hasCalibration() const {return calib_size_ > 0;}
- SizeEigen getCalibSize() const;
- virtual Eigen::VectorXd getCalibration() const;
- void setCalibration(const Eigen::VectorXd& _calib);
void move(FrameBasePtr);
void link(FrameBasePtr);
template<typename classType, typename... T>
@@ -119,15 +114,9 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
virtual CheckLog localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
- protected:
- virtual SizeEigen computeCalibSize() const;
-
private:
FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
void removeFeature(FeatureBasePtr _ft_ptr);
-
- private:
- void updateCalibSize();
};
}
@@ -147,16 +136,6 @@ std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... al
return cpt;
}
-inline SizeEigen CaptureBase::getCalibSize() const
-{
- return calib_size_;
-}
-
-inline void CaptureBase::updateCalibSize()
-{
- calib_size_ = computeCalibSize();
-}
-
inline StateBlockPtr CaptureBase::getSensorP() const
{
return getStateBlock("P");
diff --git a/include/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h
index 2300a65b2a203ccc6bb9ca155fab6dbb6b2bdd96..52e865da7d6d3e441643480d9a5e6acc0014aa89 100644
--- a/include/core/capture/capture_diff_drive.h
+++ b/include/core/capture/capture_diff_drive.h
@@ -38,9 +38,6 @@ public:
StateBlockPtr _intr_ptr = nullptr);
virtual ~CaptureDiffDrive() = default;
-
- virtual Eigen::VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
-
};
} // namespace wolf
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index 72ed561c556275804b89b8bb9a54c876506d1926..fca231eeb7142aa456171c95438d0b322927a88d 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -50,8 +50,6 @@ class CaptureMotion : public CaptureBase
const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr);
CaptureMotion(const std::string & _type,
@@ -59,8 +57,6 @@ class CaptureMotion : public CaptureBase
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr,
StateBlockPtr _p_ptr = nullptr,
StateBlockPtr _o_ptr = nullptr,
@@ -88,19 +84,18 @@ class CaptureMotion : public CaptureBase
MatrixXd getJacobianCalib(const TimeStamp& _ts) const;
// Get delta preintegrated, and corrected for changes on calibration params
- VectorXd getDeltaCorrected(const VectorXd& _calib_current) const;
- VectorXd getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const;
VectorXd getDeltaPreint() const;
VectorXd getDeltaPreint(const TimeStamp& _ts) const;
MatrixXd getDeltaPreintCov() const;
MatrixXd getDeltaPreintCov(const TimeStamp& _ts) const;
- virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const;
// Origin frame and capture
CaptureBasePtr getOriginCapture();
CaptureBasePtr getOriginCapture() const;
void setOriginCapture(CaptureBasePtr _capture_origin_ptr);
+ bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance);
+
virtual void printHeader(int depth, //
bool constr_by, //
bool metric, //
@@ -160,12 +155,6 @@ inline Eigen::MatrixXd CaptureMotion::getJacobianCalib(const TimeStamp& _ts) con
return getBuffer().getMotion(_ts).jacobian_calib_;
}
-inline Eigen::VectorXd CaptureMotion::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
-{
- WOLF_DEBUG("WARNING: using Cartesian sum for delta correction. \nIf your deltas lie on a manifold, derive this function and implement the proper delta correction!")
- return _delta + _delta_error;
-}
-
inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture()
{
return capture_origin_ptr_.lock();
diff --git a/include/core/capture/capture_odom_2d.h b/include/core/capture/capture_odom_2d.h
index d18217279601157c9a1b2eb3c4e7f4ba4dae6cbb..8856c363a7f576413a3428fa7b206ab6bd17f824 100644
--- a/include/core/capture/capture_odom_2d.h
+++ b/include/core/capture/capture_odom_2d.h
@@ -33,17 +33,8 @@ class CaptureOdom2d : public CaptureMotion
virtual ~CaptureOdom2d();
- virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
-
};
-inline Eigen::VectorXd CaptureOdom2d::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
-{
- Vector3d delta = _delta + _delta_error;
- delta(2) = pi2pi(delta(2));
- return delta;
-}
-
} /* namespace wolf */
#endif /* CAPTURE_ODOM_2d_H_ */
diff --git a/include/core/capture/capture_odom_3d.h b/include/core/capture/capture_odom_3d.h
index 7e4ee02c8b694cc6ae2b8e8b6d9e6803d7587c65..b4a293f8c4059b9346a334c31e017a967267a902 100644
--- a/include/core/capture/capture_odom_3d.h
+++ b/include/core/capture/capture_odom_3d.h
@@ -33,8 +33,6 @@ class CaptureOdom3d : public CaptureMotion
virtual ~CaptureOdom3d();
- virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
-
};
} /* namespace wolf */
diff --git a/include/core/capture/capture_velocity.h b/include/core/capture/capture_velocity.h
deleted file mode 100644
index d2d1c686dfd84c0d8d9ca3c4fbe0a1b7d17091e1..0000000000000000000000000000000000000000
--- a/include/core/capture/capture_velocity.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/**
- * \file capture_velocity.h
- *
- * Created on: Oct 20, 2016
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_CAPTURE_VELOCITY_H_
-#define _WOLF_CAPTURE_VELOCITY_H_
-
-//wolf includes
-#include "core/capture/capture_motion.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureVelocity);
-
-/**
- * @brief The CaptureVelocity class
- *
- * Represents a velocity.
- */
-class CaptureVelocity : public CaptureMotion
-{
-protected:
-
- using NodeBase::node_type_;
-
-public:
-
- /**
- * \brief Constructor
- **/
- CaptureVelocity(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXd& _velocity,
- SizeEigen _delta_size, SizeEigen _delta_cov_size,
- CaptureBasePtr _capture_origin_ptr);
-
- CaptureVelocity(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXd& _velocity,
- const Eigen::MatrixXd& _velocity_cov,
- SizeEigen _delta_size, SizeEigen _delta_cov_size,
- CaptureBasePtr _capture_origin_ptr,
- StateBlockPtr _p_ptr = nullptr,
- StateBlockPtr _o_ptr = nullptr,
- StateBlockPtr _intr_ptr = nullptr);
-
- virtual ~CaptureVelocity() = default;
-
- const Eigen::VectorXd& getVelocity() const;
-
- const Eigen::MatrixXd& getVelocityCov() const;
-};
-
-} // namespace wolf
-
-#endif /* _WOLF_CAPTURE_VELOCITY_H_ */
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index a3ec1b9e9018e041f0ab0aef448894858c265324..0e54191e5c546b18bf7f92e34054afa375faa696 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -288,7 +288,6 @@ inline bool file_exists(const std::string& _name)
inline const Eigen::Vector3d gravity(void) {
return Eigen::Vector3d(0,0,-9.806);
}
-
} // namespace wolf
#endif /* WOLF_H_ */
diff --git a/include/core/factor/factor_analytic.h b/include/core/factor/factor_analytic.h
index f1f9f02e617be2046471b7cecb26959bfe610143..34fd699a7b1461f9ab147c9d5d9f6ef2496b2f97 100644
--- a/include/core/factor/factor_analytic.h
+++ b/include/core/factor/factor_analytic.h
@@ -19,6 +19,7 @@ class FactorAnalytic: public FactorBase
public:
FactorAnalytic(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index 03fc5fbb407c1f2c6914f871ede6834701af2bab..dce0203c894704953ff3001732f418b2636343a3 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -64,6 +64,7 @@ class FactorAutodiff : public FactorBase
/** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
**/
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -83,7 +84,7 @@ class FactorAutodiff : public FactorBase
StateBlockPtr _state9Ptr,
StateBlockPtr _state10Ptr,
StateBlockPtr _state11Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr})
{
// asserts for null states
@@ -361,6 +362,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -379,7 +381,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
StateBlockPtr _state8Ptr,
StateBlockPtr _state9Ptr,
StateBlockPtr _state10Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr})
{
// asserts for null states
@@ -624,6 +626,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -641,7 +644,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
StateBlockPtr _state7Ptr,
StateBlockPtr _state8Ptr,
StateBlockPtr _state9Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr})
{
// asserts for null states
@@ -877,6 +880,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -893,7 +897,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
StateBlockPtr _state6Ptr,
StateBlockPtr _state7Ptr,
StateBlockPtr _state8Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr})
{
// asserts for null states
@@ -1120,6 +1124,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1135,7 +1140,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
StateBlockPtr _state5Ptr,
StateBlockPtr _state6Ptr,
StateBlockPtr _state7Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr})
{
// asserts for null states
@@ -1353,6 +1358,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1367,7 +1373,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr,
StateBlockPtr _state6Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr})
{
// asserts for null states
@@ -1576,6 +1582,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1589,7 +1596,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state3Ptr,
StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr})
{
// asserts for null states
@@ -1784,6 +1791,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1796,7 +1804,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state2Ptr,
StateBlockPtr _state3Ptr,
StateBlockPtr _state4Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr})
{
// asserts for null states
@@ -1982,6 +1990,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1993,7 +2002,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state1Ptr,
StateBlockPtr _state2Ptr,
StateBlockPtr _state3Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr})
{
// asserts for null states
@@ -2174,6 +2183,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -2184,7 +2194,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state0Ptr,
StateBlockPtr _state1Ptr,
StateBlockPtr _state2Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr})
{
// asserts for null states
@@ -2356,6 +2366,7 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -2365,7 +2376,7 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
FactorStatus _status,
StateBlockPtr _state0Ptr,
StateBlockPtr _state1Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr})
{
// asserts for null states
@@ -2528,6 +2539,7 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -2536,7 +2548,7 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
bool _apply_loss_function,
FactorStatus _status,
StateBlockPtr _state0Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr})
{
// asserts for null states
diff --git a/include/core/factor/factor_autodiff_distance_3d.h b/include/core/factor/factor_autodiff_distance_3d.h
index 1ef90ba493d3d3922685f7f42ff2b14402d7a546..320f7ac270cdd151f078c520561e509f108296d8 100644
--- a/include/core/factor/factor_autodiff_distance_3d.h
+++ b/include/core/factor/factor_autodiff_distance_3d.h
@@ -24,7 +24,8 @@ class FactorAutodiffDistance3d : public FactorAutodiff<FactorAutodiffDistance3d,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff("FactorAutodiffDistance3d",
- _frm_other,
+ _feat,
+ _frm_other,
nullptr,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index b58f4e9b337d169e013bea389fe841c382d5f8a2..9b19020f716da69341664ada97ae5c7a9a7f395b 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -54,7 +54,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list
- ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer list
+ ProcessorBaseWPtr processor_ptr_; ///< Processor pointer
+ Eigen::VectorXd measurement_; ///< the measurement vector
+ Eigen::MatrixXd measurement_sqrt_information_upper_;///< the squared root information matrix ///< ProcessorBase pointer list
virtual void setProblem(ProblemPtr) final;
public:
@@ -66,6 +68,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
* that does not located in the same branch.
**/
FactorBase(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -75,6 +78,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
FactorStatus _status = FAC_ACTIVE);
FactorBase(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
@@ -126,15 +130,11 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
**/
virtual std::vector<unsigned int> getStateSizes() const = 0;
- /** \brief Returns a reference to the feature measurement
+ /** \brief Returns a reference to the measurement
**/
virtual const Eigen::VectorXd& getMeasurement() const;
- /** \brief Returns a reference to the feature measurement covariance
- **/
- virtual const Eigen::MatrixXd& getMeasurementCovariance() const;
-
- /** \brief Returns a reference to the feature measurement square root information
+ /** \brief Returns a reference to the measurement square root information
**/
virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
@@ -256,7 +256,6 @@ std::shared_ptr<classType> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... a
return ctr;
}
-
inline unsigned int FactorBase::id() const
{
return factor_id_;
@@ -309,7 +308,6 @@ inline LandmarkBasePtr FactorBase::getLandmarkOther() const
return landmark_other_list_.front().lock();
}
-
inline ProcessorBasePtr FactorBase::getProcessor() const
{
return processor_ptr_.lock();
@@ -320,5 +318,15 @@ inline void FactorBase::setProcessor(const ProcessorBasePtr& _processor_ptr)
processor_ptr_ = _processor_ptr;
}
+inline const Eigen::VectorXd& FactorBase::getMeasurement() const
+{
+ return measurement_;
+}
+
+inline const Eigen::MatrixXd& FactorBase::getMeasurementSquareRootInformationUpper() const
+{
+ return measurement_sqrt_information_upper_;
+}
+
} // namespace wolf
#endif
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index 4ff0a96e89ee4bb9b6b5a77534e6656da24d7269..143308af2988a74d754f865c305e1ba089f4f778 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.h
@@ -33,11 +33,13 @@ class FactorBlockAbsolute : public FactorAnalytic
* \param _sb_ptr the constrained state block
*
*/
- FactorBlockAbsolute(StateBlockPtr _sb_ptr,
+ FactorBlockAbsolute(FeatureBasePtr _feature_ptr,
+ StateBlockPtr _sb_ptr,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockAbsolute",
+ _feature_ptr,
nullptr,
nullptr,
nullptr,
@@ -63,13 +65,15 @@ class FactorBlockAbsolute : public FactorAnalytic
* \param _start_idx the size of the state segment that is constrained, -1 = all the
*
*/
- FactorBlockAbsolute(StateBlockPtr _sb_ptr,
+ FactorBlockAbsolute(FeatureBasePtr _feature_ptr,
+ StateBlockPtr _sb_ptr,
unsigned int _start_idx,
int _size,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockAbsolute",
+ _feature_ptr,
nullptr,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index d3558ef21aa6e6577fc54d26015db3f9e3193312..ebaf8bc7be42c444f74588599e5b18d7135a5a88 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/include/core/factor/factor_block_difference.h
@@ -36,21 +36,22 @@ class FactorBlockDifference : public FactorAnalytic
* \param _sb_ptr the constrained state block
*
*/
- FactorBlockDifference(
- const StateBlockPtr& _sb1_ptr,
- const StateBlockPtr& _sb2_ptr,
- const FrameBasePtr& _frame_other = nullptr,
- const CaptureBasePtr& _cap_other = nullptr,
- const FeatureBasePtr& _feat_other = nullptr,
- const LandmarkBasePtr& _lmk_other = nullptr,
- 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) :
+ FactorBlockDifference(const FeatureBasePtr& _feature_ptr,
+ const StateBlockPtr& _sb1_ptr,
+ const StateBlockPtr& _sb2_ptr,
+ const FrameBasePtr& _frame_other = nullptr,
+ const CaptureBasePtr& _cap_other = nullptr,
+ const FeatureBasePtr& _feat_other = nullptr,
+ const LandmarkBasePtr& _lmk_other = nullptr,
+ 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("FactorBlockDifference",
+ _feature_ptr,
_frame_other,
_cap_other,
_feat_other,
@@ -129,8 +130,8 @@ inline Eigen::VectorXd FactorBlockDifference::evaluateResiduals(const std::vecto
{
// 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() == getMeasurementSquareRootInformationUpper().cols());
+ assert(getMeasurement().size() == getMeasurementSquareRootInformationUpper().rows());
assert(getMeasurement().size() == sb1_constrained_size_);
assert(getMeasurement().size() == sb2_constrained_size_);
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 7b189e4424b419b655833020ff9a94c71952748e..e267fb507ebfd811a0c17b1b6446267223510198 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -32,20 +32,20 @@ namespace wolf
WOLF_PTR_TYPEDEFS(FactorDiffDrive);
class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
- RESIDUAL_SIZE,
- POSITION_SIZE,
- ORIENTATION_SIZE,
- POSITION_SIZE,
- ORIENTATION_SIZE,
- INTRINSICS_SIZE>
+ RESIDUAL_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ INTRINSICS_SIZE>
{
using Base = FactorAutodiff<FactorDiffDrive, // @suppress("Type cannot be resolved")
- RESIDUAL_SIZE,
- POSITION_SIZE,
- ORIENTATION_SIZE,
- POSITION_SIZE,
- ORIENTATION_SIZE,
- INTRINSICS_SIZE>;
+ RESIDUAL_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ INTRINSICS_SIZE>;
public:
@@ -55,6 +55,7 @@ class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
const bool _apply_loss_function,
const FactorStatus _status = FAC_ACTIVE) :
Base("FactorDiffDrive",
+ _ftr_ptr,
_capture_origin_ptr->getFrame(),
_capture_origin_ptr,
nullptr,
diff --git a/include/core/factor/factor_odom_2d.h b/include/core/factor/factor_odom_2d.h
index 1ce2a375c131cea5234a5e603cdf2808764bc4cc..d8cfa08a9a9bf70c8b580f407f0983f60e791566 100644
--- a/include/core/factor/factor_odom_2d.h
+++ b/include/core/factor/factor_odom_2d.h
@@ -17,18 +17,19 @@ class FactorOdom2d : public FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>
{
public:
FactorOdom2d(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>("FactorOdom2d",
- _frame_other_ptr, nullptr, nullptr, nullptr,
- _processor_ptr,
- _apply_loss_function, _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO())
+ _ftr_ptr,
+ _frame_other_ptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function, _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO())
{
//
}
diff --git a/include/core/factor/factor_odom_2d_closeloop.h b/include/core/factor/factor_odom_2d_closeloop.h
index 85a1785440a03a15a899117b619d3d07aaa3eb39..a4ea7b1dff8484a89885edd424a6ac899331d3d7 100644
--- a/include/core/factor/factor_odom_2d_closeloop.h
+++ b/include/core/factor/factor_odom_2d_closeloop.h
@@ -21,13 +21,14 @@ class FactorOdom2dCloseloop : public FactorAutodiff<FactorOdom2dCloseloop, 3, 2,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorOdom2dCloseloop, 3, 2, 1, 2, 1>("FactorOdom2dCloseloop",
- _frame_other_ptr, nullptr, nullptr, nullptr,
- _processor_ptr,
- _apply_loss_function, _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO())
+ _ftr_ptr,
+ _frame_other_ptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function, _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO())
{
//
}
diff --git a/include/core/factor/factor_odom_3d.h b/include/core/factor/factor_odom_3d.h
index 2e49daf216906b4e96d92e9c195cbff579fd6545..4e32efe4067d52c23bfe978cea7ef792214e00c9 100644
--- a/include/core/factor/factor_odom_3d.h
+++ b/include/core/factor/factor_odom_3d.h
@@ -21,10 +21,10 @@ class FactorOdom3d : public FactorAutodiff<FactorOdom3d,6,3,4,3,4>
{
public:
FactorOdom3d(const FeatureBasePtr& _ftr_current_ptr,
- const FrameBasePtr& _frame_past_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
virtual ~FactorOdom3d() = default;
@@ -81,18 +81,19 @@ inline FactorOdom3d::FactorOdom3d(const FeatureBasePtr& _ftr_current_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff<FactorOdom3d, 6, 3, 4, 3, 4>("FactorOdom3d", // type
- _frame_past_ptr, // frame other
- nullptr, // capture other
- nullptr, // feature other
- nullptr, // landmark other
- _processor_ptr, // processor
- _apply_loss_function,
- _status,
- _ftr_current_ptr->getFrame()->getP(), // current frame P
- _ftr_current_ptr->getFrame()->getO(), // current frame Q
- _frame_past_ptr->getP(), // past frame P
- _frame_past_ptr->getO()) // past frame Q
+ FactorAutodiff<FactorOdom3d, 6, 3, 4, 3, 4>("FactorOdom3d", // type
+ _ftr_current_ptr, // feature
+ _frame_past_ptr, // frame other
+ nullptr, // capture other
+ nullptr, // feature other
+ nullptr, // landmark other
+ _processor_ptr, // processor
+ _apply_loss_function,
+ _status,
+ _ftr_current_ptr->getFrame()->getP(), // current frame P
+ _ftr_current_ptr->getFrame()->getO(), // current frame Q
+ _frame_past_ptr->getP(), // past frame P
+ _frame_past_ptr->getO()) // past frame Q
{
// WOLF_TRACE("Constr ODOM 3d (f", _ftr_current_ptr->id(),
// " F", _ftr_current_ptr->getCapture()->getFrame()->id(),
@@ -105,8 +106,12 @@ inline FactorOdom3d::FactorOdom3d(const FeatureBasePtr& _ftr_current_ptr,
}
template<typename T>
-inline bool FactorOdom3d::expectation(const T* const _p_current, const T* const _q_current, const T* const _p_past,
- const T* const _q_past, T* _expectation_dp, T* _expectation_dq) const
+inline bool FactorOdom3d::expectation(const T* const _p_current,
+ const T* const _q_current,
+ const T* const _p_past,
+ const T* const _q_past,
+ T* _expectation_dp,
+ T* _expectation_dq) const
{
Eigen::Map<const Eigen::Matrix<T,3,1> > p_current(_p_current);
Eigen::Map<const Eigen::Quaternion<T> > q_current(_q_current);
@@ -168,8 +173,11 @@ inline Eigen::VectorXd FactorOdom3d::expectation() const
}
template<typename T>
-inline bool FactorOdom3d::operator ()(const T* const _p_current, const T* const _q_current, const T* const _p_past,
- const T* const _q_past, T* _residuals) const
+inline bool FactorOdom3d::operator ()(const T* const _p_current,
+ const T* const _q_current,
+ const T* const _p_past,
+ const T* const _q_past,
+ T* _residuals) const
{
/* Residual expression
diff --git a/include/core/factor/factor_pose_2d.h b/include/core/factor/factor_pose_2d.h
index f11b674e7775c78c0f0f306be69c85c832e328de..a054c11673b75e69fe413afa9c5a161ac42d0778 100644
--- a/include/core/factor/factor_pose_2d.h
+++ b/include/core/factor/factor_pose_2d.h
@@ -22,6 +22,7 @@ class FactorPose2d: public FactorAutodiff<FactorPose2d,3,2,1>
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPose2d, 3, 2, 1>("FactorPose2d",
+ _ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
@@ -58,7 +59,7 @@ inline bool FactorPose2d::operator ()(const T* const _p, const T* const _o, T* _
// residual
Eigen::Map<Eigen::Matrix<T,3,1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper() * er;
+ res = getMeasurementSquareRootInformationUpper() * er;
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
diff --git a/include/core/factor/factor_pose_3d.h b/include/core/factor/factor_pose_3d.h
index 9a46cbab24101a69faaad78ea190f1823406dd35..0390ab6401f782ed30921e9090bc2e5ed796589a 100644
--- a/include/core/factor/factor_pose_3d.h
+++ b/include/core/factor/factor_pose_3d.h
@@ -21,6 +21,7 @@ class FactorPose3d: public FactorAutodiff<FactorPose3d,6,3,4>
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPose3d,6,3,4>("FactorPose3d",
+ _ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
@@ -61,7 +62,7 @@ inline bool FactorPose3d::operator ()(const T* const _p, const T* const _o, T* _
// residual
Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper() * er;
+ res = getMeasurementSquareRootInformationUpper() * er;
return true;
}
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index aca1f7f966aa33e3f970e630b984b7fe145781ed..8cb37c39234d4ae47c05453179f0354380c824e9 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -23,11 +23,13 @@ class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3
{
public:
- FactorQuaternionAbsolute(StateBlockPtr _sb_ptr,
+ FactorQuaternionAbsolute(FeatureBasePtr _ftr_ptr,
+ StateBlockPtr _sb_ptr,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorQuaternionAbsolute,3,4>("FactorQuaternionAbsolute",
+ _ftr_ptr,
nullptr,
nullptr,
nullptr,
@@ -78,7 +80,7 @@ inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residua
// residual
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper() * er;
+ res = getMeasurementSquareRootInformationUpper() * er;
return true;
}
diff --git a/include/core/factor/factor_relative_2d_analytic.h b/include/core/factor/factor_relative_2d_analytic.h
index 2dba90bc6d4ead00634f601bf232d4471860c418..e4ccf958c00e0e6185e0cf97a20c8072517c2756 100644
--- a/include/core/factor/factor_relative_2d_analytic.h
+++ b/include/core/factor/factor_relative_2d_analytic.h
@@ -19,12 +19,24 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_FRAME
**/
FactorRelative2dAnalytic(const std::string& _tp,
- const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp, _frame_other_ptr, nullptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_other_ptr->getP(), _frame_other_ptr->getO(), _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
+ const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic(_tp,
+ _ftr_ptr,
+ _frame_other_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO())
{
//
}
@@ -32,12 +44,24 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_FEATURE
**/
FactorRelative2dAnalytic(const std::string& _tp,
- const FeatureBasePtr& _ftr_ptr,
- const FeatureBasePtr& _ftr_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp, nullptr, nullptr, _ftr_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _ftr_other_ptr->getFrame()->getP(), _ftr_other_ptr->getFrame()->getO() )
+ const FeatureBasePtr& _ftr_ptr,
+ const FeatureBasePtr& _ftr_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic(_tp,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ _ftr_other_ptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_other_ptr->getFrame()->getP(),
+ _ftr_other_ptr->getFrame()->getO() )
{
//
}
@@ -45,12 +69,24 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_LANDMARK
**/
FactorRelative2dAnalytic(const std::string& _tp,
- const FeatureBasePtr& _ftr_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp, nullptr, nullptr, nullptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _landmark_other_ptr->getP(), _landmark_other_ptr->getO())
+ const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic(_tp,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _landmark_other_ptr->getP(),
+ _landmark_other_ptr->getO())
{
//
}
diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
index be13135c9c7e3e0899ac2bb9043f4dd1b7b60ef2..704cc3e7bc906a84a6702b6e8fdcad38766562ee 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -22,6 +22,7 @@ class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativeP
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
+ _ftr_ptr,
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
@@ -46,13 +47,23 @@ class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativeP
}
template<typename T>
- bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, const T* const _sp, const T* const _so,
+ bool operator ()(const T* const _p1,
+ const T* const _o1,
+ const T* const _p2,
+ const T* const _o2,
+ const T* const _sp,
+ const T* const _so,
T* _residuals) const;
};
template<typename T>
-inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
- const T* const _o2, const T* const _ps, const T* const _os, T* _residuals) const
+inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1,
+ const T* const _o1,
+ const T* const _p2,
+ const T* const _o2,
+ const T* const _ps,
+ const T* const _os,
+ T* _residuals) const
{
// MAPS
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index da1f68b8400cb94f87d1cc5ee460fbf8440b7bee..f412fd3688e114e6116cd17db7de724e0fb7fa85 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -76,9 +76,17 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
*/
double getMeasurement(unsigned int _ii) const;
const Eigen::VectorXd& getMeasurement() const;
+
+ private:
+ /* \brief Set the measurement and its noise
+ *
+ * WARNING: To be used once in the constructor only.
+ */
void setMeasurement(const Eigen::VectorXd& _meas);
void setMeasurementCovariance(const Eigen::MatrixXd & _meas_cov);
void setMeasurementInformation(const Eigen::MatrixXd & _meas_info);
+
+ public:
const Eigen::MatrixXd& getMeasurementCovariance() const;
Eigen::MatrixXd getMeasurementInformation() const;
const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
@@ -191,11 +199,6 @@ inline const Eigen::MatrixXd& FeatureBase::getMeasurementSquareRootInformationUp
return measurement_sqrt_information_upper_;
}
-inline void FeatureBase::setMeasurement(const Eigen::VectorXd& _meas)
-{
- measurement_ = _meas;
-}
-
inline const Eigen::VectorXd& FeatureBase::getExpectation() const
{
return expectation_;
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 5ed1abdcfbd9192f4a7f44424c0e66f01842df1c..37fc3a0ea2ea96144c6ab11d016250197a20dd0c 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -57,9 +57,27 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
* \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
* \param _v_ptr StateBlock pointer to the velocity (default: nullptr).
**/
- FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
-
- FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x);
+ FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr = nullptr,
+ StateBlockPtr _v_ptr = nullptr);
+
+ FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ const std::string _frame_structure,
+ const SizeEigen _dim,
+ const Eigen::VectorXd& _x);
+
+ FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ const std::string _frame_structure,
+ const VectorComposite& _state);
+
+ FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ const std::string _frame_structure,
+ const std::list<VectorXd>& _vectors);
//Auxiliary constructor, needed to put frametype in front
FrameBase(const FrameType & _tp, const std::string _frame_structure, const SizeEigen _dim, const TimeStamp& _ts, const Eigen::VectorXd& _x);
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 00f570faed8c0542a1e0312a5ad1bc7c2ab710e3..4ac32d2ded2923f212937cfa88a0994fb0dca5b4 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -10,6 +10,7 @@
#include "core/common/wolf.h"
#include "core/math/rotations.h"
+#include "core/state_block/state_composite.h"
#include <Eigen/Geometry>
#include <Eigen/Dense>
@@ -25,17 +26,26 @@ namespace wolf
namespace SO2{
template<typename T>
-Eigen::Matrix<T, 2, 2> exp(const T theta)
+inline Eigen::Matrix<T, 2, 2> exp(const T theta)
{
return Eigen::Rotation2D<T>(theta).matrix();
}
+
} // namespace SO2
namespace SE2{
+/** \brief Compute [1]_x * t = (-t.y ; t.x)
+ */
+template<class D>
+inline Eigen::Matrix<typename D::Scalar, 2, 1> skewed(const MatrixBase<D>& t)
+{
+ return Eigen::Matrix<typename D::Scalar, 2, 1>(-t(1), t(0));
+}
+
template<typename T>
-Eigen::Matrix2d V_helper(const T theta)
+inline Eigen::Matrix2d V_helper(const T theta)
{
T s; // sin(theta) / theta
T c_1; // (1-cos(theta)) / theta
@@ -56,8 +66,17 @@ Eigen::Matrix2d V_helper(const T theta)
return (Matrix<T, 2, 2>() << s, -c_1, c_1, s).finished();
}
+inline VectorComposite identity()
+{
+ VectorComposite v;
+ v["P"] = Vector2d::Zero();
+ v["O"] = Vector1d::Zero();
+
+ return v;
+}
+
template<class D1, class D2>
-void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
+inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
{
MatrixSizeCheck<3, 1>::check(_tau);
MatrixSizeCheck<3, 1>::check(_delta);
@@ -68,7 +87,7 @@ void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
}
template<class D, typename T>
-Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T theta)
+inline Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T theta)
{
MatrixSizeCheck<2, 1>::check (p);
@@ -95,7 +114,7 @@ Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T theta)
}
template<class D1, class D2, class D3>
-void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_delta_tau)
+inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_delta_tau)
{
MatrixSizeCheck<3, 1>::check(_tau);
MatrixSizeCheck<3, 1>::check(_delta);
@@ -116,6 +135,254 @@ void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_
_J_delta_tau << V, J_Vp_theta(_tau.template head<2>(), theta), 0.0, 0.0, 1.0;
}
+inline void exp(const VectorComposite& _tau, VectorComposite& _delta)
+{
+ // [1] eq. 156
+ const auto &p = _tau.at("P");
+ const auto &theta = pi2pi(_tau.at("O")(0));
+ Matrix2d V = V_helper(theta);
+
+ _delta["P"] = V * p;
+ _delta["O"] = Matrix1d(theta);
+}
+
+inline VectorComposite exp(const VectorComposite& tau)
+{
+ VectorComposite res("PO", {2,1});
+
+ exp(tau, res);
+
+ return res;
+}
+
+inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComposite& _J_delta_tau)
+{
+ // [1] eq. 156
+ const auto &p = _tau.at("P");
+ const auto &theta = pi2pi(_tau.at("O")(0));
+ Matrix2d V = V_helper(theta);
+
+ _delta["P"] = V * p;
+ _delta["O"] = Matrix1d(theta);
+
+ // Jacobian follows the composite definition in [1] eq. 89, with jacobian blocks:
+ // J_Vp_p = V: see V_helper, eq. 158
+ // J_Vp_theta: see fcn helper
+ // J_theta_theta = 1; eq. 126
+ _J_delta_tau.clear();
+ _J_delta_tau.emplace("P", "P", V);
+ _J_delta_tau.emplace("P", "O", J_Vp_theta(p, theta));
+ _J_delta_tau.emplace("O", "P", RowVector2d(0.0, 0.0));
+ _J_delta_tau.emplace("O", "O", Matrix1d(1));
+}
+
+template<class D1, class D2, class D3>
+inline void compose(const MatrixBase<D1> &_delta1, MatrixBase<D2> &_delta2, MatrixBase<D3> &_delta1_compose_delta2)
+{
+ MatrixSizeCheck<3, 1>::check(_delta1);
+ MatrixSizeCheck<3, 1>::check(_delta2);
+ MatrixSizeCheck<3, 1>::check(_delta1_compose_delta2);
+
+ _delta1_compose_delta2.template head<2>() = _delta1.template head<2>()
+ + SO2::exp(_delta1(2)) * _delta2.template head<2>();
+ _delta1_compose_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+}
+
+template<class D1, class D2, class D3, class D4, class D5>
+inline void compose(const MatrixBase<D1>& _delta1,
+ const MatrixBase<D2>& _delta2,
+ MatrixBase<D3>& _delta1_compose_delta2,
+ MatrixBase<D4>& _J_compose_delta1,
+ MatrixBase<D5>& _J_compose_delta2)
+{
+ MatrixSizeCheck<3, 1>::check(_delta1);
+ MatrixSizeCheck<3, 1>::check(_delta2);
+ MatrixSizeCheck<3, 1>::check(_delta1_compose_delta2);
+ MatrixSizeCheck<3, 3>::check(_J_compose_delta1);
+ MatrixSizeCheck<3, 3>::check(_J_compose_delta2);
+
+ Matrix2d R1 = SO2::exp(_delta1(2)); // need temporary
+
+ // tc = t1 + R1 t2
+ _delta1_compose_delta2.template head<2>() = _delta1.template head<2>() + R1 * _delta2.template head<2>();
+
+ // Rc = R1 R2 --> theta = theta1 + theta2
+ _delta1_compose_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+
+ /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
+ *
+ * resulting delta is:
+ *
+ * tc = t1 + R1 t2 (*)
+ * Rc = R1 R2 (**)
+ *
+ * Jacobians have the form:
+ *
+ * [ J_t_t J_t_R ]
+ * [ J_r_t J_R_R ]
+ *
+ * Jacobian blocks are:
+ *
+ * J_tc_t1 = I trivial from (*)
+ * J_tc_R1 = R1 [1]x t2 = R1 (-t2.y ; t2.x) see [1]: eq. (129)
+ *
+ * J_Rc_t1 = (0 0) trivial from (**)
+ * J_Rc_R1 = 1 see [1]: eq. (125)
+ *
+ * J_tc_t2 = R1 see [1]: eq. (130)
+ * J_tc_R2 = 0 trivial from (*)
+ *
+ * J_Rc_t2 = 0 trivial from (**)
+ * J_Rc_R2 = 1 see [1]: eq. (125)
+ */
+
+ _J_compose_delta1.setIdentity();
+ _J_compose_delta1.template block<2, 1>(0, 2) = R1 * skewed(_delta2.template head<2>());
+
+ _J_compose_delta2.setIdentity(3, 3);
+ _J_compose_delta2.template block<2, 2>(0, 0) = R1;
+}
+
+inline void compose(const VectorComposite& _x1,
+ const VectorComposite& _x2,
+ VectorComposite& _c)
+{
+ const auto& p1 = _x1.at("P");
+ const auto& a1 = _x1.at("O")(0); // angle
+ const auto& R1 = SO2::exp(a1);
+
+ const auto& p2 = _x2.at("P");
+ const auto& a2 = _x2.at("O")(0); // angle
+
+ // tc = t1 + R1 t2
+ _c["P"] = p1 + R1 * p2;
+
+ // Rc = R1 R2 --> theta = theta1 + theta2
+ _c["O"] = Matrix1d( pi2pi(a1 + a2) ) ;
+}
+
+inline void compose(const VectorComposite& _x1,
+ const VectorComposite& _x2,
+ VectorComposite& _c,
+ MatrixComposite& _J_c_x1,
+ MatrixComposite& _J_c_x2)
+{
+ const auto& p1 = _x1.at("P");
+ const auto& a1 = _x1.at("O")(0); // angle
+ Matrix2d R1 = SO2::exp(a1); // need temporary
+
+ const auto& p2 = _x2.at("P");
+ const auto& a2 = _x2.at("O")(0); // angle
+
+ /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
+ *
+ * resulting delta is:
+ *
+ * tc = t1 + R1 t2 (*)
+ * Rc = R1 R2 (**)
+ *
+ * Jacobians have the form:
+ *
+ * [ J_t_t J_t_R ]
+ * [ J_r_t J_R_R ]
+ *
+ * Jacobian blocks are:
+ *
+ * J_tc_t1 = I trivial from (*)
+ * J_tc_R1 = R1 [1]x t2 = R1 (-t2.y ; t2.x) see [1]: eq. (129)
+ *
+ * J_Rc_t1 = (0 0) trivial from (**)
+ * J_Rc_R1 = 1 see [1]: eq. (125)
+ *
+ * J_tc_t2 = R1 see [1]: eq. (130)
+ * J_tc_R2 = 0 trivial from (*)
+ *
+ * J_Rc_t2 = 0 trivial from (**)
+ * J_Rc_R2 = 1 see [1]: eq. (125)
+ */
+
+ _J_c_x1.clear();
+ _J_c_x1.emplace("P", "P", Matrix2d::Identity());
+ _J_c_x1.emplace("P", "O", MatrixXd(R1 * skewed(p2)) );
+ _J_c_x1.emplace("O", "P", RowVector2d(0,0));
+ _J_c_x1.emplace("O", "O", Matrix1d(1));
+
+ _J_c_x2.clear();
+ _J_c_x2.emplace("P", "P", R1);
+ _J_c_x2.emplace("P", "O", Vector2d(0,0));
+ _J_c_x2.emplace("O", "P", RowVector2d(0,0));
+ _J_c_x2.emplace("O", "O", Matrix1d(1));
+
+
+ // Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1' or 'x2'.
+
+ // tc = t1 + R1 t2
+ _c["P"] = p1 + R1 * p2;
+
+ // Rc = R1 R2 --> theta = theta1 + theta2
+ _c["O"] = Matrix1d( pi2pi(a1 + a2) ) ;
+
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
+ const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
+ MatrixBase<D7>& plus_p, MatrixBase<D8>& plus_q)
+{
+ MatrixSizeCheck<2, 1>::check(p1);
+ MatrixSizeCheck<1, 1>::check(q1);
+ MatrixSizeCheck<2, 1>::check(p2);
+ MatrixSizeCheck<1, 1>::check(o2);
+ MatrixSizeCheck<2, 1>::check(plus_p);
+ MatrixSizeCheck<1, 1>::check(plus_q);
+
+ plus_p = p1 + p2;
+ plus_q(0) = pi2pi(q1(0) + o2(0));
+}
+
+template<typename D1, typename D2, typename D3>
+inline void plus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& d_plus)
+{
+ Map<const Matrix<typename D1::Scalar, 2, 1> > p1 ( & d1(0) );
+ Map<const Matrix<typename D1::Scalar, 1, 1> > q1 ( & d1(2) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > p2 ( & d2(0) );
+ Map<const Matrix<typename D2::Scalar, 1, 1> > o2 ( & d2(2) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > p_p ( & d_plus(0) );
+ Map<Matrix<typename D1::Scalar, 1, 1> > q_p ( & d_plus(2) );
+
+ plus(p1, q1, p2, o2, p_p, q_p);
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 3, 1> plus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2)
+{
+ Matrix<typename D1::Scalar, 3, 1> d_plus;
+ plus(d1, d2, d_plus);
+ return d_plus;
+}
+
+inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
+{
+ plus(x.at("P"), x.at("O"), tau.at("P"), tau.at("O"), res.at("P"), res.at("O"));
+}
+
+inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
+{
+ VectorComposite res;
+
+ res["P"] = Vector2d();
+ res["O"] = Vector1d();
+
+ plus(x, tau, res);
+
+ return res;
+}
+
+
+
} // namespace SE2
} // namespacs wolf
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index 701c5bab7a5ec5ecc6401def52ad9ca6d0f26856..4b000889c59320e8c7e819725c415dd19308d077 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.h
@@ -10,6 +10,7 @@
#include "core/common/wolf.h"
#include "core/math/rotations.h"
+#include "core/state_block/state_composite.h"
/*
* The functions in this file are related to manipulations of Delta motion magnitudes used in 3d motion.
@@ -26,11 +27,17 @@
* - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], so that D (+) I = I (+) D = D
* - inverse: so that D (+) D.inv = D.inv (+) D = I
* - between: Db = D2 (-) D1 := D1.inv (+) D2, so that D2 = D1 (+) Db
- * - log_SE3: go from Delta manifold to tangent space (equivalent to log() in rotations)
- * - exp_SE3: go from tangent space to delta manifold (equivalent to exp() in rotations)
- * - plus: D2 = D1 * exp_SE3(d)
- * - minus: d = log_SE3( D1.inv() * D2 )
+ * - log: go from Delta manifold to tangent space (equivalent to log() in rotations)
+ * - exp: go from tangent space to delta manifold (equivalent to exp() in rotations)
+ * - plus: D2 = D1 * exp(d)
+ * - minus: d = log( D1.inv() * D2 )
* - interpolate: dd = D1 * exp ( log( D1.inv() * D2 ) * t ) = D1 (+) ( (D2 (-) D1) * t)
+ *
+ * ------------------------------------------------------------------------------------------
+ *
+ * Some of the functions below are based on:
+ *
+ * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018, https://arxiv.org/pdf/1812.01537.pdf
*/
namespace wolf
@@ -57,6 +64,13 @@ inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
q << T2(0), T2(0), T2(0), T2(1);
}
+inline void identity(VectorComposite& v)
+{
+ v.clear();
+ v["P"] = Vector3d::Zero();
+ v["O"] = Quaterniond::Identity().coeffs();
+}
+
template<typename T = double>
inline Matrix<T, 7, 1> identity()
{
@@ -67,14 +81,14 @@ inline Matrix<T, 7, 1> identity()
}
template<typename D1, typename D2, typename D4, typename D5>
-inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq,
- MatrixBase<D4>& idp, QuaternionBase<D5>& idq)
+inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q,
+ MatrixBase<D4>& ip, QuaternionBase<D5>& iq)
{
- MatrixSizeCheck<3, 1>::check(dp);
- MatrixSizeCheck<3, 1>::check(idp);
+ MatrixSizeCheck<3, 1>::check(p);
+ MatrixSizeCheck<3, 1>::check(ip);
- idp = - dq.conjugate() * dp ;
- idq = dq.conjugate() ;
+ ip = - q.conjugate() * p ;
+ iq = q.conjugate() ;
}
template<typename D1, typename D2>
@@ -84,12 +98,12 @@ inline void inverse(const MatrixBase<D1>& d,
MatrixSizeCheck<7, 1>::check(d);
MatrixSizeCheck<7, 1>::check(id);
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > dq ( & d( 3 ) );
- Map<Matrix<typename D2::Scalar, 3, 1> > idp ( & id( 0 ) );
- Map<Quaternion<typename D2::Scalar> > idq ( & id( 3 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p ( & d( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > q ( & d( 3 ) );
+ Map<Matrix<typename D2::Scalar, 3, 1> > ip ( & id( 0 ) );
+ Map<Quaternion<typename D2::Scalar> > iq ( & id( 3 ) );
- inverse(dp, dq, idp, idq);
+ inverse(p, q, ip, iq);
}
template<typename D>
@@ -101,16 +115,35 @@ inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q )
+inline void compose(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
+ MatrixBase<D7>& pc, QuaternionBase<D8>& qc )
{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(sum_p);
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<3, 1>::check(pc);
- sum_p = dp1 + dq1*dp2;
- sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
+ pc = p1 + q1 * p2;
+ qc = q1 * q2; // q here to avoid possible aliasing between d1 and sum
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void compose(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
+ const MatrixBase<D4>& p2, const MatrixBase<D5>& q2,
+ MatrixBase<D7>& pc, MatrixBase<D8>& qc )
+{
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<4, 1>::check(q1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<4, 1>::check(q2);
+ MatrixSizeCheck<3, 1>::check(pc);
+ MatrixSizeCheck<4, 1>::check(qc);
+
+ Map<const Quaternion<typename D2::Scalar> > mq1 ( & q1( 0 ) );
+ Map<const Quaternion<typename D5::Scalar> > mq2 ( & q2( 0 ) );
+ Map< Quaternion<typename D8::Scalar> > mqc ( & qc( 0 ) );
+
+ compose(p1, mq1, p2, mq2, pc, mqc);
}
template<typename D1, typename D2, typename D3>
@@ -122,14 +155,14 @@ inline void compose(const MatrixBase<D1>& d1,
MatrixSizeCheck<7, 1>::check(d2);
MatrixSizeCheck<7, 1>::check(sum);
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1( 3 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2( 0 ) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2( 3 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( & sum( 0 ) );
- Map<Quaternion<typename D3::Scalar> > sum_q ( & sum( 3 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1( 3 ) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2( 0 ) );
+ Map<const Quaternion<typename D2::Scalar> > q2 ( & d2( 3 ) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > pc ( & sum( 0 ) );
+ Map<Quaternion<typename D3::Scalar> > qc ( & sum( 3 ) );
- compose(dp1, dq1, dp2, dq2, sum_p, sum_q);
+ compose(p1, q1, p2, q2, pc, qc);
}
template<typename D1, typename D2>
@@ -155,8 +188,8 @@ inline void compose(const MatrixBase<D1>& d1,
MatrixSizeCheck< 6, 6>::check(J_sum_d2);
// Some useful temporaries
- Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //dq1.matrix(); // First Delta, DR
- Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //dq2.matrix(); // Second delta, dR
+ Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //q1.matrix(); // First Delta, DR
+ Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //q2.matrix(); // Second delta, dR
// Jac wrt first delta
J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
@@ -165,43 +198,75 @@ inline void compose(const MatrixBase<D1>& d1,
// Jac wrt second delta
J_sum_d2.setIdentity(); //
- J_sum_d2.block(0,0,3,3) = dR1; // dDp'/ddp
+ J_sum_d2.block(0,0,3,3) = dR1; // dDp'/p
// J_sum_d2.block(3,3,3,3) = Matrix3d::Identity(); // dDo'/ddo = I
// compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
compose(d1, d2, sum);
}
+inline void compose(const VectorComposite& x1, const VectorComposite& x2, VectorComposite& c)
+{
+ compose(x1.at("P"), x1.at("O"), x2.at("P"), x2.at("O"), c["P"], c["O"]);
+}
+
+inline void compose(const VectorComposite& x1,
+ const VectorComposite& x2,
+ VectorComposite& c,
+ MatrixComposite& J_c_x1,
+ MatrixComposite& J_c_x2)
+{
+
+ // Some useful temporaries
+ const auto R1 = q2R(x1.at("O")); //q1.matrix(); // make temporary
+ const auto& R2 = q2R(x2.at("O")); //q2.matrix(); // do not make temporary, only reference
+
+ // Jac wrt first delta
+ J_c_x1.emplace("P", "P", Matrix3d::Identity() ) ;
+ J_c_x1.emplace("P", "O", - R1 * skew(x2.at("P")) ) ;
+ J_c_x1.emplace("O", "P", Matrix3d::Zero() ) ;
+ J_c_x1.emplace("O", "O", R2.transpose() ) ;
+
+ // Jac wrt second delta
+ J_c_x2.emplace("P", "P", R1 );
+ J_c_x2.emplace("P", "O", Matrix3d::Zero() );
+ J_c_x2.emplace("O", "P", Matrix3d::Zero() );
+ J_c_x2.emplace("O", "O", Matrix3d::Identity() );
+
+ // compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the same variable
+ compose(x1, x2, c);
+}
+
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D7>& dp12, QuaternionBase<D8>& dq12)
+inline void between(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
+ MatrixBase<D7>& p12, QuaternionBase<D8>& q12)
{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(dp12);
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<3, 1>::check(p12);
- dp12 = dq1.conjugate() * ( dp2 - dp1 );
- dq12 = dq1.conjugate() * dq2;
+ p12 = q1.conjugate() * ( p2 - p1 );
+ q12 = q1.conjugate() * q2;
}
template<typename D1, typename D2, typename D3>
inline void between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
- MatrixBase<D3>& d2_minus_d1)
+ MatrixBase<D3>& d12)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
- MatrixSizeCheck<7, 1>::check(d2_minus_d1);
+ MatrixSizeCheck<7, 1>::check(d12);
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dp12 ( & d2_minus_d1(0) );
- Map<Quaternion<typename D3::Scalar> > dq12 ( & d2_minus_d1(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > p12 ( & d12(0) );
+ Map<Quaternion<typename D3::Scalar> > q12 ( & d12(3) );
- between(dp1, dq1, dp2, dq2, dp12, dq12);
+ between(p1, q1, p2, q2, p12, q12);
}
template<typename D1, typename D2>
@@ -216,28 +281,28 @@ inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
}
template<typename Derived>
-Matrix<typename Derived::Scalar, 6, 1> log_SE3(const MatrixBase<Derived>& delta_in)
+inline Matrix<typename Derived::Scalar, 6, 1> log(const MatrixBase<Derived>& delta_in)
{
MatrixSizeCheck<7, 1>::check(delta_in);
Matrix<typename Derived::Scalar, 6, 1> ret;
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & delta_in(0) );
- Map<const Quaternion<typename Derived::Scalar> > dq_in ( & delta_in(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > p_in ( & delta_in(0) );
+ Map<const Quaternion<typename Derived::Scalar> > q_in ( & delta_in(3) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > p_ret ( & ret(0) );
Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
Matrix<typename Derived::Scalar, 3, 3> V_inv;
- do_ret = log_q(dq_in);
+ do_ret = log_q(q_in);
V_inv = jac_SO3_left_inv(do_ret);
- dp_ret = V_inv * dp_in;
+ p_ret = V_inv * p_in;
return ret;
}
template<typename Derived>
-Matrix<typename Derived::Scalar, 7, 1> exp_SE3(const MatrixBase<Derived>& d_in)
+inline Matrix<typename Derived::Scalar, 7, 1> exp(const MatrixBase<Derived>& d_in)
{
MatrixSizeCheck<6, 1>::check(d_in);
@@ -247,27 +312,58 @@ Matrix<typename Derived::Scalar, 7, 1> exp_SE3(const MatrixBase<Derived>& d_in)
V = jac_SO3_left(d_in.template tail<3>());
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & d_in(0) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > do_in ( & d_in(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) );
- Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > p_in ( & d_in(0) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > o_in ( & d_in(3) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > p ( & ret(0) );
+ Map<Quaternion<typename Derived::Scalar> > q ( & ret(3) );
- dp = V * dp_in;
- dq = exp_q(do_in);
+ p = V * p_in;
+ q = exp_q(o_in);
return ret;
}
+inline VectorComposite exp(const VectorComposite& tau)
+{
+ const auto& p = tau.at("P");
+ const auto& theta = tau.at("O");
+
+ Matrix3d V = jac_SO3_left(theta); // see [1] eqs. (174) and (145)
+
+ VectorComposite res;
+
+ res["P"] = V * p ;
+ res["O"] = exp_q(theta).coeffs() ;
+
+ return res;
+}
+
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2,
+inline void plus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q)
{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<3, 1>::check(o2);
MatrixSizeCheck<3, 1>::check(plus_p);
- plus_p = dp1 + dp2;
- plus_q = dq1 * exp_q(do2);
+
+ plus_p = p1 + p2;
+ plus_q = q1 * exp_q(o2);
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
+ const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
+ MatrixBase<D7>& plus_p, MatrixBase<D8>& plus_q)
+{
+ MatrixSizeCheck<4, 1>::check(q1);
+ MatrixSizeCheck<4, 1>::check(plus_q);
+
+ Map<const Quaterniond> q1m ( & q1 (0) );
+ Map<Quaterniond> plus_qm ( & plus_q(0) );
+
+ plus(p1, q1m, p2, o2, plus_p, plus_qm);
}
template<typename D1, typename D2, typename D3>
@@ -275,14 +371,14 @@ inline void plus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& d_plus)
{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_plus(0) );
- Map<Quaternion<typename D3::Scalar> > dq_p ( & d_plus(3) );
-
- plus(dp1, dq1, dp2, do2, dp_p, dq_p);
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > o2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > p_p ( & d_plus(0) );
+ Map<Quaternion<typename D3::Scalar> > q_p ( & d_plus(3) );
+
+ plus(p1, q1, p2, o2, p_p, q_p);
}
template<typename D1, typename D2>
@@ -294,25 +390,42 @@ inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
return d_plus;
}
+inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
+{
+ plus(x.at("P"), x.at("O"), tau.at("P"), tau.at("O"), res.at("P"), res.at("O"));
+}
+
+inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
+{
+ VectorComposite res;
+
+ res["P"] = Vector3d();
+ res["O"] = Vector4d();
+
+ plus(x, tau, res);
+
+ return res;
+}
+
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o )
+inline void minus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
+ MatrixBase<D7>& pd, MatrixBase<D8>& od )
{
- diff_p = dp2 - dp1;
- diff_o = log_q(dq1.conjugate() * dq2);
+ pd = p2 - p1;
+ od = log_q(q1.conjugate() * q2);
}
template<typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
-inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D6>& diff_p, MatrixBase<D7>& diff_o,
- MatrixBase<D8>& J_do_dq1, MatrixBase<D9>& J_do_dq2)
+inline void minus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
+ MatrixBase<D6>& pd, MatrixBase<D7>& od,
+ MatrixBase<D8>& J_do_q1, MatrixBase<D9>& J_do_q2)
{
- minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
+ minus(p1, q1, p2, q2, pd, od);
- J_do_dq1 = - jac_SO3_left_inv(diff_o);
- J_do_dq2 = jac_SO3_right_inv(diff_o);
+ J_do_q1 = - jac_SO3_left_inv(od);
+ J_do_q2 = jac_SO3_right_inv(od);
}
template<typename D1, typename D2, typename D3>
@@ -320,14 +433,14 @@ inline void minus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& err)
{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & err(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & err(3) );
-
- minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > pd ( & err(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > od ( & err(3) );
+
+ minus(p1, q1, p2, q2, pd, od);
}
template<typename D1, typename D2, typename D3, typename D4, typename D5>
@@ -337,35 +450,30 @@ inline void minus(const MatrixBase<D1>& d1,
MatrixBase<D4>& J_diff_d1,
MatrixBase<D5>& J_diff_d2)
{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & dif(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & dif(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & dif(6) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > pd ( & dif(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > od ( & dif(3) );
- Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
+ Matrix<typename D4::Scalar, 3, 3> J_do_q1, J_do_q2;
- minus(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
+ minus(p1, q1, p2, q2, pd, od, J_do_q1, J_do_q2);
/* d = minus(d1, d2) is
- * dp = dp2 - dp1
- * do = Log(dq1.conj * dq2)
- * dv = dv2 - dv1
+ * pd = p2 - p1
+ * od = Log(q1.conj * q2)
*
- * With trivial Jacobians for dp and dv, and:
- * J_do_dq1 = - J_l_inv(theta)
- * J_do_dq2 = J_r_inv(theta)
+ * With trivial Jacobians for p and dv, and:
+ * J_do_q1 = - J_l_inv(theta)
+ * J_do_q2 = J_r_inv(theta)
*/
-
J_diff_d1 = - Matrix<typename D4::Scalar, 6, 6>::Identity();// d(p2 - p1) / d(p1) = - Identity
- J_diff_d1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
+ J_diff_d1.block(3,3,3,3) = J_do_q1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
J_diff_d2.setIdentity(6,6); // d(R1.tr*R2) / d(R2) = Identity
- J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
+ J_diff_d2.block(3,3,3,3) = J_do_q2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
}
template<typename D1, typename D2>
@@ -385,9 +493,9 @@ inline void interpolate(const MatrixBase<D1>& d1,
{
Matrix<typename D1::Scalar, 7, 1> dd = between(d1, d2);
- Matrix<typename D1::Scalar, 6, 1> tau = t * log_SE3(dd);
+ Matrix<typename D1::Scalar, 6, 1> tau = t * log(dd);
- ret = compose(d1, exp_SE3(tau));
+ ret = compose(d1, exp(tau));
}
template<typename D1, typename D2>
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index ee71997cd37847bef9f1f7a1483418b7e596beb8..863a43571478656496f9035bcefca96c29756cfc 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -22,6 +22,7 @@ struct ParamsProcessorBase;
#include "core/sensor/factory_sensor.h"
#include "core/processor/factory_processor.h"
#include "core/processor/is_motion.h"
+#include "core/state_block/state_composite.h"
// std includes
#include <mutex>
@@ -37,8 +38,8 @@ enum Notification
struct PriorOptions
{
std::string mode = "";
- Eigen::VectorXd state;
- Eigen::MatrixXd cov;
+ VectorComposite state;
+ MatrixComposite cov;
double time_tolerance;
};
WOLF_STRUCT_PTR_TYPEDEFS(PriorOptions);
@@ -64,7 +65,7 @@ class Problem : public std::enable_shared_from_this<Problem>
std::map<StateBlockPtr, Notification> state_block_notification_map_;
mutable std::mutex mut_notifications_;
mutable std::mutex mut_covariances_;
- std::string frame_structure_;
+ StateStructure frame_structure_;
PriorOptionsPtr prior_options_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
@@ -77,16 +78,27 @@ class Problem : public std::enable_shared_from_this<Problem>
static ProblemPtr autoSetup(ParamsServer &_server);
virtual ~Problem();
+
+
// Properties -----------------------------------------
- SizeEigen getFrameStructureSize() const;
- void getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const;
+ public:
SizeEigen getDim() const;
- std::string getFrameStructure() const;
+ StateStructure getFrameStructure() const;
+
+ protected:
+ void appendToStructure(const StateStructure& _structure);
+
+
+
// Tree manager --------------------------------------
+ public:
void setTreeManager(TreeManagerBasePtr _gm);
TreeManagerBasePtr getTreeManager() const;
+
+
+
// Hardware branch ------------------------------------
HardwareBasePtr getHardware() const;
@@ -169,76 +181,127 @@ class Problem : public std::enable_shared_from_this<Problem>
IsMotionPtr getProcessorIsMotion();
std::list<IsMotionPtr> getProcessorIsMotionList();
+
+
+
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectory() const;
// Prior
bool isPriorSet() const;
void setPriorOptions(const std::string& _mode,
- const double _time_tolerance = 0,
- const Eigen::VectorXd& _state = Eigen::VectorXd(0),
- const Eigen::MatrixXd& _cov = Eigen::MatrixXd(0,0));
+ const double _time_tolerance = 0,
+ const VectorComposite& _state = VectorComposite(),
+ const VectorComposite& _cov = VectorComposite());
FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
- FrameBasePtr setPriorFactor(const Eigen::VectorXd &_state,
- const Eigen::MatrixXd &_cov,
+ FrameBasePtr setPriorFactor(const VectorComposite &_state,
+ const VectorComposite &_sigma,
const TimeStamp &_ts,
const double &_time_tol);
- FrameBasePtr setPriorFix(const Eigen::VectorXd &_state,
+ FrameBasePtr setPriorFix(const VectorComposite &_state,
const TimeStamp &_ts,
const double &_time_tol);
- FrameBasePtr setPriorInitialGuess(const Eigen::VectorXd &_state,
+ FrameBasePtr setPriorInitialGuess(const VectorComposite &_state,
const TimeStamp &_ts,
const double &_time_tol);
- /** \brief Emplace frame from string frame_structure
+ /** \brief Emplace frame from string frame_structure, dimension and vector
+ * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+ * \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
* \param _dim variable indicating the dimension of the problem
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _frame_state State vector; must match the size and format of the chosen frame structure
- * \param _time_stamp Time stamp of the frame
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceKeyFrame(const std::string& _frame_structure, //
+ FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
const SizeEigen _dim, //
- const Eigen::VectorXd& _frame_state, //
- const TimeStamp& _time_stamp);
+ const Eigen::VectorXd& _frame_state);
- /** \brief Emplace frame from string frame_structure without state
+ /** \brief Emplace frame from string frame_structure and state
+ * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+ * \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
- * \param _dim variable indicating the dimension of the problem
+ * \param _frame_state State vector; must match the size and format of the chosen frame structure
+ *
+ * - The dimension is taken from Problem
+ *
+ * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
+ * - Create a Frame
+ * - Add it to Trajectory
+ * - If it is key-frame, update state-block lists in Problem
+ */
+ FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const VectorComposite& _frame_state);
+
+ /** \brief Emplace frame from state
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
+ * \param _frame_state State; must be part of the problem's frame structure
+ *
+ * - The structure is taken from Problem
+ * - The dimension is taken from Problem
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceKeyFrame(const std::string& _frame_structure, //
- const SizeEigen _dim, //
- const TimeStamp& _time_stamp);
+ FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const VectorComposite& _frame_state);
- /** \brief Emplace frame from string frame_structure without structure
+ /** \brief Emplace frame from string frame_structure and dimension
+ * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+ * \param _time_stamp Time stamp of the frame
+ * \param _frame_structure String indicating the frame structure.
+ * \param _dim variable indicating the dimension of the problem
+ *
+ * - The dimension is taken from Problem
+ * - The state is taken from Problem
+ *
+ * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
+ * - Create a Frame
+ * - Add it to Trajectory
+ * - If it is key-frame, update state-block lists in Problem
+ */
+ FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim);
+
+ /** \brief Emplace frame from state vector
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
- * \param _frame_state State vector; must match the size and format of the chosen frame structure
* \param _time_stamp Time stamp of the frame
+ * \param _frame_state State vector; must match the size and format of the chosen frame structure
+ *
+ * - The structure is taken from Problem
+ * - The dimension is taken from Problem
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceKeyFrame(const Eigen::VectorXd& _frame_state, //
- const TimeStamp& _time_stamp);
+ FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const Eigen::VectorXd& _frame_state);
- /** \brief Emplace frame from string frame_structure without structure nor state
+ /** \brief Emplace frame, guess all values
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
*
+ * - The structure is taken from Problem
+ * - The dimension is taken from Problem
+ * - The state is taken from Problem
+ *
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
@@ -285,15 +348,15 @@ class Problem : public std::enable_shared_from_this<Problem>
const double& _time_tolerance);
// State getters
- Eigen::VectorXd getCurrentState ( ) const;
- void getCurrentState (Eigen::VectorXd& _state) const;
- void getCurrentStateAndStamp (Eigen::VectorXd& _state, TimeStamp& _ts) const;
- Eigen::VectorXd getState (const TimeStamp& _ts) const;
- void getState (const TimeStamp& _ts, Eigen::VectorXd& _state) const;
+ TimeStamp getTimeStamp ( ) const;
+ VectorComposite getState ( const StateStructure& _structure = "" ) const;
+ VectorComposite getState ( const TimeStamp& _ts, const StateStructure& _structure = "" ) const;
+
// Zero state provider
- Eigen::VectorXd zeroState ( ) const;
- // Perturb state
- void perturb(double amplitude = 0.01);
+ VectorComposite stateZero ( const StateStructure& _structure = "" ) const;
+
+
+
// Map branch -----------------------------------------
MapBasePtr getMap() const;
@@ -301,7 +364,13 @@ class Problem : public std::enable_shared_from_this<Problem>
void saveMap(const std::string& _filename_dot_yaml, //
const std::string& _map_name = "Map automatically saved by Wolf");
- // Covariances -------------------------------------------
+
+
+ // All branches -------------------------------------------
+ // perturb states
+ void perturb(double amplitude = 0.01);
+
+ // Covariances
void clearCovariance();
void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov);
void addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov);
@@ -313,6 +382,8 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& _covariance) const;
bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const;
+
+
// Solver management ----------------------------------------
/** \brief Notifies a new/removed state block to update the solver manager
@@ -367,10 +438,6 @@ class Problem : public std::enable_shared_from_this<Problem>
bool metric = true, //
bool state_blocks = false,
std::ostream& stream = std::cout) const;
- void print(const std::string& depth, //
- bool constr_by = false, //
- bool metric = true, //
- bool state_blocks = false) const;
bool check(int verbose_level = 0) const;
bool check(bool verbose, std::ostream& stream) const;
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index 3ba3f97ad15249f425e357b7901a1494a5e56073..2f725e18d723463e810441c83f7fc81acc206785 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -9,6 +9,7 @@
#define PROCESSOR_IS_MOTION_H_
#include "core/common/wolf.h"
+#include "core/state_block/state_composite.h"
namespace wolf
{
@@ -24,24 +25,9 @@ class IsMotion
virtual ~IsMotion();
// Queries to the processor:
-
- /** \brief Fill a reference to the state integrated so far
- * \param _x the returned state vector
- */
- virtual void getCurrentState(Eigen::VectorXd& _x) const = 0;
- virtual void getCurrentTimeStamp(TimeStamp& _ts) const = 0;
- /** \brief Fill the state corresponding to the provided time-stamp
- * \param _ts the time stamp
- * \param _x the returned state
- * \return if state in the provided time-stamp could be resolved
- */
- virtual bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const = 0;
-
-
- // Overloaded getters
- Eigen::VectorXd getCurrentState() const;
- TimeStamp getCurrentTimeStamp() const;
- Eigen::VectorXd getState(const TimeStamp& _ts) const;
+ virtual TimeStamp getTimeStamp() const = 0;
+ virtual VectorComposite getState() const = 0;
+ virtual VectorComposite getState(const TimeStamp& _ts) const = 0;
std::string getStateStructure(){return state_structure_;};
void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;};
@@ -63,27 +49,6 @@ inline IsMotion::~IsMotion()
{
}
-inline Eigen::VectorXd IsMotion::getCurrentState() const
-{
- Eigen::VectorXd x;
- getCurrentState(x);
- return x;
-}
-
-inline TimeStamp IsMotion::getCurrentTimeStamp() const
-{
- TimeStamp ts;
- getCurrentTimeStamp(ts);
- return ts;
-}
-
-inline Eigen::VectorXd IsMotion::getState(const TimeStamp& _ts) const
-{
- Eigen::VectorXd x;
- getState(_ts, x);
- return x;
-}
-
} /* namespace wolf */
#endif /* PROCESSOR_IS_MOTION_H_ */
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index bcbf3f6034250a3ffd0ac3e00b73501643d1b69b..e3e504f36b50d95c69bf13d0a99c0107f5207b27 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -60,7 +60,8 @@ class ProcessorDiffDrive : public ProcessorOdom2d
virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
-
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_;
@@ -68,6 +69,17 @@ class ProcessorDiffDrive : public ProcessorOdom2d
};
+inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBasePtr _capture) const
+{
+ return _capture->getStateBlock("I")->getState();
+}
+
+inline void ProcessorDiffDrive::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+ _capture->getStateBlock("I")->setState(_calibration);
+}
+
+
}
diff --git a/include/core/processor/processor_loopclosure.h b/include/core/processor/processor_loopclosure.h
index cbd8fa378738ad55d1072d63b7a6162a169077fa..4b22ba3985cf04a1c5c1f7658419ea9ed384f390 100644
--- a/include/core/processor/processor_loopclosure.h
+++ b/include/core/processor/processor_loopclosure.h
@@ -47,7 +47,6 @@ class ProcessorLoopClosure : public ProcessorBase
protected:
ParamsProcessorLoopClosurePtr params_loop_closure_;
- int _dim;
public:
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index d55c279fc3fd9ce5d083529b41d3448191f1f395..8769e1cc070e6fd1b12d153d4d88771cc3e9f4cb 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -165,23 +165,9 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
virtual void resetDerived();
// Queries to the processor:
-
- /** \brief Fill a reference to the state integrated so far
- * \param _x the returned state vector
- */
- virtual void getCurrentState(Eigen::VectorXd& _x) const override;
- virtual void getCurrentTimeStamp(TimeStamp& _ts) const override { _ts = getBuffer().back().ts_; }
- using IsMotion::getCurrentState;
- using IsMotion::getCurrentTimeStamp;
-
-
- /** \brief Fill the state corresponding to the provided time-stamp
- * \param _ts the time stamp
- * \param _x the returned state
- * \return if state in the provided time-stamp could be resolved
- */
- virtual bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const override;
- using IsMotion::getState;
+ virtual TimeStamp getTimeStamp() const override;
+ virtual VectorComposite getState() const override;
+ virtual VectorComposite getState(const TimeStamp& _ts) const override;
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
@@ -212,7 +198,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
* \param _x_origin the state at the origin
* \param _ts_origin origin timestamp.
*/
- FrameBasePtr setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin);
+ FrameBasePtr setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin);
// Helper functions:
MotionBuffer& getBuffer();
@@ -395,10 +381,10 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*
* This function implements the composition (+) so that _x2 = _x1 (+) _delta.
*/
- virtual void statePlusDelta(const Eigen::VectorXd& _x,
+ virtual void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _dt,
- Eigen::VectorXd& _x_plus_delta) const = 0;
+ VectorComposite& _x_plus_delta) const = 0;
/** \brief Delta zero
* \return a delta state equivalent to the null motion.
@@ -411,6 +397,25 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
virtual Eigen::VectorXd deltaZero() const = 0;
+ /** \brief correct the preintegrated delta
+ * This produces a delta correction according to the appropriate manifold.
+ * @param delta_preint : the preintegrated delta to correct
+ * @param delta_step : an increment in the tangent space of the manifold
+ *
+ * We want to do
+ *
+ * delta_corr = delta_preint (+) d_step
+ *
+ * where the operator (+) is the right-plus operator on the delta's manifold.
+ *
+ * Note: usually in motion pre-integration we have
+ *
+ * delta_step = Jac_delta_calib * (calib - calib_pre)
+ *
+ */
+ virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const = 0;
+
/** \brief emplace a CaptureMotion
* \param _ts time stamp
* \param _sensor Sensor that produced the Capture
@@ -439,6 +444,9 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*/
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const = 0;
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
+
Motion motionZero(const TimeStamp& _ts) const;
bool hasCalibration() const {return calib_size_ > 0;}
@@ -513,20 +521,6 @@ inline bool ProcessorMotion::voteForKeyFrame() const
return false;
}
-inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
-{
- // ensure integrity
- assert(origin_ptr_ && "Trying to access origin_ptr_ but it is nullptr!");
-
- // ensure proper size of the provided reference
- Eigen::VectorXd curr_x = origin_ptr_->getFrame()->getState(state_structure_);
- _x.resize( curr_x.size() );
-
- // do get timestamp and state corrected by possible self-calibrations
- double Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
- statePlusDelta(curr_x, last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
-}
-
inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const
{
return getBuffer().back().delta_integr_cov_;
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index 567c739200d7effaece6fe95cc6f17ce36f50c0c..0b873564bb7339e9acf63f1739076b9baa4ec29a 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -13,6 +13,7 @@
#include "core/factor/factor_odom_2d.h"
#include "core/math/rotations.h"
#include "core/utils/params_server.h"
+#include "core/math/SE2.h"
namespace wolf {
@@ -67,11 +68,13 @@ class ProcessorOdom2d : public ProcessorMotion
Eigen::VectorXd& _delta1_plus_delta2,
Eigen::MatrixXd& _jacobian1,
Eigen::MatrixXd& _jacobian2) const override;
- virtual void statePlusDelta(const Eigen::VectorXd& _x,
+ virtual void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _Dt,
- Eigen::VectorXd& _x_plus_delta) const override;
+ VectorComposite& _x_plus_delta) const override;
virtual Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
@@ -84,6 +87,8 @@ class ProcessorOdom2d : public ProcessorMotion
virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin) override;
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
ParamsProcessorOdom2dPtr params_odom_2d_;
@@ -95,6 +100,23 @@ inline Eigen::VectorXd ProcessorOdom2d::deltaZero() const
return Eigen::VectorXd::Zero(delta_size_);
}
+inline Eigen::VectorXd ProcessorOdom2d::correctDelta (const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
+{
+ VectorXd delta_corrected(3);
+ SE2::plus(delta_preint, delta_step, delta_corrected);
+ return delta_corrected;
+}
+
+inline VectorXd ProcessorOdom2d::getCalibration (const CaptureBasePtr _capture) const
+{
+ return VectorXd::Zero(0);
+}
+
+inline void ProcessorOdom2d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+}
+
} // namespace wolf
#endif /* SRC_PROCESSOR_ODOM_2d_H_ */
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index ca0a9619d06dbd285bb9bf8da88943286d87e0bb..49eaf4fad2788dde0499021ec89fb58d228cb65b 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -86,11 +86,14 @@ class ProcessorOdom3d : public ProcessorMotion
Eigen::VectorXd& _delta1_plus_delta2,
Eigen::MatrixXd& _jacobian1,
Eigen::MatrixXd& _jacobian2) const override;
- void statePlusDelta(const Eigen::VectorXd& _x,
- const Eigen::VectorXd& _delta,
- const double _Dt,
- Eigen::VectorXd& _x_plus_delta) 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;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
@@ -105,6 +108,8 @@ class ProcessorOdom3d : public ProcessorMotion
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
ParamsProcessorOdom3dPtr params_odom_3d_;
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index 09072ab7fc397412c21e57b6cbf7060c111455be..6dea1c98d36d6b444f6f7f28bc46a1d1f2332649 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -108,14 +108,17 @@ class ProcessorTracker : public ProcessorBase
FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
FeatureBasePtrList known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last
FeatureBasePtrList known_features_incoming_; ///< list of the known features in \b last successfully tracked in \b incoming
- int _dim;
+ StateStructure state_structure_; ///< Structure of frames created or used by this processor
public:
ProcessorTracker(const std::string& _type,
+ const StateStructure& _structure,
int _dim,
ParamsProcessorTrackerPtr _params_tracker);
virtual ~ProcessorTracker();
+ StateStructure getStateStructure() const;
+
bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts);
bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
@@ -304,6 +307,11 @@ inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const
return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
}
+inline StateStructure ProcessorTracker::getStateStructure ( ) const
+{
+ return state_structure_;
+}
+
inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
{
new_features_incoming_.push_back(_feature_ptr);
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index 4cb706c5c3de26da86f3653ef6234251bafb3f7c..b69599ef55fce9f4624b6fe9115722aeb86aa3ca 100644
--- a/include/core/processor/processor_tracker_feature.h
+++ b/include/core/processor/processor_tracker_feature.h
@@ -85,6 +85,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
/** \brief Constructor with type
*/
ProcessorTrackerFeature(const std::string& _type,
+ const StateStructure& _structure,
int _dim,
ParamsProcessorTrackerFeaturePtr _params_tracker_feature);
virtual ~ProcessorTrackerFeature();
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index b4e7f7a41ae8bbe68d53da08288b85cda87edeab..6181810bde23395d70c8e28cabc2457f4b13f41b 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -81,6 +81,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker
{
public:
ProcessorTrackerLandmark(const std::string& _type,
+ const StateStructure& _structure,
ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark);
virtual ~ProcessorTrackerLandmark();
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 07c6d757bc09125c4a4e8a55aefd6d37b480c7d4..9462efe7a6dd314ff2ab94cbf923ee79f5c4e21e 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -9,6 +9,7 @@
#define STATE_BLOCK_HAS_STATE_BLOCKS_H_
#include "core/common/wolf.h"
+#include "core/state_block/state_composite.h"
#include <map>
@@ -17,6 +18,7 @@ namespace wolf
/// State of nodes containing several state blocks
typedef std::unordered_map<std::string, Eigen::VectorXd> State;
+//typedef std::string StateStructure;
class HasStateBlocks
@@ -26,7 +28,7 @@ class HasStateBlocks
HasStateBlocks(const std::string& _structure) : structure_(_structure), state_block_map_() {}
virtual ~HasStateBlocks();
- const std::string& getStructure() const { return structure_; }
+ const StateStructure& getStructure() const { return structure_; }
void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; }
bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
@@ -62,27 +64,29 @@ class HasStateBlocks
// Emplace base state blocks.
template<typename ... Args>
- inline StateBlockPtr emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
+ StateBlockPtr emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
// Register/remove state blocks to/from wolf::Problem
void registerNewStateBlocks(ProblemPtr _problem) const;
void removeStateBlocks(ProblemPtr _problem);
// States
- inline void setState(const Eigen::VectorXd& _state, std::string _sub_structure="", const bool _notify = true);
- void getState(Eigen::VectorXd& _state, std::string structure="") const;
- Eigen::VectorXd getState(std::string structure="") const;
- unsigned int getSize(std::string _sub_structure="") const;
- unsigned int getLocalSize(std::string _sub_structure="") const;
+ VectorComposite getState(const StateStructure& structure="") const;
+ void setState(const VectorComposite& _state, const bool _notify = true);
+ void setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const std::list<SizeEigen>& _sizes, const bool _notify = true);
+ void setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure="", const bool _notify = true);
+ void setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify = true);
- State getStateComposite();
+ VectorXd getStateVector(const StateStructure& structure="") const;
+ unsigned int getSize(const StateStructure& _sub_structure="") const;
+ unsigned int getLocalSize(const StateStructure& _sub_structure="") const;
// Perturb state
void perturb(double amplitude = 0.01);
private:
- std::string structure_;
+ StateStructure structure_;
std::unordered_map<std::string, StateBlockPtr> state_block_map_;
};
@@ -207,74 +211,125 @@ inline bool HasStateBlocks::isFixed() const
return fixed;
}
-inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string _sub_structure, const bool _notify)
+inline void HasStateBlocks::setState(const VectorComposite& _state, const bool _notify)
{
- if (_sub_structure == ""){
- _sub_structure = structure_;
+ for (const auto& pair_key_vec : _state)
+ {
+ const auto& key = pair_key_vec.first;
+ const auto& vec = pair_key_vec.second;
+ const auto& sb = getStateBlock(key);
+ if (!sb)
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+
+ sb->setState(vec, _notify);
}
- int size = getSize(_sub_structure);
+}
+
+inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure, const bool _notify)
+{
+ StateStructure structure;
+ if (_sub_structure == "")
+ structure = structure_;
+ else
+ structure = _sub_structure;
+
+ int size = getSize(structure);
assert(_state.size() == size && "In FrameBase::setState wrong state size");
unsigned int index = 0;
- for (const char key : _sub_structure)
+ for (const char ckey : structure)
{
- const auto& sb = getStateBlock(key);
- if (!sb){
- WOLF_ERROR("Stateblock key ", key, " not in the structure");
- }
+ const auto& key = string(1,ckey);
+ const auto& sb = getStateBlock(key);
+ assert (sb && "Stateblock key not in the structure");
+
sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver
index += sb->getSize();
}
}
-// _sub_structure can be either stateblock structure of the node or a subset of this structure
-inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_structure) const
+inline void HasStateBlocks::setState (const Eigen::VectorXd& _state,
+ const StateStructure& _structure,
+ const std::list<SizeEigen>& _sizes,
+ const bool _notify)
{
- if (_sub_structure == ""){
- _sub_structure = structure_;
+ SizeEigen index = 0;
+ auto size_it = _sizes.begin();
+ for (const auto& ckey : _structure)
+ {
+ const auto& key = string(1,ckey);
+ const auto& sb = getStateBlock(key);
+ assert (sb && "Stateblock key not in the structure");
+ assert(*size_it == sb->getSize() && "State block size mismatch");
+
+ sb->setState(_state.segment(index, *size_it), _notify);
+ index += *size_it;
+ size_it ++;
}
- _state.resize(getSize(_sub_structure));
+}
- unsigned int index = 0;
- for (const char key : _sub_structure)
+
+inline void HasStateBlocks::setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify)
+{
+ auto vec_it = _vectors.begin();
+ for (const auto ckey : _structure)
{
+ const auto key = string(1,ckey);
const auto& sb = getStateBlock(key);
- if (!sb){
- WOLF_ERROR("Stateblock key ", key, " not in the structure");
- }
- _state.segment(index,sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
+ assert (sb && "Stateblock key not in the structure");
+ assert(vec_it->size() == sb->getSize() && "State block size mismatch");
+ sb->setState(*vec_it, _notify);
+ vec_it ++;
+ }
}
-inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) const
+
+//// _sub_structure can be either stateblock structure of the node or a subset of this structure
+inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _sub_structure) const
{
- Eigen::VectorXd state;
+ StateStructure structure;
+ if (_sub_structure == "")
+ structure = structure_;
+ else
+ structure = _sub_structure;
- getState(state, _sub_structure);
+ VectorXd state(getSize(structure));
+ unsigned int index = 0;
+ for (const char key : structure)
+ {
+ const auto& sb = getStateBlock(key);
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
+ state.segment(index,sb->getSize()) = sb->getState();
+ index += sb->getSize();
+ }
return state;
}
-inline State HasStateBlocks::getStateComposite()
+inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure) const
{
- State state;
- for (auto& pair_key_kf : state_block_map_)
+ VectorComposite state;
+ for (auto& pair_key_sb : state_block_map_)
{
- state.emplace(pair_key_kf.first, pair_key_kf.second->getState());
+ state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
}
return state;
}
-inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const
+inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure) const
{
- if (_sub_structure == ""){
- _sub_structure = structure_;
- }
+ StateStructure structure;
+ if (_sub_structure == "")
+ structure = structure_;
+ else
+ structure = _sub_structure;
+
unsigned int size = 0;
- for (const char key : _sub_structure)
+ for (const char key : structure)
{
const auto& sb = getStateBlock(key);
if (!sb){
@@ -325,16 +380,16 @@ inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string&
}
}
-//inline unsigned int HasStateBlocks::getLocalSize() const
-//=======
-inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) const
-//>>>>>>> devel
+inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _sub_structure) const
{
- if (_sub_structure == ""){
- _sub_structure = structure_;
- }
+ StateStructure structure;
+ if (_sub_structure == "")
+ structure = structure_;
+ else
+ structure = _sub_structure;
+
unsigned int size = 0;
- for (const char key : _sub_structure)
+ for (const char key : structure)
{
const auto& sb = getStateBlock(key);
if (!sb){
diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h
index 32599d9dc72a556c06f9ade7df6339f38cc7c8cf..14f2ce38f1dd55b6571884710eb7ed7b7640f985 100644
--- a/include/core/state_block/local_parametrization_base.h
+++ b/include/core/state_block/local_parametrization_base.h
@@ -12,8 +12,6 @@
namespace wolf {
-//WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
-
class LocalParametrizationBase{
protected:
unsigned int global_size_;
@@ -22,13 +20,17 @@ class LocalParametrizationBase{
LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size);
virtual ~LocalParametrizationBase();
+ bool plus(const Eigen::VectorXd& _x,
+ const Eigen::VectorXd& _delta,
+ Eigen::VectorXd& _x_plus_delta) const;
virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x,
Eigen::Map<const Eigen::VectorXd>& _delta,
- Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0;
+ Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0;
+ virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x,
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0;
virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
Eigen::Map<const Eigen::VectorXd>& _x2,
- Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
+ Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
unsigned int getLocalSize() const;
unsigned int getGlobalSize() const;
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 5d3aa1fc2e9694211d388592c8aa2605cee00824..c8706e2b6d081802d4a3b2590538b2e45df0584b 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -146,22 +146,21 @@ public:
**/
void resetLocalParamUpdated();
+ virtual void setIdentity(bool _notify = true);
+ void setZero (bool _notify = true);
+
+ virtual Eigen::VectorXd identity() const;
+ Eigen::VectorXd zero() const;
+
/** \brief perturb state
*/
void perturb(double amplitude = 0.1);
- /** \brief Add this state_block to the problem
- **/
- //void addToProblem(ProblemPtr _problem_ptr);
- /** \brief Remove this state_block from the problem
- **/
- //void removeFromProblem(ProblemPtr _problem_ptr);
+ void plus(const Eigen::VectorXd& _dv);
- /** \brief Factory creator
- *
- */
static StateBlockPtr create (const Eigen::VectorXd& _state, bool _fixed = false);
+
};
} // namespace wolf
@@ -297,6 +296,25 @@ inline double* StateBlock::getStateData()
return state_.data();
}
+inline void StateBlock::setIdentity(bool _notify)
+{
+ setState( Eigen::VectorXd::Zero(state_size_), _notify );
+}
+
+inline void StateBlock::setZero(bool _notify)
+{
+ setIdentity(_notify);
+}
+
+inline Eigen::VectorXd StateBlock::identity() const
+{
+ return Eigen::VectorXd::Zero(state_size_);
+}
+inline Eigen::VectorXd StateBlock::zero() const
+{
+ return identity();
+}
+
inline StateBlockPtr StateBlock::create (const Eigen::VectorXd& _state, bool _fixed)
{
return std::make_shared<StateBlock>(_state, _fixed);
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
new file mode 100644
index 0000000000000000000000000000000000000000..44fdc98d588d46cbdf996ff94ec788f38bf7ce9c
--- /dev/null
+++ b/include/core/state_block/state_composite.h
@@ -0,0 +1,370 @@
+/*
+ * state_composite.h
+ *
+ * Created on: Apr 6, 2020
+ * Author: jsola
+ */
+
+#ifndef STATE_BLOCK_STATE_COMPOSITE_H_
+#define STATE_BLOCK_STATE_COMPOSITE_H_
+
+#include "core/common/wolf.h"
+
+#include <unordered_map>
+
+#include <iostream>
+
+
+namespace wolf
+{
+
+using std::string;
+using namespace Eigen;
+
+typedef std::string StateStructure;
+typedef std::unordered_map < std::string, StateBlockPtr > StateBlockMap;
+typedef StateBlockMap::const_iterator StateBlockMapCIter;
+
+class VectorComposite : public std::unordered_map < std::string, Eigen::VectorXd >
+{
+ public:
+ VectorComposite() {};
+ VectorComposite(const StateStructure& _s);
+ VectorComposite(const StateStructure& _s, const std::list<int>& _sizes);
+ VectorComposite(const VectorComposite & v) : unordered_map<string, VectorXd>(v){};
+ /**
+ * \brief Construct from Eigen::VectorXd and structure
+ *
+ * Usage:
+ *
+ * VectorXd vec_eigen(1,2,3,4,5);
+ *
+ * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
+ VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors);
+
+ unsigned int size(const StateStructure& _structure) const;
+
+ Eigen::VectorXd vector(const StateStructure& _structure) const;
+
+ /**
+ * \brief set from Eigen::VectorXd and structure
+ *
+ * Usage:
+ *
+ * Eigen::VectorXd vec_eigen;
+ * wolf::VectorComposite vec_comp;
+ *
+ * vec_comp.set( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ void set(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
+ void setZero();
+
+ friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
+ friend wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
+ friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
+ friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x);
+};
+
+
+class MatrixComposite : public std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >
+{
+
+ private:
+ std::unordered_map < string, unsigned int> size_rows_, size_cols_;
+ unsigned int rows() const;
+ unsigned int cols() const;
+ void sizeAndIndices(const StateStructure & _struct_rows,
+ const StateStructure & _struct_cols,
+ std::unordered_map < string, unsigned int>& _indices_rows,
+ std::unordered_map < string, unsigned int>& _indices_cols,
+ unsigned int& _rows,
+ unsigned int& _cols) const;
+
+ public:
+ MatrixComposite() {};
+ MatrixComposite(const StateStructure& _row_structure,
+ const StateStructure& _col_structure);
+ MatrixComposite(const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes);
+ MatrixComposite (const MatrixComposite& m);
+ /**
+ * \brief Construct from Eigen::VectorXd and structure
+ *
+ * Usage:
+ *
+ * VectorXd vec_eigen(1,2,3,4,5);
+ *
+ * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ MatrixComposite(const MatrixXd& _m,
+ const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes);
+ ~MatrixComposite() = default;
+
+ bool check() const;
+ static MatrixComposite zero(const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes);
+
+ static MatrixComposite identity(const StateStructure& _structure,
+ const std::list<int>& _sizes);
+
+ unsigned int count(const std::string &_row,
+ const std::string &_col) const;
+
+ bool emplace(const std::string &_row,
+ const std::string &_col,
+ const Eigen::MatrixXd &_mat_blk);
+
+ // throw error if queried block is not present
+ bool at(const std::string &_row,
+ const std::string &_col,
+ Eigen::MatrixXd &_mat_blk) const;
+ const MatrixXd& at(const std::string &_row,
+ const std::string &_col) const;
+ MatrixXd& at(const std::string &_row,
+ const std::string &_col);
+
+ // make some shadowed API available
+ using std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >::at;
+ using std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >::count;
+
+
+ MatrixXd matrix(const StateStructure & _struct_rows,
+ const StateStructure & _struct_cols) const;
+
+ MatrixComposite operator * (double scalar_right) const;
+ MatrixComposite operator + (const MatrixComposite & _second) const;
+ MatrixComposite operator - (const MatrixComposite & _second) const;
+ MatrixComposite operator - (void) const;
+
+ /**
+ * \brief Matrix product
+ *
+ * This function computes the matrix product M * N
+ *
+ * It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
+ * both in their structure and their individual sizes.
+ *
+ * For example: Let us call 'this' matrix with the name 'M'.
+ *
+ * The matrix 'M' maps the "PO" space into a new space "VW":
+ * M["V"]["P"] M["V"]["O"]
+ * M["W"]["P"] M["W"]["O"]
+ *
+ * The second matrix N has blocks "PO" in vertical arrangement,
+ * and "XY" in horizontal arrangement:
+ * N["P"]["X"] N["P"]["Y"]
+ * N["O"]["X"] N["O"]["Y"]
+ *
+ * Also, assume that the sizes of the blocks "P" and "O" are the same in M and N.
+ *
+ * Then, the result is a matrix S = M * N with block arrangements "VW" and "XY"
+ * S["V"]["X"] S["V"]["Y"]
+ * S["W"]["X"] S["W"]["Y"]
+ */
+ MatrixComposite operator *(const MatrixComposite & _second) const;
+
+ /**
+ * \brief Propagate a given covariances matrix, with 'this' being a Jacobian matrix
+ *
+ * This function computes the product J * Q * J.transpose.
+ *
+ * It considers that this is a Jacobian matrix, and that the provided matrix
+ * is a proper covariance (e.g. symmmetric and semi-positive).
+ * It also considers that the Jacobian blocks are compatible with the blocks
+ * of the provided covariance, both in their structure and their individual sizes.
+ *
+ * If these conditions are not satisfied, the product will fail and throw,
+ * or give aberrant results at best.
+ *
+ * -----
+ *
+ * For example: Let us call 'this' a Jacobian matrix with the name 'J'.
+ *
+ * This Jacobian 'J' maps the "PO" blocks into "VW":
+ *
+ * J["V"]["P"] J["V"]["O"]
+ * J["W"]["P"] J["W"]["O"]
+ *
+ * The provided matrix is a covariances matrix Q.
+ * Q has blocks "P", "O" in both vertical and horizontal arrangements:
+ *
+ * Q["P"]["P"] Q["P"]["O"]
+ * Q["O"]["P"] Q["O"]["O"]
+ *
+ * Also, it is required that the sizes of the blocks "P", "O" are the same in Q and J.
+ *
+ * Now, upon a call to
+ *
+ * MatrixComposite S = J.propagate(Q);
+ *
+ * the result is a covariances matrix S with blocks "V" and "W"
+ *
+ * S["V"]["V"] S["V"]["W"]
+ * S["W"]["V"] S["W"]["W"]
+ */
+ MatrixComposite propagate(const MatrixComposite & _Cov); // This performs Jac * this * Jac.tr
+
+ /**
+ * \brief Matrix-vector product
+ *
+ * This function computes the matrix product M * N
+ *
+ * It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
+ * both in their structure and their individual sizes.
+ *
+ * For example: Let us call 'this' matrix with the name 'M'.
+ *
+ * The matrix 'M' maps the "PO" space into a new space "VW":
+ * M["V"]["P"] M["V"]["O"]
+ * M["W"]["P"] M["W"]["O"]
+ *
+ * The second vector V has blocks "PO" in vertical arrangement,
+ * V["P"]
+ * V["O"]
+ *
+ * Also, assume that the sizes of the blocks "P" and "O" are the same in M and V.
+ *
+ * Then, the result is a vector S = M * V with block arrangement "VW"
+ * S["V"]
+ * S["W"]
+ */
+ VectorComposite operator *(const VectorComposite & _second) const;
+
+ friend std::ostream& operator << (std::ostream & _os, const MatrixComposite & _M);
+
+ friend MatrixComposite operator * (double scalar_left, const MatrixComposite& M);
+};
+
+
+class StateBlockComposite
+{
+ public:
+ StateBlockComposite() = default;
+ ~StateBlockComposite() = default;
+
+ const StateBlockMap& getStateBlockMap() const;
+
+ StateBlockPtr add(const std::string& _sb_type, const StateBlockPtr& _sb);
+
+ // Emplace derived state blocks (angle, quaternion, etc).
+ template<typename SB, typename ... Args>
+ std::shared_ptr<SB> emplace(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor);
+
+ // Emplace base state blocks.
+ template<typename ... Args>
+ inline StateBlockPtr emplace(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
+
+ unsigned int remove(const std::string& _sb_type);
+ bool has(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
+ bool has(const StateBlockPtr& _sb) const;
+ StateBlockPtr at(const std::string& _sb_type) const;
+ void set(const std::string& _sb_type, const StateBlockPtr& _sb);
+ void set(const std::string& _sb_type, const VectorXd& _vec);
+ void setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors);
+ bool key(const StateBlockPtr& _sb, std::string& _key) const;
+ std::string key(const StateBlockPtr& _sb) const;
+ StateBlockMapCIter find(const StateBlockPtr& _sb) const;
+
+ // identity and zero (they are the same with different names)
+ void setIdentity(bool _notify = true);
+ void setZero(bool _notify = true);
+ VectorComposite identity() const;
+ VectorComposite zero() const;
+
+ // Plus operator
+ void plus(const VectorComposite& _dx);
+
+ // Perturb state with random noise
+ void perturb(double amplitude = 0.01);
+
+ // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
+ void fix();
+ void unfix();
+ bool isFixed() const;
+
+ // Get composite of state vectors (not blocks)
+ VectorComposite getState() const;
+ bool getState(VectorComposite& _state) const;
+ void setState(const VectorComposite& _state);
+
+ // Get compact Eigen vector and related things
+ SizeEigen stateSize() const;
+ SizeEigen stateSize(const StateStructure& _structure) const;
+ VectorXd stateVector(const StateStructure& _structure) const;
+ void stateVector(const StateStructure& _structure, VectorXd& _vector) const;
+
+ private:
+ StateBlockMap state_block_map_;
+};
+
+//////////// IMPLEMENTATION ////////////
+
+inline unsigned int MatrixComposite::count(const std::string &_row, const std::string &_col) const
+{
+ const auto& rit = this->find(_row);
+
+ if (rit == this->end())
+ return 0;
+
+ else
+ return rit->second.count(_col);
+}
+
+
+template<typename SB, typename ... Args>
+inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const std::string &_sb_type,
+ Args &&... _args_of_derived_state_block_constructor)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
+
+ add(_sb_type, sb);
+
+ return sb;
+}
+
+template<typename ... Args>
+inline StateBlockPtr wolf::StateBlockComposite::emplace(const std::string &_sb_type,
+ Args &&... _args_of_base_state_block_constructor)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
+
+ add(_sb_type, sb);
+
+ return sb;
+}
+
+inline MatrixComposite::MatrixComposite (const MatrixComposite& m)
+ : unordered_map<string, unordered_map<string, MatrixXd> >(m), size_rows_(m.size_rows_), size_cols_(m.size_cols_)
+{
+}
+
+}
+
+
+#endif /* STATE_BLOCK_STATE_COMPOSITE_H_ */
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index f9a0c099841c025671bff1b51d50df86fccff51a..ec42d19e188593b61db2a78b8f6ecbf35680216d 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -20,6 +20,9 @@ class StateHomogeneous3d : public StateBlock
StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false);
virtual ~StateHomogeneous3d();
+ virtual void setIdentity(bool _notify = true) override;
+ virtual Eigen::VectorXd identity() const override;
+
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
@@ -42,6 +45,17 @@ inline StateHomogeneous3d::~StateHomogeneous3d()
// The local_param_ptr_ pointer is already removed by the base class
}
+
+inline void StateHomogeneous3d::setIdentity(bool _notify)
+{
+ setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
+}
+
+inline Eigen::VectorXd StateHomogeneous3d::identity() const
+{
+ return Eigen::Quaterniond::Identity().coeffs();
+}
+
inline StateBlockPtr StateHomogeneous3d::create (const Eigen::VectorXd& _state, bool _fixed)
{
MatrixSizeCheck<4,1>::check(_state);
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 817d436aef72c9b128ce97dd8569f380a575ce1b..1533607134e89684674a4eec1d88d7cc83c9e6f0 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -21,6 +21,9 @@ class StateQuaternion : public StateBlock
StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false);
virtual ~StateQuaternion();
+ virtual void setIdentity(bool _notify = true) override;
+ virtual Eigen::VectorXd identity() const override;
+
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
@@ -47,6 +50,17 @@ inline StateQuaternion::~StateQuaternion()
// The local_param_ptr_ pointer is already removed by the base class
}
+
+inline void StateQuaternion::setIdentity(bool _notify)
+{
+ setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
+}
+
+inline Eigen::VectorXd StateQuaternion::identity() const
+{
+ return Eigen::Quaterniond::Identity().coeffs();
+}
+
inline StateBlockPtr StateQuaternion::create (const Eigen::VectorXd& _state, bool _fixed)
{
MatrixSizeCheck<4,1>::check(_state);
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 7adcb70aefe9db68c44a5ac08c4d42e47162a921..a5fca0aa7338af088a33622a83a348ef9ddbf567 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -64,12 +64,9 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
// FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame
public:
- TrajectoryBase(const std::string& _frame_sturcture);
+ TrajectoryBase();
virtual ~TrajectoryBase();
- // Properties
- std::string getFrameStructure() const;
-
// Frames
const FrameList& getFrameList() const;
FrameBasePtr getLastFrame() const;
@@ -146,11 +143,6 @@ inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const
else return nullptr;
}
-inline std::string TrajectoryBase::getFrameStructure() const
-{
- return frame_structure_;
-}
-
} // namespace wolf
#endif
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index a1b4a5a86ee2082b6f44bd3eb8d37e0c3a5c88d3..8c8e34935d728a505537643fa4c51bf001787bb6 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -4,6 +4,7 @@
// wolf
#include "core/common/wolf.h"
#include "core/utils/converter_utils.h"
+#include "core/state_block/state_composite.h"
// Eigen
#include <eigen3/Eigen/Dense>
@@ -22,6 +23,15 @@
*/
namespace wolf{
+template<class A, class B>
+struct Wpair : std::pair<A,B>
+{
+ Wpair(A first, B second): std::pair<A,B>(first, second)
+ {
+
+ }
+};
+ //// CONVERTERS ~~~~ STRING ----> TYPE
template<typename T>
struct converter{
static T convert(std::string val){
@@ -120,6 +130,11 @@ struct converter<std::string>{
static std::string convert(std::pair<A,B> val){
return "{" + converter<std::string>::convert(val.first) + ":" + converter<std::string>::convert(val.second) + "}";
}
+ //NEW GUY
+ template<typename A, typename B>
+ static std::string convert(Wpair<A,B> val){
+ return "(" + converter<std::string>::convert(val.first) + "," + converter<std::string>::convert(val.second) + ")";
+ }
template<typename A, typename B>
static std::string convert(std::map<A,B> val){
std::string result = "";
@@ -129,6 +144,15 @@ struct converter<std::string>{
if(result.size() > 0) result = result.substr(1,result.size());
return "[" + result + "]";
}
+ template<typename A, typename B>
+ static std::string convert(std::unordered_map<A,B> val){
+ std::string result = "";
+ for(auto it : val){
+ result += "," + converter<std::string>::convert(it);
+ }
+ if(result.size() > 0) result = result.substr(1,result.size());
+ return "[" + result + "]";
+ }
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
static std::string convert(Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> val){
std::string result = "";
@@ -143,7 +167,25 @@ struct converter<std::string>{
}
return "[" + result + "]";
}
+ //// WOLF DEFINED TYPES -----> TO STRING
+ static std::string convert(VectorComposite val){
+ //TODO
+ // throw std::runtime_error("TODO! We still haven't got around to implement convert for VectorComposite to String :(");
+ auto helper = std::unordered_map<StateStructure, Eigen::VectorXd>();
+ for(const auto& pair: val)
+ helper.insert(std::pair<StateStructure, Eigen::VectorXd>(pair.first, pair.second));
+ return converter<std::string>::convert(helper);
+ }
+ static std::string convert(MatrixComposite val){
+ //TODO
+ // throw std::runtime_error("TODO! We still haven't got around to implement convert for MatrixComposite to String :(");
+ auto helper = std::unordered_map< StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>();
+ for(const auto& pair: val)
+ helper.insert(std::pair<StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>(pair.first, pair.second));
+ return converter<std::string>::convert(helper);
+ }
};
+ //// CONVERTERS ~~~~ TYPE ----> STRING
template<typename A, typename B>
struct converter<std::pair<A,B>>{
static std::pair<A,B> convert(std::string val){
@@ -153,6 +195,17 @@ struct converter<std::pair<A,B>>{
return std::pair<A,B>(converter<A>::convert(matches[1].str()), converter<B>::convert(matches[2].str()));
} else throw std::runtime_error("Invalid string format representing a pair. Correct format is {identifier:value}. String provided: " + val);
}
+};
+ //NEW GUY
+template<typename A, typename B>
+struct converter<Wpair<A,B>>{
+ static Wpair<A,B> convert(std::string val){
+ std::regex rgxP("\\(([^\\(,]+),([^\\)]+)\\)");
+ std::smatch matches;
+ if(std::regex_match(val, matches, rgxP)) {
+ return Wpair<A,B>(converter<A>::convert(matches[1].str()), converter<B>::convert(matches[2].str()));
+ } else throw std::runtime_error("Invalid string format representing a Wpair. Correct format is (first,second). String provided: " + val);
+ }
};
// TODO: WARNING!! For some reason when trying to specialize converter to std::array
// it defaults to the generic T type, thus causing an error!
@@ -216,5 +269,67 @@ struct converter<std::map<std::string,A>>{
return map;
}
};
+template<typename A, typename B>
+struct converter<std::map<A,B>>{
+ static std::map<A,B> convert(std::string val){
+ std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
+ if(not std::regex_match(val, rgxM))
+ throw std::runtime_error("Invalid string representation of a Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
+
+ auto v = utils::parseList(val);
+ auto map = std::map<A, B>();
+ for(auto it : v){
+ auto p = converter<std::pair<A,B>>::convert(it);
+ map.insert(std::pair<A, B>(p.first, p.second));
+ }
+ return map;
+ }
+};
+template<typename A, typename B>
+struct converter<std::unordered_map<A,B>>{
+ static std::unordered_map<A,B> convert(std::string val){
+ std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
+ if(not std::regex_match(val, rgxM))
+ throw std::runtime_error("Invalid string representation of an unordered Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
+
+ auto v = utils::parseList(val);
+ auto map = std::unordered_map<A, B>();
+ for(auto it : v){
+ auto p = converter<std::pair<A,B>>::convert(it);
+ map.insert(std::pair<A, B>(p.first, p.second));
+ }
+ return map;
+ }
+};
+//// FROM STRING -----> WOLF DEFINED TYPES
+template<>
+struct converter<VectorComposite>{
+ static VectorComposite convert(std::string val){
+ auto unordered_map = converter<std::unordered_map<StateStructure, Eigen::VectorXd>>::convert(val);
+ // VectorComposite result = VectorComposite(unordered_map);
+ // return result;
+ auto helper = VectorComposite();
+ for(auto const& it: unordered_map)
+ {
+ helper.insert(std::pair<StateStructure, Eigen::VectorXd>(it.first, it.second));
+ }
+ return helper;
+ }
+};
+template<>
+struct converter<MatrixComposite>{
+ static MatrixComposite convert(std::string val){
+ auto unordered_map = converter<std::unordered_map<StateStructure,
+ std::unordered_map<StateStructure, Eigen::MatrixXd>>>::convert(val);
+ // VectorComposite result = VectorComposite(unordered_map);
+ // return result;
+ auto helper = MatrixComposite();
+ for(auto const& it: unordered_map)
+ {
+ helper.insert(std::pair<StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>(it.first, it.second));
+ }
+ return helper;
+ }
+};
}
#endif
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 072a729a6d0cc8ec278f0b30e33141bbf7ca16db..c091b5527677fb491fb2b819de8770aae7e497c2 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -17,7 +17,6 @@ CaptureBase::CaptureBase(const std::string& _type,
HasStateBlocks(""),
frame_ptr_(), // nullptr
sensor_ptr_(_sensor_ptr),
- calib_size_(0),
capture_id_(++capture_id_count_),
time_stamp_(_ts)
{
@@ -52,8 +51,6 @@ CaptureBase::CaptureBase(const std::string& _type,
{
WOLF_ERROR("Provided sensor parameters but no sensor pointer");
}
-
- updateCalibSize();
}
CaptureBase::~CaptureBase()
@@ -151,7 +148,7 @@ bool CaptureBase::isConstrainedBy(const FactorBasePtr &_factor) const
}
-const std::string& CaptureBase::getStructure() const
+const StateStructure& CaptureBase::getStructure() const
{
if (getSensor())
return getSensor()->getStructure();
@@ -175,57 +172,11 @@ StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
void CaptureBase::fix()
{
HasStateBlocks::fix();
- updateCalibSize();
}
void CaptureBase::unfix()
{
HasStateBlocks::unfix();
- updateCalibSize();
-}
-
-SizeEigen CaptureBase::computeCalibSize() const
-{
- SizeEigen sz = 0;
- for (const auto& key : getStructure())
- {
- auto sb = getStateBlock(key);
- if (sb && !sb->isFixed())
- sz += sb->getSize();
- }
- return sz;
-}
-
-Eigen::VectorXd CaptureBase::getCalibration() const
-{
- Eigen::VectorXd calib(calib_size_);
- SizeEigen index = 0;
- for (const auto& key : getStructure())
- {
- auto sb = getStateBlock(key);
- if (sb && !sb->isFixed())
- {
- calib.segment(index, sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
- }
- return calib;
-}
-
-void CaptureBase::setCalibration(const VectorXd& _calib)
-{
- updateCalibSize();
- assert(_calib.size() == calib_size_ && "Wrong size of calibration vector");
- SizeEigen index = 0;
- for (const auto& key : getStructure())
- {
- auto sb = getStateBlock(key);
- if (sb && !sb->isFixed())
- {
- sb->setState(_calib.segment(index, sb->getSize()));
- index += sb->getSize();
- }
- }
}
void CaptureBase::move(FrameBasePtr _frm_ptr)
@@ -312,7 +263,7 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
{
_stream << _tabs << (isFixed() ? "Fix" : "Est") << ", ts=" << std::setprecision(5)
<< getTimeStamp();
- _stream << ",\t x= ( " << std::setprecision(2) << getState().transpose() << " )";
+ _stream << ",\t x= ( " << std::setprecision(2) << getStateVector() << " )";
_stream << std::endl;
}
else if (_state_blocks)
@@ -326,22 +277,8 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
_stream << std::endl;
}
}
-
-
-
-// if (_state_blocks)
-// for (const auto& sb : getStateBlockVec())
-// {
-// if(sb != nullptr)
-// {
-// _stream << " sb: ";
-// _stream << (sb->isFixed() ? "Fix" : "Est");
-// if (_metric)
-// _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
-// _stream << std::endl;
-// }
-// }
}
+
void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp
index 05f8a5c49fb73e228b135fffc4de398e9e4ba038..7a09336470bfefeaf304cfbb817a8793c4c9a82d 100644
--- a/src/capture/capture_diff_drive.cpp
+++ b/src/capture/capture_diff_drive.cpp
@@ -18,8 +18,6 @@ CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
_sensor_ptr,
_data,
_data_cov,
- 3,
- 3,
_capture_origin_ptr,
_p_ptr,
_o_ptr,
@@ -28,12 +26,4 @@ CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
//
}
-Eigen::VectorXd CaptureDiffDrive::correctDelta(const VectorXd& _delta,
- const VectorXd& _delta_error) const
-{
- Vector3d delta_corrected = _delta + _delta_error;
- delta_corrected(2) = pi2pi(delta_corrected(2));
- return delta_corrected;
-}
-
} // namespace wolf
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index aa7d4f23a633a01090c9a0d042e46ff153613bd6..1a61a7f5c763a4508427c5fb2566c3fd53248af7 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -14,8 +14,6 @@ CaptureMotion::CaptureMotion(const std::string & _type,
const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr) :
CaptureBase(_type, _ts, _sensor_ptr),
data_(_data),
@@ -31,8 +29,6 @@ CaptureMotion::CaptureMotion(const std::string & _type,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr,
StateBlockPtr _p_ptr ,
StateBlockPtr _o_ptr ,
@@ -51,26 +47,29 @@ CaptureMotion::~CaptureMotion()
//
}
-Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current) const
+bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance)
{
- VectorXd calib_preint = getCalibrationPreint();
- VectorXd delta_preint = getBuffer().back().delta_integr_;
- MatrixXd jac_calib = getBuffer().back().jacobian_calib_;
- VectorXd delta_error = jac_calib * (_calib_current - calib_preint);
- VectorXd delta_corrected = correctDelta(delta_preint, delta_error);
- return delta_corrected;
-}
+ // the same capture is within tolerance
+ if (this->time_stamp_ - _time_tolerance <= _ts && _ts <= this->time_stamp_ + _time_tolerance)
+ return true;
-Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const
-{
- Motion motion = getBuffer().getMotion(_ts);
- VectorXd delta_preint = motion.delta_integr_;
- MatrixXd jac_calib = motion.jacobian_calib_;
- VectorXd delta_error = jac_calib * (_calib_current - calib_preint_);
- VectorXd delta_corrected = correctDelta(delta_preint, delta_error);
- return delta_corrected;
+ // buffer is empty, and the capture is not within tolerance
+ if (this->getBuffer().empty())
+ return false;
+
+ // buffer encloses timestamp, if ts is:
+ // from : origin.tx + tt not included
+ // to : capture.ts + tt included
+ if (this->getOriginCapture()->getTimeStamp() +_time_tolerance < _ts and _ts <= this->getBuffer().back().ts_ + _time_tolerance)
+ return true;
+
+ // not found anywhere
+ return false;
}
+
+
+
void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
_stream << _tabs << "CapM" << id() << " " << getType();
@@ -114,13 +113,13 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
if ( _metric && ! getBuffer().empty())
{
_stream << _tabs << " " << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl;
- if (hasCalibration())
- {
- _stream << _tabs << " " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl;
- _stream << _tabs << " " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl;
- _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" << std::endl;
- _stream << _tabs << " " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
- }
+// if (hasCalibration())
+// {
+// _stream << _tabs << " " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl;
+// _stream << _tabs << " " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl;
+//// _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" << std::endl;
+//// _stream << _tabs << " " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
+// }
}
}
diff --git a/src/capture/capture_odom_2d.cpp b/src/capture/capture_odom_2d.cpp
index b024294a5df38772bc040495728958aa8cb08c4c..4755b6fe067fcbcd81c3ae66ecb4be6b562078e7 100644
--- a/src/capture/capture_odom_2d.cpp
+++ b/src/capture/capture_odom_2d.cpp
@@ -14,7 +14,7 @@ CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, 3, 3, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
{
//
}
@@ -24,7 +24,7 @@ CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr)
{
//
}
diff --git a/src/capture/capture_odom_3d.cpp b/src/capture/capture_odom_3d.cpp
index 447e1d500f072e2754de4ed107aeb61e6e6299c0..a692011ffc2b61c0d46e9f024d39b354106feeb7 100644
--- a/src/capture/capture_odom_3d.cpp
+++ b/src/capture/capture_odom_3d.cpp
@@ -14,7 +14,7 @@ CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, 7, 6, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
{
//
}
@@ -24,7 +24,7 @@ CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr)
{
//
}
@@ -34,13 +34,5 @@ CaptureOdom3d::~CaptureOdom3d()
//
}
-Eigen::VectorXd CaptureOdom3d::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
-{
- VectorXd delta(7);
- delta.head(3) = _delta.head(3) + _delta_error.head(3);
- delta.tail(4) = (Quaterniond(_delta.data()+3) * exp_q(_delta_error.tail(3))).coeffs();
- return delta;
-}
-
} /* namespace wolf */
diff --git a/src/capture/capture_velocity.cpp b/src/capture/capture_velocity.cpp
deleted file mode 100644
index 538121825ba5ca528d4e850d55edb514de90a87f..0000000000000000000000000000000000000000
--- a/src/capture/capture_velocity.cpp
+++ /dev/null
@@ -1,42 +0,0 @@
-#include "core/capture/capture_velocity.h"
-
-namespace wolf {
-
-CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXd& _velocity,
- SizeEigen _delta_size, SizeEigen _delta_cov_size,
- CaptureBasePtr _capture_origin_ptr) :
- CaptureMotion("CaptureVelocity", _ts, _sensor_ptr, _velocity,
- _delta_size, _delta_cov_size, _capture_origin_ptr)
-{
- //
-}
-
-CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXd& _velocity,
- const Eigen::MatrixXd& _velocity_cov,
- SizeEigen _delta_size, SizeEigen _delta_cov_size,
- CaptureBasePtr _capture_origin_ptr,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr) :
- CaptureMotion("CaptureVelocity", _ts, _sensor_ptr, _velocity, _velocity_cov,
- _delta_size, _delta_cov_size, _capture_origin_ptr,
- _p_ptr, _o_ptr, _intr_ptr)
-{
- //
-}
-
-const Eigen::VectorXd& CaptureVelocity::getVelocity() const
-{
- return getData();
-}
-
-const Eigen::MatrixXd& CaptureVelocity::getVelocityCov() const
-{
- return getDataCovariance();
-}
-
-} // namespace wolf
diff --git a/src/common/time_stamp.cpp b/src/common/time_stamp.cpp
index 107bd0eb85917959f0b4fbdaacba3c136deacea7..528d0ad54872bc077ba74d72be0cb2cb79099d13 100644
--- a/src/common/time_stamp.cpp
+++ b/src/common/time_stamp.cpp
@@ -61,7 +61,7 @@ TimeStamp TimeStamp::operator -(const double& dt) const
return ts;
}
-inline void TimeStamp::operator -=(const double& dt)
+void TimeStamp::operator -=(const double& dt)
{
unsigned long int dt_nano = (unsigned long int)(dt*NANOSECS);
time_stamp_nano_ = (dt_nano > time_stamp_nano_ ? 0 : time_stamp_nano_ - dt_nano);
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
index 73bc391c2c038ec27da7e8b2fc936e054795a415..6d146706065f2ecc0d1ce51bfe1ac290a2f1d2bd 100644
--- a/src/factor/factor_analytic.cpp
+++ b/src/factor/factor_analytic.cpp
@@ -4,6 +4,7 @@
namespace wolf {
FactorAnalytic::FactorAnalytic(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -12,7 +13,7 @@ FactorAnalytic::FactorAnalytic(const std::string& _tp,
bool _apply_loss_function, FactorStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 0c9286337d850796006c208a67aba4c2775e137e..4e18cb47d8485a7b9ee83853e863ddf20bb7e78a 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -7,6 +7,7 @@ namespace wolf {
unsigned int FactorBase::factor_id_count_ = 0;
FactorBase::FactorBase(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -15,7 +16,7 @@ FactorBase::FactorBase(const std::string& _tp,
bool _apply_loss_function,
FactorStatus _status) :
NodeBase("FACTOR", _tp),
- feature_ptr_(),
+ feature_ptr_(), // will be filled in link()
factor_id_(++factor_id_count_),
status_(_status),
apply_loss_function_(_apply_loss_function),
@@ -33,9 +34,14 @@ FactorBase::FactorBase(const std::string& _tp,
feature_other_list_.push_back(_feature_other_ptr);
if (_landmark_other_ptr)
landmark_other_list_.push_back(_landmark_other_ptr);
+
+ assert(_feature_ptr && "null feature pointer when creating a factor");
+ measurement_ = _feature_ptr->getMeasurement();
+ measurement_sqrt_information_upper_ = _feature_ptr->getMeasurementSquareRootInformationUpper();
}
FactorBase::FactorBase(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
@@ -44,7 +50,7 @@ FactorBase::FactorBase(const std::string& _tp,
bool _apply_loss_function,
FactorStatus _status) :
NodeBase("FACTOR", _tp),
- feature_ptr_(),
+ feature_ptr_(), // will be filled in link()
factor_id_(++factor_id_count_),
status_(_status),
apply_loss_function_(_apply_loss_function),
@@ -62,6 +68,10 @@ FactorBase::FactorBase(const std::string& _tp,
feature_other_list_.push_back(fo);
for (auto& Lo : landmark_other_list_)
landmark_other_list_.push_back(Lo);
+
+ assert(_feature_ptr && "null feature pointer when creating a factor");
+ measurement_ = _feature_ptr->getMeasurement();
+ measurement_sqrt_information_upper_ = _feature_ptr->getMeasurementSquareRootInformationUpper();
}
@@ -97,7 +107,7 @@ void FactorBase::remove(bool viral_remove_empty_parent)
if (frm_o)
{
frm_o->removeConstrainedBy(this_fac);
- if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
+ if (viral_remove_empty_parent && frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
frm_o->remove(viral_remove_empty_parent);
}
}
@@ -108,7 +118,7 @@ void FactorBase::remove(bool viral_remove_empty_parent)
if (cap_o)
{
cap_o->removeConstrainedBy(this_fac);
- if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
+ if (viral_remove_empty_parent && cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
cap_o->remove(viral_remove_empty_parent);
}
}
@@ -119,7 +129,7 @@ void FactorBase::remove(bool viral_remove_empty_parent)
if (ftr_o)
{
ftr_o->removeConstrainedBy(this_fac);
- if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
+ if (viral_remove_empty_parent && ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
ftr_o->remove(viral_remove_empty_parent);
}
}
@@ -130,7 +140,7 @@ void FactorBase::remove(bool viral_remove_empty_parent)
if (lmk_o)
{
lmk_o->removeConstrainedBy(this_fac);
- if (lmk_o->getConstrainedByList().empty())
+ if (viral_remove_empty_parent && lmk_o->getConstrainedByList().empty())
lmk_o->remove(viral_remove_empty_parent);
}
}
@@ -139,24 +149,6 @@ void FactorBase::remove(bool viral_remove_empty_parent)
}
}
-const Eigen::VectorXd& FactorBase::getMeasurement() const
-{
- assert(getFeature() != nullptr && "calling getMeasurement before linking with a feature");
- return getFeature()->getMeasurement();
-}
-
-const Eigen::MatrixXd& FactorBase::getMeasurementCovariance() const
-{
- assert(getFeature() != nullptr && "calling getMeasurementCovariance before linking with a feature");
- return getFeature()->getMeasurementCovariance();
-}
-
-const Eigen::MatrixXd& FactorBase::getMeasurementSquareRootInformationUpper() const
-{
- assert(getFeature() != nullptr && "calling getMeasurementSquareRootInformationUpper before linking with a feature");
- return getFeature()->getMeasurementSquareRootInformationUpper();
-}
-
CaptureBasePtr FactorBase::getCapture() const
{
assert(getFeature() != nullptr && "calling getCapture before linking with a feature");
@@ -249,11 +241,6 @@ bool FactorBase::hasLandmarkOther(const LandmarkBasePtr &_lmk_other) const
return false;
}
-//void FactorBase::setApplyLossFunction(const bool _apply)
-//{
-// apply_loss_function_ = _apply;
-//}
-
void FactorBase::link(FeatureBasePtr _ftr_ptr)
{
assert(!is_removing_ && "linking a removed factor");
@@ -264,16 +251,15 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
{
WOLF_WARN("Linking with nullptr");
return;
-
- // if frame, should not be not-key
- if (getCapture() and getFrame())
- assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame.");
}
// link with feature
_ftr_ptr->addFactor(shared_from_this());
this->setFeature(_ftr_ptr);
+ // if frame, should be key
+ if (getCapture() and getFrame())
+ assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame.");
// set problem ( and register factor )
WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " THAT IS NOT CONNECTED WITH PROBLEM.");
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 86d56b3e46bf20390a7083d695b0151b910d17ee..69133345e0f57c28e3135aa3b50e0d99e179be50 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -147,6 +147,11 @@ void FeatureBase::setMeasurementInformation(const Eigen::MatrixXd & _meas_info)
measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info);
}
+void FeatureBase::setMeasurement(const Eigen::VectorXd& _meas)
+{
+ measurement_ = _meas;
+}
+
void FeatureBase::setProblem(ProblemPtr _problem)
{
NodeBase::setProblem(_problem);
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index a8c27ed499faf27eeb1e0c5e3231f1c6cf62dbf5..aed4c7bc69b99233426faeb6baa592844e0fa63a 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -6,12 +6,68 @@
#include "core/state_block/state_block.h"
#include "core/state_block/state_angle.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/factory_state_block.h"
namespace wolf {
unsigned int FrameBase::frame_id_count_ = 0;
-FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
+FrameBase::FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ const std::string _frame_structure,
+ const VectorComposite& _state) :
+ NodeBase("FRAME", "FrameBase"),
+ HasStateBlocks(_frame_structure),
+ trajectory_ptr_(),
+ frame_id_(++frame_id_count_),
+ type_(_tp),
+ time_stamp_(_ts)
+{
+ for (const auto& pair_key_vec : _state)
+ {
+ const auto& key = pair_key_vec.first;
+ const auto& vec = pair_key_vec.second;
+
+ StateBlockPtr sb = FactoryStateBlock::get().create(key, vec, false); // false = non fixed
+
+ addStateBlock(key, sb);
+ }
+}
+
+
+FrameBase::FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ const std::string _frame_structure,
+ const std::list<VectorXd>& _vectors) :
+ NodeBase("FRAME", "FrameBase"),
+ HasStateBlocks(_frame_structure),
+ trajectory_ptr_(),
+ frame_id_(++frame_id_count_),
+ type_(_tp),
+ time_stamp_(_ts)
+{
+ assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
+
+ auto vec_it = _vectors.begin();
+ for (const auto ckey : _frame_structure)
+ {
+ const auto& key = string(1,ckey);
+ const auto& vec = *vec_it;
+
+ StateBlockPtr sb = FactoryStateBlock::get().create(key, vec, false); // false = non fixed
+
+ addStateBlock(key, sb);
+
+ vec_it++;
+ }
+}
+
+
+FrameBase::FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ StateBlockPtr _v_ptr) :
NodeBase("FRAME", "FrameBase"),
HasStateBlocks(""),
trajectory_ptr_(),
@@ -33,7 +89,11 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
}
}
-FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x) :
+FrameBase::FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ const std::string _frame_structure,
+ const SizeEigen _dim,
+ const Eigen::VectorXd& _x) :
NodeBase("FRAME", "FrameBase"),
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
@@ -321,7 +381,10 @@ void FrameBase::setProblem(ProblemPtr _problem)
void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- _stream << _tabs << (isKey() ? (isKey() ? "KFrm" : "AFrm") : "Frm") << id() << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : "");
+ _stream << _tabs << (isKey() ? "KFrm" : "Frm") << id()
+ << " " << getStructure()
+ << " ts = " << std::setprecision(3) << getTimeStamp()
+ << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : "");
if (_constr_by)
{
_stream << " <-- ";
@@ -330,18 +393,21 @@ void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _sta
}
_stream << std::endl;
- if (_metric && _state_blocks){
+ if (_metric && _state_blocks)
+ {
for (const auto& key : getStructure())
{
auto sb = getStateBlock(key);
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
+ _stream << _tabs << " " << key
+ << "[" << (sb->isFixed() ? "Fix" : "Est")
+ << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
+ << std::endl;
}
}
else if (_metric)
{
- _stream << _tabs << " " << (isFixed() ? "Fix" : "Est") << ", ts=" << std::setprecision(5)
- << getTimeStamp();
- _stream << ",\t x = ( " << std::setprecision(2) << getState().transpose() << " )";
+ _stream << _tabs << " " << (isFixed() ? "Fix" : "Est");
+ _stream << ",\t x = ( " << std::setprecision(3) << getStateVector().transpose() << " )";
_stream << std::endl;
}
else if (_state_blocks)
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index a89afb2fda060dcaf503501ea81f34c03b3f5324..ada2bef9648e15ca6a2809d9bf35dc88f2e454ac 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -179,7 +179,7 @@ void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _
else if (_metric)
{
_stream << _tabs << " " << (isFixed() ? "Fix" : "Est");
- _stream << ",\t x = ( " << std::setprecision(2) << getState().transpose() << " )";
+ _stream << ",\t x = ( " << std::setprecision(2) << getStateVector().transpose() << " )";
_stream << std::endl;
}
else if (_state_blocks)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 8ef2d8183a4e479d7a43fcbb6dadeac08a521d01..535a60350ec1e2ef84671c6e0644f478d2cfbf98 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -23,6 +23,7 @@
#include "core/utils/loader.h"
#include "core/utils/check_log.h"
#include "core/math/covariance.h"
+#include "core/state_block/factory_state_block.h"
// IRI libs includes
@@ -42,7 +43,7 @@ namespace wolf
Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
tree_manager_(nullptr),
hardware_ptr_(std::make_shared<HardwareBase>()),
- trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
+ trajectory_ptr_(std::make_shared<TrajectoryBase>()),
map_ptr_(std::make_shared<MapBase>()),
processor_is_motion_list_(std::list<IsMotionPtr>()),
frame_structure_(_frame_structure),
@@ -184,15 +185,18 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
{
problem->setPriorOptions(prior_mode,
_server.getParam<double>("problem/prior/time_tolerance"),
- _server.getParam<Eigen::VectorXd>("problem/prior/state"),
- _server.getParam<Eigen::MatrixXd>("problem/prior/cov"));
+ _server.getParam<VectorComposite>("problem/prior/state"),
+ _server.getParam<VectorComposite>("problem/prior/sigma"));
+
+
+
}
else
{
WOLF_TRACE("Prior mode: ", prior_mode);
problem->setPriorOptions(prior_mode,
_server.getParam<double>("problem/prior/time_tolerance"),
- _server.getParam<Eigen::VectorXd>("problem/prior/state"));
+ _server.getParam<VectorComposite>("problem/prior/state"));
}
// Done
@@ -321,176 +325,183 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
return (*sen_it);
}
-FrameBasePtr Problem::emplaceKeyFrame(const std::string& _frame_structure, //
+FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
const SizeEigen _dim, //
- const Eigen::VectorXd& _frame_state, //
- const TimeStamp& _time_stamp)
-{
- auto frm = FrameBase::emplaceKeyFrame<FrameBase>(trajectory_ptr_, _frame_structure, _dim, _time_stamp, _frame_state);
+ const Eigen::VectorXd& _frame_state)
+{
+ auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_,
+ _frame_key_type,
+ _time_stamp,
+ _frame_structure,
+ _dim,
+ _frame_state);
return frm;
}
-FrameBasePtr Problem::emplaceKeyFrame(const std::string& _frame_structure, //
- const SizeEigen _dim, //
- const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim)
{
- return emplaceKeyFrame(_frame_structure, _dim, getState(_time_stamp), _time_stamp);
+ return emplaceFrame(_frame_key_type,
+ _time_stamp,
+ _frame_structure,
+ getState(_time_stamp));
}
-FrameBasePtr Problem::emplaceKeyFrame(const Eigen::VectorXd& _frame_state, //
- const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceFrame (FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const VectorComposite& _frame_state)
{
- return emplaceKeyFrame(this->getFrameStructure(), this->getDim(), _frame_state, _time_stamp);
+ return FrameBase::emplace<FrameBase>(getTrajectory(),
+ _frame_key_type,
+ _time_stamp,
+ _frame_structure,
+ _frame_state);
}
-FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceFrame (FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const VectorComposite& _frame_state)
{
- return emplaceKeyFrame(this->getFrameStructure(), this->getDim(), _time_stamp);
+ return FrameBase::emplace<FrameBase>(getTrajectory(),
+ _frame_key_type,
+ _time_stamp,
+ getFrameStructure(),
+ _frame_state);
}
-Eigen::VectorXd Problem::getCurrentState() const
+FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const Eigen::VectorXd& _frame_state)
{
- Eigen::VectorXd state(getFrameStructureSize());
- getCurrentState(state);
- return state;
+ return emplaceFrame(_frame_key_type,
+ _time_stamp,
+ this->getFrameStructure(),
+ this->getDim(),
+ _frame_state);
}
-void Problem::getCurrentState(Eigen::VectorXd& _state) const
+FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp)
{
- TimeStamp ts; // throwaway timestamp
- getCurrentStateAndStamp(_state, ts);
+ return emplaceFrame(_frame_key_type,
+ _time_stamp,
+ this->getFrameStructure(),
+ this->getDim());
}
-void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) const
-{
- if (!processor_is_motion_list_.empty())
+TimeStamp Problem::getTimeStamp ( ) const
+{
+ if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
{
- // retrieve the minimum of the most recent ts in all processor is motion then call getSate(ts, state)
- std::list<TimeStamp> proc_is_motion_current_ts;
- for (auto proc: processor_is_motion_list_){
- proc_is_motion_current_ts.push_back(proc->getCurrentTimeStamp());
- }
- auto min_it = std::min_element(proc_is_motion_current_ts.begin(), proc_is_motion_current_ts.end());
- getState(*min_it, _state);
- _ts = *min_it;
+ auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
+
+ assert(last_kf_or_aux != nullptr && "Problem has no Keyframe so no timestamp can be obtained!");
+
+ return last_kf_or_aux->getTimeStamp();
}
- else if (trajectory_ptr_->rbegin() != trajectory_ptr_->rend())
+ else
{
- // kind of redundant with getState(_ts, _state)
- auto last = trajectory_ptr_->rbegin();
- (*last)->getState(_state);
- (*last)->getTimeStamp(_ts);
+ TimeStamp ts(0);
+ for (const auto& prc : processor_is_motion_list_)
+ if (prc->getTimeStamp() > ts)
+ ts = prc->getTimeStamp();
+ return ts;
}
- else
- _state = zeroState();
}
-// Problem of this implmentation: if more state
-void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
+VectorComposite Problem::getState(const StateStructure& _structure) const
{
- // if _ts is too recent, for some of the processor is motion, they return the state corresponding to their last frame timestamp
- FrameBasePtr closest_frame = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
- if (processor_is_motion_list_.empty()){
- if (closest_frame != nullptr)
- _state = closest_frame->getState();
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+ VectorComposite state;
+
+ if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
+ {
+ auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
+ if (last_kf_or_aux)
+ state = last_kf_or_aux->getState(structure);
else
- _state = zeroState();
+ state = stateZero(structure);
}
-
- // RETRIEVE FROM PROCESSOR MOTION
- // TODO: current implementation messy, would be much easier with a state being an std::unordered_map
- else {
- // Iterate over the problem state structure and get the corresponding state
- // in the first processor is motion that provides it
- // finally check if the state to concatenate list has the same total size as the problem state_size
-
- // an map is necessary as a temporary structure because we do not know in which order we will find the state blocks in processors
- std::unordered_map<char, Eigen::VectorXd> states_to_concat_map; // not necessary to be ordered
-
- for (auto proc: processor_is_motion_list_)
+ else // Compose from different processor motion
+ {
+ // compose the states of all processor motions into one only state
+ for (const auto& prc : processor_is_motion_list_)
{
- Eigen::VectorXd proc_state = proc->getState(_ts);
-
- int idx = 0;
- for (char sb_name: proc->getStateStructure()){
- // not already there
- if (states_to_concat_map.find(sb_name) == states_to_concat_map.end()){
- if (sb_name == 'O'){
- int size_sb = dim_ == 3 ? 4 : 1; // really bad: should be more transparent
- states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
- idx += size_sb;
- }
- else{
- int size_sb = dim_ == 3 ? 3 : 2;
- states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
- idx += size_sb;
- }
- }
+ const auto& prc_state = prc->getState();
+ for (const auto& pair_key_vec : prc_state)
+ {
+ if (state.count(pair_key_vec.first) == 0) // only add those keys that do not exist yet
+ state.insert(pair_key_vec);
}
}
-
- int concat_size = 0;
- for (auto state_map_it: states_to_concat_map){
- concat_size += state_map_it.second.size();
- }
- // assert(concat_size == state_size_ && "Problem with the concatenated size: " );
-
- // fill the state value from the state concatenation in the order dictated by frame_structure_
- int idx = 0;
- _state.resize(state_size_);
- for (char sb_name: frame_structure_){
- Eigen::VectorXd sb_state;
- int size_sb; // really bad...
- if (sb_name == 'O'){
- size_sb = dim_ == 3 ? 4 : 1;
- }
- else {
- size_sb = dim_ == 3 ? 3 : 2;
- }
- if (states_to_concat_map.find(sb_name) != states_to_concat_map.end()){
- sb_state = states_to_concat_map[sb_name];
- }
- else {
- // Should be taken from the last state but too messy already
- sb_state.resize(size_sb);
- sb_state.setZero();
- }
-
- _state.segment(idx, size_sb) = sb_state;
- idx += size_sb;
-
+ // check for empty blocks and fill them with zeros
+ for (const auto& ckey : frame_structure_)
+ {
+ const auto& key = string(1,ckey);
+ if (state.count(key) == 0)
+ state.emplace(key, stateZero(key).at(key));
}
}
-}
-Eigen::VectorXd Problem::getState(const TimeStamp& _ts) const
-{
- Eigen::VectorXd state(getFrameStructureSize());
- getState(_ts, state);
return state;
}
-SizeEigen Problem::getFrameStructureSize() const
+VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _structure) const
{
- return state_size_;
-}
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
-void Problem::getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const
-{
- _x_size = state_size_;
- _cov_size = state_cov_size_;
+ if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
+ {
+ const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+ if (last_kf_or_aux)
+ {
+ return last_kf_or_aux->getState(structure);
+ }
+ else
+ {
+ return stateZero(structure);
+ }
+ }
+
+ VectorComposite state;
+ for (const auto& prc : processor_is_motion_list_)
+ {
+ const auto& prc_state = prc->getState(_ts);
+
+ // transfer processor vector blocks to problem state
+ for (const auto& pair_key_vec : prc_state)
+ {
+ const auto& key = pair_key_vec.first;
+
+ if (state.count(key) == 0) // Only write once. This gives priority to processors upfront in the list
+ state.insert(pair_key_vec);
+ }
+ }
+ return state;
}
SizeEigen Problem::getDim() const
{
return dim_;
}
-std::string Problem::getFrameStructure() const
+StateStructure Problem::getFrameStructure() const
{
return frame_structure_;
}
+void Problem::appendToStructure(const StateStructure& _structure)
+{
+ for (const auto& ckey : _structure)
+ if (frame_structure_.find(ckey) == std::string::npos) // now key not found in problem structure -> append!
+ frame_structure_ += ckey;
+}
+
void Problem::setTreeManager(TreeManagerBasePtr _gm)
{
if (tree_manager_)
@@ -503,6 +514,7 @@ void Problem::setTreeManager(TreeManagerBasePtr _gm)
void Problem::addProcessorIsMotion(IsMotionPtr _processor_motion_ptr)
{
processor_is_motion_list_.push_back(_processor_motion_ptr);
+ appendToStructure(_processor_motion_ptr->getStateStructure());
}
void Problem::clearProcessorIsMotion(IsMotionPtr proc)
@@ -511,16 +523,33 @@ void Problem::clearProcessorIsMotion(IsMotionPtr proc)
if (it != processor_is_motion_list_.end()){
processor_is_motion_list_.erase(it);
}
+
+ // rebuild frame structure with remaining motion processors
+ frame_structure_.clear();
+ for (const auto& pm : processor_is_motion_list_)
+ appendToStructure(pm->getStateStructure());
}
-Eigen::VectorXd Problem::zeroState() const
+VectorComposite Problem::stateZero ( const StateStructure& _structure ) const
{
- Eigen::VectorXd state = Eigen::VectorXd::Zero(getFrameStructureSize());
-
- // Set the quaternion identity for 3d states. Set only the real part to 1:
- if(this->getDim() == 3)
- state(6) = 1.0;
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+ VectorComposite state;
+ for (const auto& ckey : structure)
+ {
+ const auto& key = string(1,ckey);
+ VectorXd vec;
+ if (key == "O")
+ {
+ if (dim_ == 2) vec = VectorXd::Zero(1);
+ else if (dim_ == 3) vec = Quaterniond::Identity().coeffs();
+ }
+ else
+ {
+ vec = VectorXd::Zero(dim_);
+ }
+ state.emplace(key, vec);
+ }
return state;
}
@@ -890,9 +919,9 @@ FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
}
void Problem::setPriorOptions(const std::string& _mode,
- const double _time_tolerance,
- const Eigen::VectorXd& _state,
- const Eigen::MatrixXd& _cov)
+ const double _time_tolerance ,
+ const VectorComposite& _state ,
+ const VectorComposite& _sigma )
{
assert(prior_options_ != nullptr && "prior options have already been applied");
assert(prior_options_->mode == "" && "prior options have already been set");
@@ -906,16 +935,30 @@ void Problem::setPriorOptions(const std::string& _mode,
{
assert(_time_tolerance > 0 && "time tolerance should be bigger than 0");
- WOLF_TRACE("prior state: ", _state.transpose());
+ WOLF_TRACE("prior state: ", _state);
WOLF_TRACE("prior time tolerance: ", _time_tolerance);
prior_options_->state = _state;
prior_options_->time_tolerance = _time_tolerance;
if (prior_options_->mode == "factor")
{
- assert(isCovariance(_cov) && "cov is not a covariance matrix (symmetric and Pos Def)");
- WOLF_TRACE("prior covariance:\n" , _cov);
- prior_options_->cov = _cov;
+ bool isPositive = true;
+ for(const auto& it: _sigma)
+ isPositive = isPositive and (it.second.array() > Constants::EPS).all();
+
+ assert(isPositive && "sigma is not positive");
+
+ MatrixComposite Q;
+ for (const auto& pair_key_sig : _sigma)
+ {
+ const auto& key = pair_key_sig.first;
+ const auto& sig_blk = pair_key_sig.second;
+
+ const auto& cov_blk = (sig_blk.array() * sig_blk.array()).matrix().asDiagonal();
+ Q.emplace(key,key,cov_blk);
+ }
+ WOLF_TRACE("prior covariance:" , Q);
+ prior_options_->cov = Q;
}
}
}
@@ -928,56 +971,51 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
if (prior_options_->mode != "nothing" and prior_options_->mode != "")
{
- prior_keyframe = emplaceKeyFrame(this->getFrameStructure(),
- this->getDim(),
- prior_options_->state,
- _ts);
+ prior_keyframe = emplaceFrame(KEY,
+ _ts,
+ prior_options_->state);
if (prior_options_->mode == "fix")
prior_keyframe->fix();
else if (prior_options_->mode == "factor")
{
- // FIXME: change whenever state is changed to a map<string,vector>
// ASSUMPTION: Independent measurements (non-correlated block-diagonal covariance matrix)
// Emplace a capture
auto prior_cap = CaptureBase::emplace<CaptureVoid>(prior_keyframe, _ts, nullptr);
// Emplace a feature and a factor for each state block
- int state_idx = 0;
- int cov_idx = 0;
- for (auto sb : prior_keyframe->getStateBlockVec())
+ for (const auto& pair_key_sb : prior_keyframe->getStateBlockMap())
{
- assert(sb != nullptr);
- assert(state_idx+sb->getSize() <= prior_options_->state.size() && "prior_options state wrong size (dimension too small)");
- assert(cov_idx+sb->getLocalSize() <= prior_options_->cov.rows() && "prior_options cov wrong size (dimension too small)");
+ const auto& key = pair_key_sb.first;
+ const auto& sb = pair_key_sb.second;
- // state block segment
- Eigen::VectorXd state_segment = prior_options_->state.segment(state_idx, sb->getSize());
- Eigen::MatrixXd cov_block = prior_options_->cov.block(cov_idx, cov_idx, sb->getLocalSize(), sb->getLocalSize());
+ const auto& state_blk = prior_options_->state.at(key);
+ const auto& covar_blk = prior_options_->cov.at(key,key);
+
+ assert(sb->getSize() == state_blk.size() && "prior_options state wrong size");
+ assert(sb->getLocalSize() == covar_blk.rows() && "prior_options cov. wrong size");
// feature
- auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "prior", state_segment, cov_block);
+ auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + key, state_blk, covar_blk);
// factor
if (sb->hasLocalParametrization())
{
if (std::dynamic_pointer_cast<StateQuaternion>(sb) != nullptr)
- auto prior_fac = FactorBase::emplace<FactorQuaternionAbsolute>(prior_fea, sb, nullptr, false);
+ auto prior_fac = FactorBase::emplace<FactorQuaternionAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
else if (std::dynamic_pointer_cast<StateAngle>(sb) != nullptr)
- auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
+ auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
else
throw std::runtime_error("not implemented...!");
}
else
{
- auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
+ auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
}
- state_idx += sb->getSize();
- cov_idx += sb->getLocalSize();
+
}
- assert(state_idx == prior_options_->state.size() && "prior_options state wrong size (dimension too big)");
- assert(cov_idx == prior_options_->cov.rows() && "prior_options cov wrong size (dimension too big)");
+
}
else
assert(prior_options_->mode == "initial_guess" && "wrong prior_options->mode");
@@ -991,17 +1029,17 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
return prior_keyframe;
}
-FrameBasePtr Problem::setPriorFactor(const Eigen::VectorXd &_state,
- const Eigen::MatrixXd &_cov,
+FrameBasePtr Problem::setPriorFactor(const VectorComposite &_state,
+ const VectorComposite &_sigma,
const TimeStamp &_ts,
const double &_time_tol)
{
- setPriorOptions("factor", _time_tol, _state, _cov);
+ setPriorOptions("factor", _time_tol, _state, _sigma);
return applyPriorOptions(_ts);
}
-FrameBasePtr Problem::setPriorFix(const Eigen::VectorXd &_state,
+FrameBasePtr Problem::setPriorFix(const VectorComposite &_state,
const TimeStamp &_ts,
const double &_time_tol)
{
@@ -1009,7 +1047,7 @@ FrameBasePtr Problem::setPriorFix(const Eigen::VectorXd &_state,
return applyPriorOptions(_ts);
}
-FrameBasePtr Problem::setPriorInitialGuess(const Eigen::VectorXd &_state,
+FrameBasePtr Problem::setPriorInitialGuess(const VectorComposite &_state,
const TimeStamp &_ts,
const double &_time_tol)
{
@@ -1087,21 +1125,6 @@ bool Problem::check(int _verbose_level) const
{
return check((_verbose_level > 0), std::cout);
}
-void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks) const
-{
- if (depth.compare("T") == 0)
- print(0, constr_by, metric, state_blocks);
- else if (depth.compare("F") == 0)
- print(1, constr_by, metric, state_blocks);
- else if (depth.compare("C") == 0)
- print(2, constr_by, metric, state_blocks);
- else if (depth.compare("f") == 0)
- print(3, constr_by, metric, state_blocks);
- else if (depth.compare("c") == 0)
- print(4, constr_by, metric, state_blocks);
- else
- print(0, constr_by, metric, state_blocks);
-}
void Problem::perturb(double amplitude)
{
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 55841ede92a1bb5dbf4d49bc190b53ab3392cb64..43f15c52d8e0eee217a4c824c67bca1e9ae5b9e3 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -138,7 +138,7 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o
_data,
_data_cov,
_capture_origin);
- cap_motion->setCalibration (_calib);
+ setCalibration (cap_motion, _calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
@@ -168,7 +168,6 @@ FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
return fac_odom;
}
-
} /* namespace wolf */
diff --git a/src/processor/processor_loopclosure.cpp b/src/processor/processor_loopclosure.cpp
index 9d8589a239466f2ba7bbd30473186c7dbdba52f7..16e037b1c7634c4d0d7509fea51e075fbc8d0066 100644
--- a/src/processor/processor_loopclosure.cpp
+++ b/src/processor/processor_loopclosure.cpp
@@ -11,7 +11,9 @@
namespace wolf
{
-ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type, int _dim, ParamsProcessorLoopClosurePtr _params_loop_closure):
+ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type,
+ int _dim,
+ ParamsProcessorLoopClosurePtr _params_loop_closure):
ProcessorBase(_type, _dim, _params_loop_closure),
params_loop_closure_(_params_loop_closure)
{
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 817dfb367f6aacee7d8be66231f5d2e7e4d56c29..a1006ed3838b21a66afaa6f25467b001e53d0dbc 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -73,7 +73,7 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
_capture_source->setOriginCapture(_capture_target);
// Get optimized calibration params from 'origin' keyframe
- VectorXd calib_preint_optim = _capture_source->getOriginCapture()->getCalibration();
+ VectorXd calib_preint_optim = getCalibration(_capture_source->getOriginCapture());
// Write the calib params into the capture before re-integration
_capture_source->setCalibrationPreint(calib_preint_optim);
@@ -107,13 +107,13 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
case FIRST_TIME_WITHOUT_KF :
{
// There is no KF: create own origin
- setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp());
+ setOrigin(getProblem()->stateZero(getStateStructure()), _incoming_ptr->getTimeStamp());
break;
}
case FIRST_TIME_WITH_KF_BEFORE_INCOMING :
{
// cannot joint to the KF: create own origin
- setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp());
+ setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
break;
}
case FIRST_TIME_WITH_KF_ON_INCOMING :
@@ -125,7 +125,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
case FIRST_TIME_WITH_KF_AFTER_INCOMING :
{
// cannot joint to the KF: create own origin
- setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp());
+ setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
break;
}
case RUNNING_WITHOUT_KF :
@@ -178,7 +178,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto capture_origin = capture_existing->getOriginCapture();
// Get calibration params for preintegration from origin, since it has chances to have optimized values
- VectorXd calib_origin = capture_origin->getCalibration();
+ VectorXd calib_origin = getCalibration(capture_origin);
// emplace a new motion capture to the new keyframe
TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
@@ -204,17 +204,24 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// modify existing feature and factor (if they exist in the existing capture)
if (!capture_existing->getFeatureList().empty())
{
- auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
-
- // Modify existing feature --------
- feature_existing->setMeasurement (capture_existing->getBuffer().back().delta_integr_);
- feature_existing->setMeasurementCovariance(capture_existing->getBuffer().back().delta_integr_cov_);
-
- // Modify existing factor --------
- // Instead of modifying, we remove one ctr, and create a new one.
- auto fac_to_remove = feature_existing->getFactorList().back(); // there is only one factor!
- auto new_ctr = emplaceFactor(feature_existing, capture_for_keyframe_callback);
- fac_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
+ // setMeasurement is not allowed: feature (and factor) will be removed and a new feature and factor will be emplaced
+ capture_existing->getFeatureList().back()->remove(); // factor is removed automatically
+
+ 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);
+
+// auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
+//
+// // Modify existing feature --------
+// feature_existing->setMeasurement (capture_existing->getBuffer().back().delta_integr_);
+// feature_existing->setMeasurementCovariance(capture_existing->getBuffer().back().delta_integr_cov_);
+//
+// // Modify existing factor --------
+// // Instead of modifying, we remove one ctr, and create a new one.
+// auto fac_to_remove = feature_existing->getFactorList().back(); // there is only one factor!
+// auto new_ctr = emplaceFactor(feature_existing, capture_for_keyframe_callback);
+// fac_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
}
break;
}
@@ -263,7 +270,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto & capture_origin = origin_ptr_;
// Get calibration params for preintegration from origin, since it has chances to have optimized values
- VectorXd calib_origin = capture_origin->getCalibration();
+ VectorXd calib_origin = getCalibration(capture_origin);
// emplace a new motion capture to the new keyframe
TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
@@ -303,9 +310,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
integrateOneStep();
// Update state and time stamps
- last_ptr_->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFrame()->setState(getProblem()->getState(getCurrentTimeStamp()));
+ const auto& ts = getTimeStamp();
+ last_ptr_->setTimeStamp( ts );
+ last_ptr_->getFrame()->setTimeStamp( ts );
+ last_ptr_->getFrame()->setState( getProblem()->getState( ts ) );
if (permittedKeyFrame() && voteForKeyFrame())
{
@@ -352,19 +360,18 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto factor = emplaceFactor(key_feature, origin_ptr_);
// create a new frame
- auto prb_ptr = getProblem();
- auto frame_new = FrameBase::createNonKeyFrame<FrameBase>(prb_ptr->getFrameStructure(),
- prb_ptr->getDim(),
- getCurrentTimeStamp(),
- prb_ptr->getCurrentState());
+ auto frame_new = getProblem()->emplaceFrame(NON_ESTIMATED,
+ getTimeStamp(),
+ getStateStructure(),
+ getProblem()->getState());
// create a new capture
auto capture_new = emplaceCapture(frame_new,
getSensor(),
key_frame->getTimeStamp(),
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
- last_ptr_->getCalibration(),
- last_ptr_->getCalibration(),
+ getCalibration(last_ptr_),
+ getCalibration(last_ptr_),
last_ptr_);
// reset the new buffer
capture_new->getBuffer().push_back( motionZero(key_frame->getTimeStamp()) ) ;
@@ -389,55 +396,164 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
postProcess();
}
-// _x needs to have the size of the processor state
-bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
+VectorComposite ProcessorMotion::getState() const
{
- CaptureMotionPtr capture_motion;
- if (origin_ptr_ && _ts >= origin_ptr_->getTimeStamp())
+
+ if (last_ptr_ == nullptr or last_ptr_->getFrame() == nullptr) // We do not have any info of where to find a valid state
+ // Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
{
- // timestamp found in the current processor buffer
- capture_motion = last_ptr_;
+ WOLF_ERROR("Processor has no state. Returning an empty VectorComposite with no blocks");
+ return VectorComposite(); // return empty state
}
- else
+
+
+ // From here on, we do have info to compute a valid state
+
+ // if buffer is empty --> we did not advance from origin!
+ // this may happen when in the very first frame where the capture has no motion info --> empty buffer
+ if (last_ptr_->getBuffer().empty())
{
- // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
- capture_motion = findCaptureContainingTimeStamp(_ts);
+ return last_ptr_->getFrame()->getState(state_structure_);
}
- if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp
+ /* Doing this:
+ *
+ * x_t = x_0 [+] ( D_0t (+) J_D_c * ( c - c_preint ) )
+ *
+ * or, put in code form:
+ *
+ * _state = state_origin [+] (delta_preint (+) Jac_delta_calib * (calib - calib_preint) )
+ *
+ * with
+ * [+] : group composition
+ * (+) : block-wise plus
+ */
+
+ // Get state of origin
+ const auto& x_origin = getOrigin()->getFrame()->getState();
+
+ // Get most rescent motion
+ const auto& motion = last_ptr_->getBuffer().back();
+
+ // Get delta preintegrated up to now
+ const auto& delta_preint = motion.delta_integr_;
+
+ // Get calibration preint -- stored in last capture
+ const auto& calib_preint = last_ptr_->getCalibrationPreint();
+
+ VectorComposite state;
+ if ( hasCalibration() )
{
- // Get origin state and calibration
- CaptureBasePtr cap_orig = capture_motion->getOriginCapture();
- VectorXd state_0 = cap_orig->getFrame()->getState(state_structure_);
- VectorXd calib = cap_orig->getCalibration();
+ // Get current calibration -- from origin capture
+ const auto& calib = getCalibration(origin_ptr_);
- // Get delta and correct it with new calibration params
- VectorXd calib_preint = capture_motion->getCalibrationPreint();
- Motion motion = capture_motion->getBuffer().getMotion(_ts);
+ // get Jacobian of delta wrt calibration
+ const auto& J_delta_calib = motion.jacobian_calib_;
- VectorXd delta_step = motion.jacobian_calib_ * (calib - calib_preint);
- VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step);
+ // compute delta change
+ const auto& delta_step = J_delta_calib * (calib - calib_preint);
- // ensure proper size of the provided reference
- _x.resize( state_0.size() );
+ // correct delta // this is (+)
+ const auto& delta_corrected = correctDelta(delta_preint, delta_step);
- // Compose on top of origin state using the buffered time stamp, not the query time stamp
- double dt = motion.ts_ - capture_motion->getBuffer().front().ts_;
- statePlusDelta(state_0, delta, dt, _x);
+ // compute current state // this is [+]
+ statePlusDelta(x_origin, delta_corrected, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), state);
}
else
{
- // We could not find any CaptureMotion for the time stamp requested
- WOLF_ERROR("Could not find any Capture for the time stamp requested. ", _ts);
- WOLF_TRACE("Did you forget to call Problem::setPrior() in your application?")
- return false;
+ statePlusDelta(x_origin, delta_preint, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), state);
+ }
+
+ return state;
+}
+
+
+
+// _x needs to have the size of the processor state
+VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const
+{
+
+ // We need to search for the capture containing a motion buffer with the queried time stamp
+ CaptureMotionPtr capture_motion = findCaptureContainingTimeStamp(_ts);
+
+ if (capture_motion == nullptr) // we do not have any info of where to find a valid state
+ {
+ WOLF_ERROR("Processor has no state. Returning an empty VectorComposite with no blocks");
+ return VectorComposite(); // return empty state
+ }
+
+
+ else // We found a CaptureMotion whose buffer contains the time stamp
+ {
+ // if buffer is empty --> we did not advance from origin!
+ // this may happen when in the very first frame where the capture has no motion info --> empty buffer
+ if (capture_motion->getBuffer().empty())
+ {
+ return capture_motion->getFrame()->getState(state_structure_);
+ }
+
+ /* Doing this:
+ *
+ * x_t = x_0 [+] ( D_0t (+) J_D_c * ( c - c_preint ) )
+ *
+ * or, put in code form:
+ *
+ * _state = state_origin [+] (delta_preint (+) Jac_delta_calib * (calib - calib_preint) )
+ *
+ * with
+ * [+] : group composition
+ * (+) : block-wise plus
+ */
+
+ // Get state of origin
+ CaptureBasePtr cap_orig = capture_motion->getOriginCapture();
+ const auto& x_origin = cap_orig->getFrame()->getState();
+
+ // Get motion at time stamp
+ const auto& motion = capture_motion->getBuffer().getMotion(_ts);
+
+ // Get delta preintegrated up to time stamp
+ const auto& delta_preint = motion.delta_integr_;
+
+ // Get calibration preint -- stored in last capture
+ const auto& calib_preint = capture_motion->getCalibrationPreint();
+
+ VectorComposite state;
+
+ if ( hasCalibration() )
+ {
+ // Get current calibration -- from origin capture
+ const auto& calib = getCalibration(cap_orig);
+
+ // get Jacobian of delta wrt calibration
+ const auto& J_delta_calib = motion.jacobian_calib_;
+
+ // compute delta change
+ const auto& delta_step = J_delta_calib * (calib - calib_preint);
+
+ // correct delta // this is (+)
+ const auto& delta_corrected = correctDelta(delta_preint, delta_step);
+
+ // compute current state // this is [+]
+ statePlusDelta(x_origin, delta_corrected, _ts - cap_orig->getTimeStamp(), state);
+ }
+ else
+ {
+ statePlusDelta(x_origin, delta_preint, _ts - cap_orig->getTimeStamp(), state);
+ }
+
+ // return success
+ return state;
+
}
- return true;
}
-FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin)
+FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin)
{
- FrameBasePtr key_frame_ptr = getProblem()->emplaceKeyFrame(_x_origin, _ts_origin);
+ FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY,
+ _ts_origin,
+ getStateStructure(),
+ _x_origin);
setOrigin(key_frame_ptr);
return key_frame_ptr;
@@ -465,11 +581,10 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ----------
// Make non-key-frame for last Capture
- auto prb_ptr = getProblem();
- auto new_frame_ptr = FrameBase::createNonKeyFrame<FrameBase>(prb_ptr->getFrameStructure(),
- prb_ptr->getDim(),
- origin_ts,
- _origin_frame->getState());
+ auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
+ origin_ts,
+ getStateStructure(),
+ _origin_frame->getState());
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(new_frame_ptr,
@@ -515,7 +630,7 @@ void ProcessorMotion::integrateOneStep()
jacobian_delta_);
// integrate Jacobian wrt calib
- if ( hasCalibration() )
+ if ( hasCalibration() ) // if no calibration, matrices can have mismatching sizes, and this computation is nevertheless pointless
{
jacobian_calib_.noalias()
= jacobian_delta_preint_ * getBuffer().back().jacobian_calib_
@@ -529,15 +644,15 @@ void ProcessorMotion::integrateOneStep()
// push all into buffer
getBuffer().emplace_back(incoming_ptr_->getTimeStamp(),
- incoming_ptr_->getData(),
- incoming_ptr_->getDataCovariance(),
- delta_,
- delta_cov_,
- delta_integrated_,
- delta_integrated_cov_,
- jacobian_delta_,
- jacobian_delta_preint_,
- jacobian_calib_);
+ incoming_ptr_->getData(),
+ incoming_ptr_->getDataCovariance(),
+ delta_,
+ delta_cov_,
+ delta_integrated_,
+ delta_integrated_cov_,
+ jacobian_delta_,
+ jacobian_delta_preint_,
+ jacobian_calib_);
}
void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
@@ -621,7 +736,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
{
// Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
capture_motion = std::static_pointer_cast<CaptureMotion>(capture);
- if (_ts < frame->getTimeStamp() and !capture_motion->getBuffer().empty() and _ts >= capture_motion->getBuffer().front().ts_)
+
+ if (capture_motion->containsTimeStamp(_ts, params_->time_tolerance))
{
// Found time stamp satisfying rule 3 above !! ==> break for loop
break;
@@ -677,17 +793,15 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
if (pack)
{
if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
- {
- WOLF_WARN("||*||");
- WOLF_INFO(" ... It seems you missed something!");
- WOLF_ERROR("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
- // throw std::runtime_error("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
+
processing_step_ = RUNNING_WITH_KF_ON_ORIGIN;
- }
+
else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
+
processing_step_ = RUNNING_WITH_KF_BEFORE_ORIGIN;
else
+
processing_step_ = RUNNING_WITH_KF_AFTER_ORIGIN;
}
@@ -710,6 +824,20 @@ bool ProcessorMotion::storeCapture(CaptureBasePtr _cap_ptr)
return false;
}
+TimeStamp ProcessorMotion::getTimeStamp ( ) const
+{
+ if (not last_ptr_)
+ {
+ WOLF_ERROR("Processor has no time stamp. Returning a non-valid timestamp equal to 0");
+ return TimeStamp(0);
+ }
+
+ if (getBuffer().empty())
+ return last_ptr_->getTimeStamp();
+
+ return getBuffer().back().ts_;
+}
+
void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
_stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
@@ -723,4 +851,5 @@ void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, boo
_stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
}
-}
+
+} // namespace wolf
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 931efc2e3ea522f4afde3a1647e329d26507859d..284090e238f3c75327424cb76f3946e277e23bdf 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -1,6 +1,7 @@
#include "core/processor/processor_odom_2d.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/math/covariance.h"
+#include "core/state_block/state_composite.h"
namespace wolf
{
@@ -84,15 +85,16 @@ void ProcessorOdom2d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen
_jacobian2.topLeftCorner<2, 2>() = Eigen::Rotation2Dd(_delta1(2)).matrix();
}
-void ProcessorOdom2d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, const double _Dt,
- Eigen::VectorXd& _x_plus_delta) const
+void ProcessorOdom2d::statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const
{
- assert(_x.size() == x_size_ && "Wrong _x vector size");
- assert(_x_plus_delta.size() == x_size_ && "Wrong _x_plus_delta vector size");
+ assert(_delta.size() == delta_size_ && "Wrong _x_plus_delta vector size");
// This is just a frame composition in 2d
- _x_plus_delta.head<2>() = _x.head<2>() + Eigen::Rotation2Dd(_x(2)).matrix() * _delta.head<2>();
- _x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
+ _x_plus_delta["P"] = _x.at("P") + Eigen::Rotation2Dd(_x.at("O")(0)).matrix() * _delta.head<2>();
+ _x_plus_delta["O"] = Vector1d(pi2pi(_x.at("O")(0) + _delta(2)));
}
bool ProcessorOdom2d::voteForKeyFrame() const
@@ -133,7 +135,7 @@ CaptureMotionPtr ProcessorOdom2d::emplaceCapture(const FrameBasePtr& _frame_own,
const CaptureBasePtr& _capture_origin_ptr)
{
auto cap_motion = CaptureBase::emplace<CaptureOdom2d>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
- cap_motion->setCalibration(_calib);
+ setCalibration(cap_motion, _calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index d8f2781f4667989b155ad65e5640a01e4aa52db3..5b01e98b3ad8fbbe6f31bedc4f25b1a9cfe76747 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -1,4 +1,6 @@
#include "core/processor/processor_odom_3d.h"
+#include "core/math/SE3.h"
+
namespace wolf
{
@@ -138,23 +140,20 @@ void ProcessorOdom3d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen
dq_out = dq1 * dq2;
}
-void ProcessorOdom3d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, const double _Dt,
- Eigen::VectorXd& _x_plus_delta) const
+void ProcessorOdom3d::statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const
{
- assert(_x.size() >= x_size_ && "Wrong _x vector size"); //we need a state vector which size is at least x_size_
assert(_delta.size() == delta_size_ && "Wrong _delta vector size");
- assert(_x_plus_delta.size() >= x_size_ && "Wrong _x_plus_delta vector size");
-
- Eigen::Map<const Eigen::Vector3d> p (_x.data() );
- Eigen::Map<const Eigen::Quaterniond> q (_x.data() + 3 );
- Eigen::Map<const Eigen::Vector3d> dp (_delta.data() );
- Eigen::Map<const Eigen::Quaterniond> dq (_delta.data() + 3 );
- Eigen::Map<Eigen::Vector3d> p_out (_x_plus_delta.data() );
- Eigen::Map<Eigen::Quaterniond> q_out (_x_plus_delta.data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> p (_x.at("P").data() );
+ Eigen::Map<const Eigen::Quaterniond> q (_x.at("O").data() );
+ Eigen::Map<const Eigen::Vector3d> dp (_delta.data() );
+ Eigen::Map<const Eigen::Quaterniond> dq (_delta.data() + 3 );
- p_out = p + q * dp;
- q_out = q * dq;
+ _x_plus_delta["P"] = p + q * dp;
+ _x_plus_delta["O"] = (q * dq).coeffs();
}
@@ -207,7 +206,7 @@ CaptureMotionPtr ProcessorOdom3d::emplaceCapture(const FrameBasePtr& _frame_own,
const CaptureBasePtr& _capture_origin)
{
auto cap_motion = CaptureBase::emplace<CaptureOdom3d>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin);
- cap_motion->setCalibration(_calib);
+ setCalibration(cap_motion, _calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
@@ -222,6 +221,14 @@ FeatureBasePtr ProcessorOdom3d::emplaceFeature(CaptureMotionPtr _capture_motion)
return key_feature_ptr;
}
+Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
+{
+ VectorXd delta_corrected(7);
+ SE3::plus(delta_preint, delta_step, delta_corrected);
+ return delta_corrected;
+}
+
FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
auto fac_odom = FactorBase::emplace<FactorOdom3d>(_feature_motion,
@@ -232,12 +239,21 @@ FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, Cap
return fac_odom;
}
+VectorXd ProcessorOdom3d::getCalibration (const CaptureBasePtr _capture) const
+{
+ return VectorXd::Zero(0);
+}
+void ProcessorOdom3d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
}
+} // namespace wolf
+
// Register in the FactorySensor
#include "core/processor/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR(ProcessorOdom3d);
WOLF_REGISTER_PROCESSOR_AUTO(ProcessorOdom3d);
} // namespace wolf
+
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 18464e22ea8c904e2b091aed51f1d2e2d42649a5..be506f6da3a3369578164002b2f6a21590ba1c6f 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -15,6 +15,7 @@ namespace wolf
{
ProcessorTracker::ProcessorTracker(const std::string& _type,
+ const StateStructure& _structure,
int _dim,
ParamsProcessorTrackerPtr _params_tracker) :
ProcessorBase(_type, _dim, _params_tracker),
@@ -26,6 +27,7 @@ ProcessorTracker::ProcessorTracker(const std::string& _type,
origin_frame_ptr_(nullptr),
last_frame_ptr_(nullptr),
incoming_frame_ptr_(nullptr)
+ state_structure_(_structure)
{
//
}
@@ -81,7 +83,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_PACK" );
- FrameBasePtr kfrm = getProblem()->emplaceKeyFrame(incoming_ptr_->getTimeStamp());
+ FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY,
+ incoming_ptr_->getTimeStamp(),
+ getStateStructure(),
+ getProblem()->getState(getStateStructure()));
incoming_ptr_->link(kfrm);
// Process info
@@ -110,12 +115,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
case SECOND_TIME_WITHOUT_PACK :
{
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
- auto prb_ptr = getProblem();
- auto incoming_ts = incoming_ptr_->getTimeStamp();
- FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(prb_ptr->getFrameStructure(),
- prb_ptr->getDim(),
- incoming_ts,
- prb_ptr->getState(incoming_ts));
+
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
+ incoming_ptr_->getTimeStamp());
incoming_ptr_->link(frm);
incoming_frame_ptr_ = frm;
// We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
@@ -153,12 +155,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
last_old_frame->remove();
// Create new frame
- auto prb_ptr = getProblem();
- auto incoming_ts = incoming_ptr_->getTimeStamp();
- FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(prb_ptr->getFrameStructure(),
- prb_ptr->getDim(),
- incoming_ts,
- prb_ptr->getState(incoming_ts));
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
+ incoming_ptr_->getTimeStamp());
incoming_ptr_->link(frm);
incoming_frame_ptr_ = frm;
@@ -211,11 +209,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
resetDerived();
// make F; append incoming to new F
- auto prb_ptr = getProblem();
- FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(prb_ptr->getFrameStructure(),
- prb_ptr->getDim(),
- incoming_ptr_->getTimeStamp(),
- last_ptr_->getFrame()->getState());
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
+ incoming_ptr_->getTimeStamp(),
+ last_ptr_->getFrame()->getStateVector());
incoming_ptr_->link(frm);
incoming_frame_ptr_ = frm;
@@ -266,11 +262,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
advanceDerived();
// Replace last frame for a new one in incoming
- auto prb_ptr = getProblem();
- FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(prb_ptr->getFrameStructure(),
- prb_ptr->getDim(),
- incoming_ptr_->getTimeStamp(),
- last_ptr_->getFrame()->getState());
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
+ incoming_ptr_->getTimeStamp(),
+ last_ptr_->getFrame()->getStateVector());
incoming_ptr_->link(frm);
incoming_frame_ptr_ = frm;
last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 5b3abbf52bbc5715b9613635494a1719f94f17e8..3804962f62fcbfba13b56e7c64866be7497eeaa8 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -11,9 +11,10 @@ namespace wolf
{
ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type,
+ const StateStructure& _structure,
int _dim,
ParamsProcessorTrackerFeaturePtr _params_tracker_feature) :
- ProcessorTracker(_type, _dim, _params_tracker_feature),
+ ProcessorTracker(_type, _structure, _dim, _params_tracker_feature),
params_tracker_feature_(_params_tracker_feature)
{
}
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 0e593a917d9c7715cb16a62a43154cbd53992d58..ec16f76dc095de7cf01ebf45400d0eacd7733516 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -14,8 +14,12 @@ namespace wolf
{
ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
+ const StateStructure& _structure,
ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark) :
- ProcessorTracker(_type, 0, _params_tracker_landmark),
+ ProcessorTracker(_type,
+ _structure,
+ 0,
+ _params_tracker_landmark),
params_tracker_landmark_(_params_tracker_landmark)
{
//
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index e96a3afdfe1a4618baf113f0114ac4f0330eb974..5ccf914d7aac8e0aa74b38adb3adffe973494b19 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -184,9 +184,9 @@ void SensorBase::addPriorParameter(const std::string& _key, const Eigen::VectorX
// create & add factor absolute
if (is_quaternion)
- FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, _sb, nullptr, false);
+ FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, ftr_prior, _sb, nullptr, false);
else
- FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, _sb, _start_idx, _size, nullptr, false);
+ FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, ftr_prior, _sb, _start_idx, _size, nullptr, false);
// store feature in params_prior_map_
params_prior_map_[_key] = ftr_prior;
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 03634eb0938d9ae7c6a20b5d5a5d11a201989101..afdc43e02a8085feb99c4ae0ac1a9a4cdefe2162 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -22,7 +22,7 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
getIntrinsic()->setState(Eigen::Vector3d(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
unfixIntrinsics();
- std::cout << "Prior cov diag " << params_diff_drive_->prior_cov_diag.transpose() << std::endl;
+
if(params_diff_drive_->set_intrinsics_prior)
addPriorIntrinsics(getIntrinsic()->getState(), params_diff_drive_->prior_cov_diag.asDiagonal());
@@ -31,17 +31,13 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
double sigma_rev = 2*radians_per_tick;
Eigen::Vector2d noise_sigma; noise_sigma << sigma_rev, sigma_rev;
-// // 2. unmeasured lateral wheel slippage: sigma = 1mm
-// double sigma_slippage = 0.001;
-// Eigen::Vector3d noise_sigma; noise_sigma << sigma_rev, sigma_rev, sigma_slippage;
-
setNoiseStd(noise_sigma);
}
SensorDiffDrive::~SensorDiffDrive()
{
- // TODO Auto-generated destructor stub
+ // Auto-generated destructor stub
}
diff --git a/src/state_block/local_parametrization_base.cpp b/src/state_block/local_parametrization_base.cpp
index f46a93f9c488d0b899e94df156f3faff04a63554..aee3f704126bf4ba81e9658b3c337ded088c78a5 100644
--- a/src/state_block/local_parametrization_base.cpp
+++ b/src/state_block/local_parametrization_base.cpp
@@ -22,3 +22,14 @@ unsigned int LocalParametrizationBase::getGlobalSize() const
}
} // namespace wolf
+
+bool wolf::LocalParametrizationBase::plus(const Eigen::VectorXd &_x,
+ const Eigen::VectorXd &_delta,
+ Eigen::VectorXd &_x_plus_delta) const
+{
+ Eigen::Map<const Eigen::VectorXd> x(_x.data(), _x.size());
+ Eigen::Map<const Eigen::VectorXd> delta(_delta.data(), _delta.size());
+ Eigen::Map<Eigen::VectorXd> x_plus_delta(_x_plus_delta.data(), _x_plus_delta.size());
+
+ return plus(x, delta, x_plus_delta);
+}
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index 030febd46ba50917d8681bd2ce3bb6ee8be55e23..ea2c9a4cd4f32b4a1c49e1bfec6ad28efb397cbe 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -26,31 +26,22 @@ void StateBlock::setFixed(bool _fixed)
fixed_.store(_fixed);
}
-//void StateBlock::addToProblem(ProblemPtr _problem_ptr)
-//{
-// _problem_ptr->addStateBlock(shared_from_this());
-//}
-//
-//void StateBlock::removeFromProblem(ProblemPtr _problem_ptr)
-//{
-// _problem_ptr->removeStateBlock(shared_from_this());
-//}
-
void StateBlock::perturb(double amplitude)
{
using namespace Eigen;
VectorXd perturbation(VectorXd::Random(getLocalSize()) * amplitude);
+ this->plus(perturbation);
+}
+
+void StateBlock::plus(const Eigen::VectorXd &_dv)
+{
if (local_param_ptr_ == nullptr)
- state_ += perturbation;
+ setState(getState() + _dv);
else
{
- VectorXd state_perturbed(getSize());
- // Note: LocalParametrizationBase::plus() works with Eigen::Map only. Build all necessary maps:
- Map<const VectorXd> state_map(state_.data(), getSize());
- Map<const VectorXd> perturbation_map(perturbation.data(), getLocalSize());
- Map<VectorXd> state_perturbed_map(state_perturbed.data(), getSize());
- local_param_ptr_->plus(state_map, perturbation_map, state_perturbed_map);
- state_ = state_perturbed;
+ Eigen::VectorXd state(getSize());
+ local_param_ptr_->plus(getState(), _dv, state);
+ setState(state);
}
}
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..612b8e53ec55d7e5b787a5aa0c49312d21dfe4a9
--- /dev/null
+++ b/src/state_block/state_composite.cpp
@@ -0,0 +1,1010 @@
+
+
+
+#include "core/state_block/state_composite.h"
+#include "core/state_block/state_block.h"
+
+namespace wolf{
+
+////// VECTOR COMPOSITE //////////
+
+VectorComposite::VectorComposite(const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ auto size_it = _sizes.begin();
+ for ( const auto& ckey : _structure)
+ {
+ const auto& key = string(1,ckey);
+ const auto& size = *size_it;
+
+ this->emplace(key, VectorXd(size).setZero());
+
+ size_it ++;
+ }
+}
+
+VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ int index = 0;
+ auto size_it = _sizes.begin();
+ for ( const auto& ckey : _structure)
+ {
+ const auto& key = string(1,ckey);
+ const auto& size = *size_it;
+
+ (*this)[key] = _v.segment(index,size);
+
+ index += size;
+ size_it ++;
+ }
+}
+
+VectorComposite::VectorComposite (const StateStructure& _s)
+{
+ for (const auto& ckey : _s)
+ {
+ const auto& key = string(1,ckey); // ckey is char
+ this->emplace(key,VectorXd(0));
+ }
+}
+
+VectorComposite::VectorComposite (const StateStructure& _structure, const std::list<VectorXd>& _vectors)
+{
+ assert(_structure.size() == _vectors.size() && "Structure and vector list size mismatch");
+
+ auto vector_it = _vectors.begin();
+
+ for (const auto& ckey : _structure)
+ {
+ string key(1,ckey);
+ this->emplace(key, *vector_it);
+ vector_it++;
+ }
+}
+
+
+unsigned int VectorComposite::size(const StateStructure &_structure) const
+{
+ unsigned int size = 0;
+ for (const auto& ckey : _structure)
+ {
+ std::string key(1,ckey); // ckey is char
+ const VectorXd& v = this->at(key);
+ size += v.size();
+ }
+ return size;
+}
+
+Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
+{
+ // traverse once with unordered_map access
+ std::vector<const VectorXd*> vp;
+ unsigned int size = 0;
+ for (const auto& ckey : _structure)
+ {
+ std::string key(1,ckey); // ckey is char
+ vp.push_back(&(this->at(key)));
+ size += vp.back()->size();
+ }
+
+ Eigen::VectorXd x(size);
+
+ // traverse once linearly
+ unsigned int index = 0;
+ for (const auto& blkp : vp)
+ {
+ x.segment(index, blkp->size()) = *blkp;
+ index += blkp->size();
+ }
+ return x;
+}
+
+std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x)
+{
+ for (const auto &pair_key_vec : _x)
+ {
+ const auto &key = pair_key_vec.first;
+ const auto &vec = pair_key_vec.second;
+ _os << key << ": (" << vec.transpose() << "); ";
+ }
+ return _os;
+}
+
+wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y)
+{
+ wolf::VectorComposite xpy;
+ for (const auto& pair_i_xi : _x)
+ {
+ const auto& i = pair_i_xi.first;
+
+ xpy.emplace(i, _x.at(i) + _y.at(i));
+ }
+ return xpy;
+}
+
+wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y)
+{
+ wolf::VectorComposite xpy;
+ for (const auto& pair_i_xi : _x)
+ {
+ const auto& i = pair_i_xi.first;
+
+ xpy.emplace(i, _x.at(i) - _y.at(i));
+ }
+ return xpy;
+}
+
+void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ int index = 0;
+ auto size_it = _sizes.begin();
+ for ( const auto& ckey : _structure)
+ {
+ const auto& key = string(1,ckey); // ckey is char
+ const auto& size = *size_it;
+
+ (*this)[key] = _v.segment(index,size);
+
+ index += size;
+ size_it ++;
+ }
+}
+
+void VectorComposite::setZero()
+{
+ for (auto& pair_key_vec : *this)
+ pair_key_vec.second.setZero();
+}
+
+wolf::VectorComposite operator -(const wolf::VectorComposite &_x)
+{
+ wolf::VectorComposite m;
+ for (const auto& pair_i_xi : _x)
+ {
+ const auto& i = pair_i_xi.first;
+ m.emplace(i, - _x.at(i));
+ }
+ return m;
+}
+
+////// MATRIX COMPOSITE //////////
+
+bool MatrixComposite::emplace(const std::string &_row, const std::string &_col, const Eigen::MatrixXd &_mat_blk)
+{
+ // check rows
+ if (size_rows_.count(_row) == 0)
+ size_rows_[_row] = _mat_blk.rows();
+ else
+ assert(size_rows_.at(_row) == _mat_blk.rows() && "Provided matrix block has wrong number of rows!");
+
+ // check cols
+ if (size_cols_.count(_col) == 0)
+ size_cols_[_col] = _mat_blk.cols();
+ else
+ assert(size_rows_.at(_row) == _mat_blk.rows() && "Provided matrix block has wrong number of rows!");
+
+ // now it's safe to use [] operator. the first one is a function-like call to [] -- just weird for this->[]
+ this->operator[](_row)[_col] = _mat_blk;
+ return true;
+}
+
+bool MatrixComposite::at(const std::string &_row, const std::string &_col, Eigen::MatrixXd &_mat_blk) const
+{
+ const auto &row_it = this->find(_row);
+ if(row_it != this->end())
+ return false;
+
+ const auto &col_it = row_it->second.find(_col);
+ if(col_it != row_it->second.end())
+ return false;
+
+ _mat_blk = col_it->second;
+
+ return true;
+}
+
+Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string &_col)
+{
+ const auto &row_it = this->find(_row);
+ assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
+
+ const auto &col_it = row_it->second.find(_col);
+ assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
+
+ return col_it->second;
+}
+
+const Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string &_col) const
+{
+ const auto &row_it = this->find(_row);
+ assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
+
+ const auto &col_it = row_it->second.find(_col);
+ assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
+
+ return col_it->second;
+}
+
+wolf::MatrixComposite MatrixComposite::operator *(const MatrixComposite &_N) const
+{
+ MatrixComposite MN;
+ for (const auto &pair_i_Mi : (*this))
+ {
+ const auto &i = pair_i_Mi.first;
+ const auto &Mi = pair_i_Mi.second;
+
+ for (const auto &pair_k_Nk : _N)
+ {
+ const auto &k = pair_k_Nk.first;
+ const auto &Nk = pair_k_Nk.second;
+
+ for (const auto &pair_j_Nkj : Nk)
+ {
+ const auto &j = pair_j_Nkj.first;
+ const auto &Nkj = pair_j_Nkj.second;
+ const auto &Mik = Mi.at(k);
+
+ if (MN.count(i, j) == 0)
+ MN.emplace(i, j, Mik * Nkj);
+ else
+ MN.at(i, j) += Mik * Nkj;
+ }
+ }
+ }
+ return MN;
+}
+
+wolf::MatrixComposite MatrixComposite::operator +(const MatrixComposite &_N) const
+{
+ MatrixComposite MpN;
+ for (const auto &pair_i_Mi : *this)
+ {
+ const auto &i = pair_i_Mi.first;
+ const auto &Mi = pair_i_Mi.second;
+
+ for (const auto& pair_j_Mij : Mi)
+ {
+ const auto& j = pair_j_Mij.first;
+ const auto& Mij = pair_j_Mij.second;
+ const auto& Nij = _N.at(i,j);
+
+ MpN.emplace(i, j, Mij + Nij);
+ }
+ }
+ return MpN;
+}
+
+wolf::MatrixComposite MatrixComposite::operator -(const MatrixComposite &_N) const
+{
+ MatrixComposite MpN;
+ for (const auto &pair_i_Mi : *this)
+ {
+ const auto &i = pair_i_Mi.first;
+ const auto &Mi = pair_i_Mi.second;
+
+ for (const auto& pair_j_Mij : Mi)
+ {
+ const auto& j = pair_j_Mij.first;
+ const auto& Mij = pair_j_Mij.second;
+ const auto& Nij = _N.at(i,j);
+
+ MpN.emplace(i, j, Mij - Nij);
+ }
+ }
+ return MpN;
+}
+
+MatrixComposite MatrixComposite::operator - (void) const
+{
+ MatrixComposite m;
+
+ for (const auto& pair_rkey_row : *this)
+ {
+ m.unordered_map<string,unordered_map<string, MatrixXd>>::emplace(pair_rkey_row.first, unordered_map<string, MatrixXd>());
+ for (const auto& pair_ckey_blk : pair_rkey_row.second)
+ {
+ m[pair_rkey_row.first].emplace(pair_ckey_blk.first, - pair_ckey_blk.second);
+ }
+ }
+ return m;
+}
+
+
+wolf::VectorComposite MatrixComposite::operator *(const VectorComposite &_x) const
+{
+ VectorComposite y;
+ for (const auto &pair_rkey_row : *this)
+ {
+ const auto &rkey = pair_rkey_row.first;
+ const auto &row = pair_rkey_row.second;
+
+ for (const auto &pair_ckey_mat : row)
+ {
+ const auto &ckey = pair_ckey_mat.first;
+ const auto &J_r_c = pair_ckey_mat.second;
+
+ const auto& it = y.find(rkey);
+ if (it != y.end())
+ it->second += J_r_c * _x.at(ckey);
+ else
+ y.emplace(rkey, J_r_c * _x.at(ckey));
+ }
+ }
+ return y;
+}
+
+MatrixComposite MatrixComposite::propagate(const MatrixComposite& _Cov)
+{
+
+
+ // simplify names of operands
+ const auto& J = *this;
+ const auto& Q = _Cov;
+
+ MatrixComposite S; // S = J * Q * J.tr
+
+
+ /* Covariance propagation
+ *
+ * 1. General formula for the output block S(i,j)
+ *
+ * S_ij = sum_n (J_in * sum_k(Q_nk * J_jk^T)) (A)
+ *
+ * which develops as
+ *
+ * S_ij = sum_n (J_in * QJt_nj)
+ *
+ * with:
+ *
+ * QJt_nj = sum_k (Q_nk * J_jk^T) (B)
+ *
+ *
+ * 2. Algorithm to accomplish the above:
+ *
+ * for i = 1 : S.rows = J.rows (1) // iterate for i and j
+ * {
+ * J_i = J[i]
+ * for j = i : S.cols = J.rows (2) // j starts at i: do not loop over the symmetrical blocks
+ * {
+ * S_ij = 0 (3) // start formula (A) for S_ij
+ * for n = 1 : Q.rows (4)
+ * {
+ * J_in = J[i][n]
+ * QJt_nj = 0 (5) // start formula (B) for QJt_nj
+ * for k = 1 : Q.cols (6)
+ * {
+ * J_jk = J[j][k]
+ * QJt_nj += Q_nk * J_jk^T (7) // sum on QJt_nj
+ * }
+ * S_ij += J_in * QJt_nj (8) // sum on S_ij
+ * }
+ * S[i][j] = S_ij // write block in output composite
+ * if (i != j)
+ * S[j][i] = S_ij^T (9) // write symmetrical block in output composite
+ * }
+ * }
+ */
+
+ // Iterate on the output matrix S first, row i, col j.
+ for (const auto& pair_i_Si : J) // (1)
+ {
+ const auto& i = pair_i_Si.first;
+ const auto& J_i = pair_i_Si.second;
+
+ double i_size = J_i.begin()->second.rows();
+
+ for (const auto& pair_j_Sj : J) // (2)
+ {
+ const auto& j = pair_j_Sj.first;
+ const auto& J_j = pair_j_Sj.second;
+
+ double j_size = J_j.begin()->second.rows();
+
+ MatrixXd S_ij(MatrixXd::Zero(i_size, j_size)); // (3)
+
+ for (const auto& pair_n_Qn : Q) // (4)
+ {
+ const auto& n = pair_n_Qn.first;
+ const auto& Q_n = pair_n_Qn.second;
+
+ double n_size = Q_n.begin()->second.rows();
+
+ const auto& J_in = J_i.at(n);
+
+ MatrixXd QJt_nj(MatrixXd::Zero(n_size, j_size)); // (5)
+
+ for (const auto& pair_k_Qnk : Q_n) // (6)
+ {
+ const auto& k = pair_k_Qnk.first;
+ const auto& Q_nk = pair_k_Qnk.second;
+
+ const auto& J_jk = J_j.at(k);
+
+ QJt_nj += Q_nk * J_jk.transpose(); // (7)
+ }
+
+ S_ij += J_in * QJt_nj; // (8)
+ }
+
+ S.emplace(i,j,S_ij);
+ if (j == i)
+ break; // avoid computing the symmetrical block!
+ else
+ S.emplace(j, i, S_ij.transpose()); // (9)
+ }
+ }
+
+ return S;
+}
+
+MatrixComposite MatrixComposite::operator * (double scalar_right) const
+{
+ MatrixComposite S(*this);
+ for (const auto& pair_rkey_row : *this)
+ {
+ const auto& rkey = pair_rkey_row.first;
+ for (const auto& pair_ckey_block : pair_rkey_row.second)
+ {
+ const auto ckey = pair_ckey_block.first;
+ S[rkey][ckey] *= scalar_right;
+ }
+ }
+ return S;
+}
+
+MatrixComposite operator * (double scalar_left, const MatrixComposite& M)
+{
+ MatrixComposite S;
+ S = M * scalar_left;
+ return S;
+}
+
+MatrixXd MatrixComposite::matrix(const StateStructure &_struct_rows, const StateStructure &_struct_cols) const
+{
+
+ std::unordered_map < string, unsigned int> indices_rows;
+ std::unordered_map < string, unsigned int> indices_cols;
+ unsigned int rows, cols;
+
+ sizeAndIndices(_struct_rows, _struct_cols, indices_rows, indices_cols, rows, cols);
+
+ MatrixXd M ( MatrixXd::Zero(rows, cols) );
+
+ for (const auto& pair_row_rband : (*this))
+ {
+ const auto& row = pair_row_rband.first;
+ const auto& rband = pair_row_rband.second;
+ for (const auto& pair_col_blk : rband)
+ {
+ const auto& col = pair_col_blk.first;
+ const auto& blk = pair_col_blk.second;
+
+ const unsigned int & blk_rows = size_rows_.at(row);
+ const unsigned int & blk_cols = size_cols_.at(col);
+
+ assert(blk.rows() == blk_rows && "MatrixComposite block size mismatch! Wrong number of rows.");
+ assert(blk.cols() == blk_cols && "MatrixComposite block size mismatch! Wrong number of cols.");
+
+ M.block(indices_rows.at(row), indices_cols.at(col), blk_rows, blk_cols) = blk;
+
+ }
+ }
+
+ return M;
+}
+
+unsigned int MatrixComposite::rows() const
+{
+ unsigned int rows = 0;
+ for (const auto& pair_row_size : this->size_rows_)
+ rows += pair_row_size.second;
+ return rows;
+}
+
+unsigned int MatrixComposite::cols() const
+{
+ unsigned int cols = 0;
+ for (const auto& pair_col_size : this->size_rows_)
+ cols += pair_col_size.second;
+ return cols;
+}
+
+void MatrixComposite::sizeAndIndices(const StateStructure &_struct_rows,
+ const StateStructure &_struct_cols,
+ std::unordered_map<string, unsigned int> &_indices_rows,
+ std::unordered_map<string, unsigned int> &_indices_cols,
+ unsigned int &_rows,
+ unsigned int &_cols) const
+{
+ _rows = 0;
+ _cols = 0;
+ for (const auto& crow : _struct_rows)
+ {
+ string row = string(1,crow); // crow is char
+ _indices_rows[row] = _rows;
+ _rows += size_rows_.at(row);
+ }
+ for (const auto& ccol : _struct_cols)
+ {
+ string col = string(1,ccol); // ccol is char
+ _indices_cols[col] = _cols;
+ _cols += size_cols_.at(col);
+ }
+}
+
+MatrixComposite::MatrixComposite (const StateStructure& _row_structure, const StateStructure& _col_structure)
+{
+ for (const auto& rkey_char : _row_structure)
+ {
+ string rkey(1,rkey_char); // key was char
+ for (const auto& ckey_char : _col_structure)
+ {
+ string ckey(1,ckey_char); // key was char
+
+ this->emplace(rkey, ckey, MatrixXd(0,0));
+ }
+ }
+}
+
+MatrixComposite::MatrixComposite (const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes)
+{
+ assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
+ assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
+
+ auto rsize_it = _row_sizes.begin();
+ for (const auto& rkey_char : _row_structure)
+ {
+ string rkey(1,rkey_char); // key was char
+
+ auto csize_it = _col_sizes.begin();
+ for (const auto& ckey_char : _col_structure)
+ {
+ string ckey(1,ckey_char); // key was char
+
+ this->emplace(rkey, ckey, MatrixXd(*rsize_it, *csize_it)); // caution: blocks non initialized.
+
+ csize_it ++;
+ }
+ rsize_it++;
+ }
+}
+
+MatrixComposite::MatrixComposite (const MatrixXd& _m,
+ const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes)
+{
+ assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
+ assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
+
+ SizeEigen rindex = 0, cindex;
+ auto rsize_it = _row_sizes.begin();
+ for (const auto& rkey_char : _row_structure)
+ {
+ assert(_m.rows() >= rindex + *rsize_it && "Provided matrix has insufficient number of rows");
+
+ string rkey(1,rkey_char); // key was char
+
+ cindex = 0;
+ auto csize_it = _col_sizes.begin();
+
+ for (const auto& ckey_char : _col_structure)
+ {
+ assert(_m.cols() >= cindex + *csize_it && "Provided matrix has insufficient number of columns");
+
+ string ckey(1,ckey_char); // key was char
+
+ this->emplace(rkey, ckey, _m.block(rindex, cindex, *rsize_it, *csize_it));
+
+ cindex += *csize_it;
+ csize_it ++;
+ }
+
+ assert(_m.cols() == cindex && "Provided matrix has too many columns");
+
+ rindex += *rsize_it;
+ rsize_it++;
+ }
+
+ assert(_m.rows() == rindex && "Provided matrix has too many rows");
+}
+
+MatrixComposite MatrixComposite::zero (const StateStructure& _row_structure, const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure, const std::list<int>& _col_sizes)
+{
+ MatrixComposite m;
+
+ assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
+ assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
+
+ auto rsize_it = _row_sizes.begin();
+ for (const auto& rkey_char : _row_structure)
+ {
+ string rkey(1,rkey_char); // key was char
+
+ auto csize_it = _col_sizes.begin();
+ for (const auto& ckey_char : _col_structure)
+ {
+ string ckey(1,ckey_char); // key was char
+
+ m.emplace(rkey, ckey, MatrixXd::Zero(*rsize_it, *csize_it));
+
+ csize_it ++;
+ }
+ rsize_it++;
+ }
+ return m;
+}
+
+MatrixComposite MatrixComposite::identity (const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ MatrixComposite m;
+
+ assert (_structure.size() == _sizes.size() && "Structure and sizes have different number of elements!");
+
+ auto rsize_it = _sizes.begin();
+ auto rkey_it = _structure.begin();
+
+ while (rkey_it != _structure.end())
+ {
+
+ const auto& rkey = string(1,*rkey_it);
+ const auto rsize = *rsize_it;
+
+ m.emplace(rkey, rkey, MatrixXd::Identity(rsize,rsize)); // diagonal block
+
+ auto csize_it = rsize_it;
+ auto ckey_it = rkey_it;
+
+ csize_it ++;
+ ckey_it++;
+
+ while (ckey_it != _structure.end())
+ {
+ const auto& ckey = string(1,*ckey_it);
+ const auto& csize = *csize_it;
+
+ m.emplace(rkey, ckey, MatrixXd::Zero(rsize, csize)); // above-diagonal block
+ m.emplace(ckey, rkey, MatrixXd::Zero(csize, rsize)); // symmetric block
+
+ csize_it ++;
+ ckey_it++;
+ }
+
+ rsize_it ++;
+ rkey_it++;
+ }
+ return m;
+}
+
+bool MatrixComposite::check ( ) const
+{
+ bool size_ok = true;
+
+ // first see matrix itself, check that sizes are OK
+ for (const auto& pair_rkey_row : *this)
+ {
+ const auto& rkey = pair_rkey_row.first;
+ const auto& row = pair_rkey_row.second;
+ for (const auto& pair_ckey_blk : row)
+ {
+ const auto& ckey = pair_ckey_blk.first;
+ const auto& blk = pair_ckey_blk.second;
+
+ if (size_rows_.count(rkey) != 0)
+ {
+ if( size_rows_.at(rkey) != blk.rows())
+ {
+ WOLF_ERROR("row size for row ", rkey, " has wrong size");
+ size_ok = false;
+ }
+ }
+ else
+ {
+ WOLF_ERROR("row size for row ", rkey, " does not exist");
+ size_ok = false;
+ }
+ if (size_cols_.count(ckey) != 0)
+ {
+ if( size_cols_.at(ckey) != blk.cols())
+ {
+ WOLF_ERROR("col size for col ", rkey, " has wrong size");
+ size_ok = false;
+ }
+ }
+ else
+ {
+ WOLF_ERROR("col size for col ", ckey, " does not exist");
+ size_ok = false;
+ }
+ }
+ }
+ return size_ok;
+}
+
+std::ostream& operator <<(std::ostream &_os, const MatrixComposite &_M)
+{
+ for (const auto &pair_rkey_row : _M)
+ {
+ const auto rkey = pair_rkey_row.first;
+
+ for (const auto &pair_ckey_mat : pair_rkey_row.second)
+ {
+ const auto &ckey = pair_ckey_mat.first;
+
+ _os << "\n[" << rkey << "," << ckey << "]: \n" << pair_ckey_mat.second;
+ }
+ }
+ return _os;
+}
+
+
+
+////// STATE BLOCK COMPOSITE //////////
+
+const StateBlockMap& StateBlockComposite::getStateBlockMap() const
+{
+ return state_block_map_;
+}
+
+void StateBlockComposite::fix()
+{
+ for (auto pair_key_sbp : state_block_map_)
+ if (pair_key_sbp.second != nullptr)
+ pair_key_sbp.second->fix();
+}
+
+void StateBlockComposite::unfix()
+{
+ for (auto pair_key_sbp : state_block_map_)
+ if (pair_key_sbp.second != nullptr)
+ pair_key_sbp.second->unfix();
+}
+
+bool StateBlockComposite::isFixed() const
+{
+ bool fixed = true;
+ for (auto pair_key_sbp : state_block_map_)
+ {
+ if (pair_key_sbp.second)
+ fixed &= pair_key_sbp.second->isFixed();
+ }
+ return fixed;
+}
+
+unsigned int StateBlockComposite::remove(const std::string &_sb_type)
+{
+ return state_block_map_.erase(_sb_type);
+}
+
+bool StateBlockComposite::has(const StateBlockPtr &_sb) const
+{
+ bool found = false;
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ found = found || (pair_key_sb.second == _sb);
+ }
+ return found;
+}
+
+StateBlockPtr StateBlockComposite::at(const std::string &_sb_type) const
+{
+ auto it = state_block_map_.find(_sb_type);
+ if (it != state_block_map_.end())
+ return it->second;
+ else
+ return nullptr;
+}
+
+void StateBlockComposite::set(const std::string& _sb_type, const StateBlockPtr &_sb)
+{
+ auto it = state_block_map_.find(_sb_type);
+ assert ( it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead.");
+
+ it->second = _sb;
+}
+
+void StateBlockComposite::set(const std::string& _key, const VectorXd &_vec)
+{
+ auto it = state_block_map_.find(_key);
+ assert ( it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead.");
+
+ assert ( it->second->getSize() == _vec.size() && "Provided vector size mismatch with associated state block");
+
+ it->second->setState(_vec);
+}
+
+void StateBlockComposite::setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors)
+{
+ assert(_structure.size() == _vectors.size() && "Sizes of structure and vector list do not match");
+
+ auto vec_it = _vectors.begin();
+ for (const auto ckey : _structure)
+ {
+ string key(1,ckey);
+ set (key, *vec_it);
+ vec_it++;
+ }
+}
+
+bool StateBlockComposite::key(const StateBlockPtr &_sb, std::string &_key) const
+{
+ const auto& it = this->find(_sb);
+
+ bool found = (it != state_block_map_.end());
+
+ if (found)
+ {
+ _key = it->first;
+ return true;
+ }
+ else
+ {
+ _key = "";
+ return false;
+ }
+}
+
+std::string StateBlockComposite::key(const StateBlockPtr& _sb) const
+{
+ const auto& it = this->find(_sb);
+
+ bool found = (it != state_block_map_.end());
+
+ if (found)
+ return it->first;
+ else
+ return "";
+}
+
+
+StateBlockMapCIter StateBlockComposite::find(const StateBlockPtr &_sb) const
+{
+ const auto& it = std::find_if(state_block_map_.begin(),
+ state_block_map_.end(),
+ [_sb](const std::pair<std::string, StateBlockPtr>& pair)
+ {
+ return pair.second == _sb;
+ }
+ );
+
+ return it;
+}
+
+StateBlockPtr StateBlockComposite::add(const std::string &_sb_type, const StateBlockPtr &_sb)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+
+ state_block_map_.emplace(_sb_type, _sb);
+
+ return _sb;
+}
+
+void StateBlockComposite::perturb(double amplitude)
+{
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ auto& sb = pair_key_sb.second;
+ if (!sb->isFixed())
+ sb->perturb(amplitude);
+ }
+}
+
+void StateBlockComposite::plus(const VectorComposite &_dx)
+{
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ const auto& key = pair_key_sb.first;
+ const auto& sb = pair_key_sb.second;
+ const auto& dv = _dx.at(key);
+
+ sb->plus(dv);
+ }
+}
+
+VectorComposite StateBlockComposite::getState() const
+{
+ VectorComposite v;
+ getState(v);
+ return v;
+}
+
+bool StateBlockComposite::getState(VectorComposite &_state) const
+{
+ for (auto &pair_key_sb : state_block_map_)
+ {
+ _state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
+ }
+ return true;
+}
+
+void StateBlockComposite::setState(const VectorComposite &_state)
+{
+ for (const auto &pair_key_sb : _state)
+ {
+ state_block_map_[pair_key_sb.first]->setState(pair_key_sb.second);
+ }
+}
+
+void StateBlockComposite::setIdentity(bool _notify)
+{
+ for (const auto& pair_key_sb : getStateBlockMap())
+ {
+ pair_key_sb.second->setIdentity(_notify);
+ }
+}
+
+void StateBlockComposite::setZero(bool _notify)
+{
+ setIdentity(_notify);
+}
+
+VectorComposite StateBlockComposite::identity() const
+{
+ VectorComposite x;
+ for (const auto& pair_key_sb : getStateBlockMap())
+ x.emplace(pair_key_sb.first, pair_key_sb.second->identity());
+ return x;
+}
+
+VectorComposite StateBlockComposite::zero() const
+{
+ return identity();
+}
+
+
+SizeEigen StateBlockComposite::stateSize() const
+{
+ SizeEigen size = 0;
+ for (const auto& pair_key_sb : state_block_map_)
+ size += pair_key_sb.second->getSize();
+ return size;
+}
+
+SizeEigen StateBlockComposite::stateSize(const StateStructure &_structure) const
+{
+ SizeEigen size = 0;
+ for (const auto& ckey : _structure)
+ {
+ string key(1,ckey);
+ size += state_block_map_.at(key)->getSize();
+ }
+ return size;
+}
+
+VectorXd StateBlockComposite::stateVector(const StateStructure &_structure) const
+{
+ VectorXd x(stateSize(_structure));
+ SizeEigen index = 0;
+ SizeEigen size;
+ for (const auto& ckey : _structure)
+ {
+ string key(1,ckey);
+ const auto& sb = state_block_map_.at(key);
+ size = sb->getSize();
+ x.segment(index,size) = sb->getState();
+ index += size;
+ }
+ return x;
+}
+
+void StateBlockComposite::stateVector(const StateStructure &_structure, VectorXd &_vector) const
+{
+ _vector.resize(stateSize(_structure));
+ SizeEigen index = 0;
+ SizeEigen size;
+ for (const auto& ckey : _structure)
+ {
+ string key(1,ckey);
+ const auto& sb = state_block_map_.at(key);
+ size = sb->getSize();
+ _vector.segment(index,size) = sb->getState();
+ index += size;
+ }
+}
+
+} // namespace wolf
+
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index be5d2c3fbe75a756d92624e3cb9a529f85881ca0..4d0b6de19dc18a77d8c62f7e6b7fa329965fba3f 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -3,7 +3,7 @@
namespace wolf {
-TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) :
+TrajectoryBase::TrajectoryBase() :
NodeBase("TRAJECTORY", "TrajectoryBase"),
frame_structure_(_frame_structure)
// last_key_frame_ptr_(nullptr),
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index cb9421c5ed18c64af24816a7ba14c13d92ea3471..05717bad197c33e241146a8cf4d08fc337d99ea2 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -125,6 +125,7 @@ wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
# SE3 test
wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
+target_link_libraries(gtest_SE3 ${PROJECT_NAME})
# SensorBase test
wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
@@ -142,6 +143,10 @@ target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
target_link_libraries(gtest_state_block ${PROJECT_NAME})
+# StateBlock class test
+wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp)
+target_link_libraries(gtest_state_composite ${PROJECT_NAME})
+
# TimeStamp class test
wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
target_link_libraries(gtest_time_stamp ${PROJECT_NAME})
diff --git a/test/dummy/factor_dummy_zero_1.h b/test/dummy/factor_dummy_zero_1.h
index ffe372f0cb9f5671cc491f295d38ec24b305da3b..1ebe1b8fe2e4eba79fe2c9fbf236da5844327189 100644
--- a/test/dummy/factor_dummy_zero_1.h
+++ b/test/dummy/factor_dummy_zero_1.h
@@ -13,13 +13,17 @@ WOLF_PTR_TYPEDEFS(FactorDummyZero1);
//class
class FactorDummyZero1 : public FactorAutodiff<FactorDummyZero1, 1, 1>
{
+ using Base = FactorAutodiff<FactorDummyZero1, 1, 1>;
public:
- FactorDummyZero1(const StateBlockPtr& _sb_ptr) :
- FactorAutodiff<FactorDummyZero1, 1, 1>("FactorDummy1",
- nullptr, nullptr, nullptr, nullptr,
- nullptr,
- false, FAC_ACTIVE,
- _sb_ptr)
+ FactorDummyZero1(const FeatureBasePtr& _ftr_ptr,
+ const StateBlockPtr& _sb_ptr) :
+ Base("FactorDummy1",
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ nullptr,
+ false,
+ FAC_ACTIVE,
+ _sb_ptr)
{
//
}
diff --git a/test/dummy/factor_dummy_zero_12.h b/test/dummy/factor_dummy_zero_12.h
index 3a57aa386d20524fbbec30aa154f555af0fbb9a1..c60b9ce3232aa5c18807e80fbfb766cde275c953 100644
--- a/test/dummy/factor_dummy_zero_12.h
+++ b/test/dummy/factor_dummy_zero_12.h
@@ -12,9 +12,11 @@ namespace wolf {
template <unsigned int ID>
class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>
{
+ using Base = FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>;
static const unsigned int id = ID;
public:
- FactorDummyZero12(const StateBlockPtr& _sb1_ptr,
+ FactorDummyZero12(const FeatureBasePtr& _ftr_ptr,
+ const StateBlockPtr& _sb1_ptr,
const StateBlockPtr& _sb2_ptr,
const StateBlockPtr& _sb3_ptr,
const StateBlockPtr& _sb4_ptr,
@@ -26,22 +28,24 @@ class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2,
const StateBlockPtr& _sb10_ptr,
const StateBlockPtr& _sb11_ptr,
const StateBlockPtr& _sb12_ptr) :
- FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>("FactorDummy12",
- nullptr, nullptr, nullptr, nullptr,
- nullptr,
- false, FAC_ACTIVE,
- _sb1_ptr,
- _sb2_ptr,
- _sb3_ptr,
- _sb4_ptr,
- _sb5_ptr,
- _sb6_ptr,
- _sb7_ptr,
- _sb8_ptr,
- _sb9_ptr,
- _sb10_ptr,
- _sb11_ptr,
- _sb12_ptr)
+ Base("FactorDummy12",
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ nullptr,
+ false,
+ FAC_ACTIVE,
+ _sb1_ptr,
+ _sb2_ptr,
+ _sb3_ptr,
+ _sb4_ptr,
+ _sb5_ptr,
+ _sb6_ptr,
+ _sb7_ptr,
+ _sb8_ptr,
+ _sb9_ptr,
+ _sb10_ptr,
+ _sb11_ptr,
+ _sb12_ptr)
{
assert(id > 0 && id <= 12);
}
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index 6b31ffa63a398dc166a4e51fc552352f05a28390..48c7d66ff9482ed55df9f166374948a0d07ef672 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/test/dummy/factor_feature_dummy.h
@@ -49,23 +49,36 @@ class FactorFeatureDummy : public FactorBase
public:
static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
- const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr);
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr);
};
-inline FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& /*_feature_ptr*/, const FeatureBasePtr& _feature_other_ptr,
+inline FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& _feature_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status) :
- FactorBase("FactorFeatureDummy", nullptr, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status)
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorBase("FactorFeatureDummy",
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ _feature_other_ptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
{
//
}
-inline FactorBasePtr FactorFeatureDummy::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr)
+inline FactorBasePtr FactorFeatureDummy::create(const FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr)
{
- return std::make_shared<FactorFeatureDummy>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr), _processor_ptr);
+ return std::make_shared<FactorFeatureDummy>(_feature_ptr,
+ std::static_pointer_cast<FeatureBase>(_correspondant_ptr),
+ _processor_ptr);
}
} // namespace wolf
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index b4db44b7890f73289d54cb2708c85edb8a1b8b46..8adcfff4ec7891cee87da0fe0b70fd0bb15e0065 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/test/dummy/factor_landmark_dummy.h
@@ -25,11 +25,15 @@ class FactorLandmarkDummy : public FactorBase
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
- virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override {return true;};
+ virtual bool evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const override {return true;};
/** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
**/
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override {};
+ virtual void evaluate(const std::vector<const double*>& _states_ptr,
+ Eigen::VectorXd& residual_,
+ std::vector<Eigen::MatrixXd>& jacobians_) const override {};
/** \brief Returns the jacobians computation method
**/
@@ -37,7 +41,10 @@ class FactorLandmarkDummy : public FactorBase
/** \brief Returns a vector of pointers to the states in which this factor depends
**/
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override
+ {
+ return std::vector<StateBlockPtr>(0);
+ }
/** \brief Returns the factor residual size
**/
@@ -45,7 +52,10 @@ class FactorLandmarkDummy : public FactorBase
/** \brief Returns the factor states sizes
**/
- virtual std::vector<unsigned int> getStateSizes() const override {return std::vector<unsigned int>({1});}
+ virtual std::vector<unsigned int> getStateSizes() const override
+ {
+ return std::vector<unsigned int>({1});
+ }
public:
static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
@@ -54,18 +64,31 @@ class FactorLandmarkDummy : public FactorBase
};
-inline FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& /*_feature_ptr*/, const LandmarkBasePtr& _landmark_other_ptr,
+inline FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& _feature_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status) :
- FactorBase("FactorFeatureDummy", nullptr, nullptr, nullptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status)
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorBase("FactorFeatureDummy",
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
{
//
}
-inline FactorBasePtr FactorLandmarkDummy::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
+inline FactorBasePtr FactorLandmarkDummy::create(const FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
const ProcessorBasePtr& _processor_ptr)
{
- return std::make_shared<FactorLandmarkDummy>(_feature_ptr, std::static_pointer_cast<LandmarkBase>(_correspondant_ptr), _processor_ptr);
+ return std::make_shared<FactorLandmarkDummy>(_feature_ptr,
+ std::static_pointer_cast<LandmarkBase>(_correspondant_ptr),
+ _processor_ptr);
}
} // namespace wolf
diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
index c6f651ea013d2fac02164e0d3ea2cb96182fb6b2..d06ac67503acfda0a55d9418bac339359b3d9625 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -22,7 +22,8 @@ struct ParamsProcessorTrackerFeatureDummy : public ParamsProcessorTrackerFeature
unsigned int n_tracks_lost; ///< number of tracks lost each time track is called (the first ones)
ParamsProcessorTrackerFeatureDummy() = default;
- ParamsProcessorTrackerFeatureDummy(std::string _unique_name, const wolf::ParamsServer & _server):
+ ParamsProcessorTrackerFeatureDummy(std::string _unique_name,
+ const wolf::ParamsServer & _server):
ParamsProcessorTrackerFeature(_unique_name, _server)
{
n_tracks_lost = _server.getParam<unsigned int>(prefix + _unique_name + "/n_tracks_lost");
@@ -43,7 +44,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureDummy, ParamsProcessorTrackerFeatureDummy);
- virtual void configure(SensorBasePtr _sensor) { };
+ virtual void configure(SensorBasePtr _sensor) override { };
protected:
@@ -63,14 +64,16 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
- FeatureMatchMap& _feature_correspondences);
+ FeatureMatchMap& _feature_correspondences) override;
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
* \param _last_feature input feature in last capture tracked
* \param _incoming_feature input/output feature in incoming capture to be corrected
* \return false if the the process discards the correspondence with origin's feature
*/
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature);
+ virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
+ const FeatureBasePtr _last_feature,
+ FeatureBasePtr _incoming_feature) override;
/** \brief Vote for KeyFrame generation
*
@@ -79,7 +82,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() const;
+ virtual bool voteForKeyFrame() const override;
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
@@ -97,18 +100,19 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
*/
virtual unsigned int detectNewFeatures(const int& _max_new_features,
const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out);
+ FeatureBasePtrList& _features_out) override;
/** \brief Emplaces a new factor
* \param _feature_ptr pointer to the parent Feature
* \param _feature_other_ptr pointer to the other feature constrained.
*
* This function emplaces a factor of the appropriate type for the derived processor.
*/
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr,
+ FeatureBasePtr _feature_other_ptr) override;
};
inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ParamsProcessorTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
- ProcessorTrackerFeature("ProcessorTrackerFeatureDummy", 0, _params_tracker_feature_dummy),
+ ProcessorTrackerFeature("ProcessorTrackerFeatureDummy", "PO", 0, _params_tracker_feature_dummy),
params_tracker_feature_dummy_(_params_tracker_feature_dummy)
{
//
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index 472b21ab150a7945987d0d64d5cd171bdf9975eb..d36efd3f804cd71ff38fc7b835dc1b5230c59885 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -13,7 +13,7 @@ namespace wolf
{
ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ParamsProcessorTrackerLandmarkDummyPtr _params_tracker_landmark_dummy) :
- ProcessorTrackerLandmark("ProcessorTrackerLandmarkDummy", _params_tracker_landmark_dummy),
+ ProcessorTrackerLandmark("ProcessorTrackerLandmarkDummy", "PO", _params_tracker_landmark_dummy),
params_tracker_landmark_dummy_(_params_tracker_landmark_dummy)
{
//
@@ -101,10 +101,14 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_fe
LandmarkBasePtr ProcessorTrackerLandmarkDummy::emplaceLandmark(FeatureBasePtr _feature_ptr)
{
//std::cout << "ProcessorTrackerLandmarkDummy::emplaceLandmark" << std::endl;
- return LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(), "LandmarkBase", std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
+ return LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkBase",
+ std::make_shared<StateBlock>(2),
+ std::make_shared<StateBlock>(1));
}
-FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
+FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _feature_ptr,
+ LandmarkBasePtr _landmark_ptr)
{
std::cout << "\tProcessorTrackerLandmarkDummy::emplaceFactor" << std::endl;
std::cout << "\t\tfeature " << _feature_ptr->id() << std::endl;
diff --git a/test/dummy/processor_tracker_landmark_dummy.h b/test/dummy/processor_tracker_landmark_dummy.h
index b9485b3378e7644aa9206213dc3466fd41b83340..5140a2b2762db740944019ac1595e7d37bf07a2c 100644
--- a/test/dummy/processor_tracker_landmark_dummy.h
+++ b/test/dummy/processor_tracker_landmark_dummy.h
@@ -38,7 +38,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorTrackerLandmarkDummy, ParamsProcessorTrackerLandmarkDummy);
- virtual void configure(SensorBasePtr _sensor) { };
+ virtual void configure(SensorBasePtr _sensor) override { };
protected:
@@ -58,7 +58,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
- LandmarkMatchMap& _feature_landmark_correspondences);
+ LandmarkMatchMap& _feature_landmark_correspondences) override;
/** \brief Vote for KeyFrame generation
*
@@ -67,7 +67,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() const;
+ virtual bool voteForKeyFrame() const override;
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
@@ -85,13 +85,13 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
*/
virtual unsigned int detectNewFeatures(const int& _max_new_features,
const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out);
+ FeatureBasePtrList& _features_out) override;
/** \brief Emplace one landmark
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
- virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
+ virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) override;
/** \brief Emplace a new factor
* \param _feature_ptr pointer to the Feature to constrain
@@ -99,7 +99,8 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
*
* Implement this method in derived classes.
*/
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr,
+ LandmarkBasePtr _landmark_ptr) override;
};
} // namespace wolf
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index 0e6a1956b97728e8440edca7931c6d5ac32c3bc0..fe62d065c64fbf2f4c37f58b00ff4f51ea2a0ef1 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_manager_dummy.h
@@ -26,7 +26,7 @@ class SolverManagerDummy : public SolverManager
{
};
- bool isStateBlockRegistered(const StateBlockPtr& st)
+ bool isStateBlockRegistered(const StateBlockPtr& st) override
{
return state_blocks_.find(st)!=state_blocks_.end();
};
@@ -36,7 +36,7 @@ class SolverManagerDummy : public SolverManager
return state_block_fixed_.at(st);
};
- bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
+ bool isFactorRegistered(const FactorBasePtr& fac_ptr) const override
{
return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
};
@@ -51,46 +51,46 @@ class SolverManagerDummy : public SolverManager
return state_block_local_param_.find(st) != state_block_local_param_.end();
};
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
- virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
+ virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) override {};
+ virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) override {};
+ virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override {return true;};
+ virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override {return true;};
// The following are dummy implementations
- bool hasConverged() { return true; }
- SizeStd iterations() { return 1; }
- double initialCost() { return double(1); }
- double finalCost() { return double(0); }
+ bool hasConverged() override { return true; }
+ SizeStd iterations() override { return 1; }
+ double initialCost() override { return double(1); }
+ double finalCost() override { return double(0); }
protected:
virtual bool checkDerived(std::string prefix="") const override {return true;}
- virtual std::string solveDerived(const ReportVerbosity report_level){ return std::string("");};
- virtual void addFactorDerived(const FactorBasePtr& fac_ptr)
+ virtual std::string solveDerived(const ReportVerbosity report_level) override { return std::string("");};
+ virtual void addFactorDerived(const FactorBasePtr& fac_ptr) override
{
factors_.push_back(fac_ptr);
};
- virtual void removeFactorDerived(const FactorBasePtr& fac_ptr)
+ virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) override
{
factors_.remove(fac_ptr);
};
- virtual void addStateBlockDerived(const StateBlockPtr& state_ptr)
+ virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) override
{
state_block_fixed_[state_ptr] = state_ptr->isFixed();
state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
};
- virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr)
+ virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) override
{
state_block_fixed_.erase(state_ptr);
state_block_local_param_.erase(state_ptr);
};
- virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr)
+ virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override
{
state_block_fixed_[state_ptr] = state_ptr->isFixed();
};
- virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr)
+ virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override
{
if (state_ptr->getLocalParametrization() == nullptr)
state_block_local_param_.erase(state_ptr);
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index 329feed0d0aad5565204f4b746cc1781ec872c9d..27acfea3069b92076bd30e4abd4ac28d787147bf 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -20,7 +20,7 @@ TEST(SE3, exp_0)
Vector6d tau = Vector6d::Zero();
Vector7d pose; pose << 0,0,0, 0,0,0,1;
- ASSERT_MATRIX_APPROX(pose, exp_SE3(tau), 1e-8);
+ ASSERT_MATRIX_APPROX(pose, exp(tau), 1e-8);
}
TEST(SE3, log_I)
@@ -28,16 +28,16 @@ TEST(SE3, log_I)
Vector7d pose; pose << 0,0,0, 0,0,0,1;
Vector6d tau = Vector6d::Zero();
- ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
+ ASSERT_MATRIX_APPROX(tau, log(pose), 1e-8);
}
TEST(SE3, exp_log)
{
Vector6d tau = Vector6d::Random() / 5.0;
- Vector7d pose = exp_SE3(tau);
+ Vector7d pose = exp(tau);
- ASSERT_MATRIX_APPROX(tau, log_SE3(exp_SE3(tau)), 1e-8);
- ASSERT_MATRIX_APPROX(pose, exp_SE3(log_SE3(pose)), 1e-8);
+ ASSERT_MATRIX_APPROX(tau, log(exp(tau)), 1e-8);
+ ASSERT_MATRIX_APPROX(pose, exp(log(pose)), 1e-8);
}
TEST(SE3, exp_of_multiple)
@@ -45,20 +45,20 @@ TEST(SE3, exp_of_multiple)
Vector6d tau, tau2, tau3;
Vector7d pose, pose2, pose3;
tau = Vector6d::Random() / 5;
- pose << exp_SE3(tau);
+ pose << exp(tau);
tau2 = 2*tau;
tau3 = 3*tau;
pose2 = compose(pose, pose);
pose3 = compose(pose2, pose);
// 1
- ASSERT_MATRIX_APPROX(exp_SE3(tau), pose, 1e-8);
+ ASSERT_MATRIX_APPROX(exp(tau), pose, 1e-8);
// 2
- ASSERT_MATRIX_APPROX(exp_SE3(tau2), pose2, 1e-8);
+ ASSERT_MATRIX_APPROX(exp(tau2), pose2, 1e-8);
// 3
- ASSERT_MATRIX_APPROX(exp_SE3(tau3), pose3, 1e-8);
+ ASSERT_MATRIX_APPROX(exp(tau3), pose3, 1e-8);
}
TEST(SE3, log_of_power)
@@ -66,20 +66,177 @@ TEST(SE3, log_of_power)
Vector6d tau, tau2, tau3;
Vector7d pose, pose2, pose3;
tau = Vector6d::Random() / 5;
- pose << exp_SE3(tau);
+ pose << exp(tau);
tau2 = 2*tau;
tau3 = 3*tau;
pose2 = compose(pose, pose);
pose3 = compose(pose2, pose);
// 1
- ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
+ ASSERT_MATRIX_APPROX(tau, log(pose), 1e-8);
// 2
- ASSERT_MATRIX_APPROX(tau2, log_SE3(pose2), 1e-8);
+ ASSERT_MATRIX_APPROX(tau2, log(pose2), 1e-8);
// 3
- ASSERT_MATRIX_APPROX(tau3, log_SE3(pose3), 1e-8);
+ ASSERT_MATRIX_APPROX(tau3, log(pose3), 1e-8);
+}
+
+TEST(SE3, composeBlocks)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ compose(p1, q1, p2, q2, pc, qc);
+
+ ASSERT_MATRIX_APPROX(pc, p1 + q1*p2, 1e-8);
+ ASSERT_QUATERNION_APPROX(qc, q1*q2, 1e-8);
+}
+
+TEST(SE3, composeVectorBlocks)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ compose(p1, q1.coeffs(), p2, q2.coeffs(), pc, qc.coeffs());
+
+ ASSERT_MATRIX_APPROX(pc, p1 + q1*p2, 1e-8);
+ ASSERT_QUATERNION_APPROX(qc, q1*q2, 1e-8);
+}
+
+TEST(SE3, composeEigenVectors)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
+
+
+ Vector7d x1; x1 << p1, q1.coeffs();
+ Vector7d x2; x2 << p2, q2.coeffs();
+ Vector7d xc, xc_true; xc_true << pc, qc.coeffs();
+
+ compose(x1, x2, xc);
+
+ ASSERT_MATRIX_APPROX(xc, xc_true, 1e-8);
+}
+
+TEST(SE3, composeVectorComposite)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
+
+ VectorComposite x1, x2, xc("PO", {3,4});
+
+ x1.emplace("P", p1);
+ x1.emplace("O", q1.coeffs());
+ x2.emplace("P", p2);
+ x2.emplace("O", q2.coeffs());
+
+ compose(x1, x2, xc);
+
+ ASSERT_MATRIX_APPROX(xc.at("P"), pc, 1e-8);
+ ASSERT_MATRIX_APPROX(xc.at("O"), qc.coeffs(), 1e-8);
+}
+
+TEST(SE3, composeVectorComposite_Jacobians)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ Matrix3d R1 = q1.matrix();
+ Matrix3d R2 = q2.matrix();
+
+ compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
+
+ VectorComposite x1, x2, xc("PO", {3,4});
+ MatrixComposite J_xc_x1, J_xc_x2;
+
+ x1.emplace("P", p1);
+ x1.emplace("O", q1.coeffs());
+ x2.emplace("P", p2);
+ x2.emplace("O", q2.coeffs());
+
+ // we'll do xc = x1 * x2 and obtain Jacobians
+ compose(x1, x2, xc, J_xc_x1, J_xc_x2);
+
+ ASSERT_MATRIX_APPROX(xc.at("P"), pc, 1e-8);
+ ASSERT_MATRIX_APPROX(xc.at("O"), qc.coeffs(), 1e-8);
+
+ ASSERT_MATRIX_APPROX(J_xc_x1.at("P", "P"), Matrix3d::Identity() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at("P", "O"), - R1 * skew(p2) , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at("O", "P"), Matrix3d::Zero() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at("O", "O"), R2.transpose() , 1e-8);
+
+ ASSERT_MATRIX_APPROX(J_xc_x2.at("P", "P"), R1 , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at("P", "O"), Matrix3d::Zero() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at("O", "P"), Matrix3d::Zero() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at("O", "O"), Matrix3d::Identity() , 1e-8);
+}
+
+TEST(SE3, exp_0_Composite)
+{
+ // we check that exp(zero) = identity
+
+ Vector3d p; p.setZero();
+ Vector3d theta; theta.setZero();
+
+ VectorComposite tau;
+
+ tau.emplace("P", p);
+ tau.emplace("O", theta);
+
+ VectorComposite x = SE3::exp(tau);
+
+ ASSERT_MATRIX_APPROX(x.at("P"), Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(x.at("O"), Quaterniond::Identity().coeffs(), 1e-8);
+
+}
+
+TEST(SE3, plus_0_Composite)
+{
+ // we check that x plus zero = x
+
+ Vector3d p; p.setRandom();
+ Vector4d q; q.setRandom().normalized();
+
+ VectorComposite x;
+ x.emplace("P", p);
+ x.emplace("O", q);
+
+ Vector3d dp; dp.setZero();
+ Vector3d dtheta; dtheta.setZero();
+
+ VectorComposite tau;
+ tau.emplace("P", dp);
+ tau.emplace("O", dtheta);
+
+ VectorComposite res = plus(x, tau);
+
+ ASSERT_MATRIX_APPROX(res.at("P"), p, 1e-8);
+ ASSERT_MATRIX_APPROX(res.at("O"), q, 1e-8);
}
TEST(SE3, interpolate_I_xyz)
@@ -183,7 +340,7 @@ TEST(SE3, interpolate_half)
p1.setRandom(); p1.tail(4).normalize();
da.setRandom(); da /= 5; // small rotation
- dp << exp_SE3(da);
+ dp << exp(da);
// compose double, then interpolate 1/2
@@ -204,7 +361,7 @@ TEST(SE3, interpolate_third)
p1.setRandom(); p1.tail(4).normalize();
da.setRandom(); da /= 5; // small rotation
- dp << exp_SE3(da);
+ dp << exp(da);
dp2 = compose(dp, dp);
dp3 = compose(dp2, dp);
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index fc7d37ad3bb46fe16f50c4cdec38387a317c0b9d..d2b5c7e01b70fa65472f3e18bbfa7b38d8989679 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -318,7 +318,7 @@ TEST(CeresManager, AddFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
@@ -340,7 +340,7 @@ TEST(CeresManager, DoubleAddFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
@@ -365,7 +365,7 @@ TEST(CeresManager, RemoveFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
@@ -393,7 +393,7 @@ TEST(CeresManager, AddRemoveFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
@@ -420,7 +420,7 @@ TEST(CeresManager, DoubleRemoveFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
@@ -546,11 +546,11 @@ TEST(CeresManager, FactorsRemoveLocalParam)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
// update solver
ceres_manager_ptr->update();
@@ -588,11 +588,11 @@ TEST(CeresManager, FactorsUpdateLocalParam)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
// update solver
ceres_manager_ptr->update();
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 1e9f1019dba9e192e522b204519964e3790de9fb..82b56da523c70dff134908ba3e63adf97493d914 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -35,10 +35,11 @@ ProblemPtr problem_ptr = Problem::create("POV", 3);
CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(problem_ptr->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0, problem_ptr->stateZero() );
-// Capture, feature and factor
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
+// Capture
+// auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, nullptr);
////////////////////////////////////////////////////////
/* In the tests below, we check that FactorBlockAbsolute and FactorQuaternionAbsolute are working fine
@@ -49,7 +50,7 @@ auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pos
TEST(FactorBlockAbs, fac_block_abs_p)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>());
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP(), nullptr, false);
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getP(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
@@ -67,7 +68,7 @@ TEST(FactorBlockAbs, fac_block_abs_p)
TEST(FactorBlockAbs, fac_block_abs_p_tail2)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>());
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP(),1,2, nullptr, false);
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getP(),1,2, nullptr, false);
// Unfix frame 0, perturb frm0
frm0->unfix();
@@ -83,7 +84,7 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2)
TEST(FactorBlockAbs, fac_block_abs_v)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureVelocity", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>());
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getV(), nullptr, false);
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getV(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
@@ -101,7 +102,7 @@ TEST(FactorBlockAbs, fac_block_abs_v)
TEST(FactorQuatAbs, fac_block_abs_o)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureQuaternion", pose10.segment<4>(3), data_cov.block<3,3>(3,3));
- FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0->getFrame()->getO(), nullptr, false);
+ FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0, fea0->getFrame()->getO(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 68712f969a298f5986b93378480f1a66e03cafe1..0acd77891bf1bae60fe41575edec629e9864999b 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -23,8 +23,9 @@ using namespace Eigen;
TEST(FactorAutodiff, AutodiffDummy1)
{
StateBlockPtr sb = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+ FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
- auto fac = std::make_shared<FactorDummyZero1>(sb);
+ auto fac = std::make_shared<FactorDummyZero1>(ftr, sb);
// COMPUTE JACOBIANS
std::vector<const double*> states_ptr({sb->getStateData()});
@@ -56,9 +57,10 @@ TEST(FactorAutodiff, AutodiffDummy12)
Eigen::VectorXd residuals;
unsigned int i;
FactorBasePtr fac = nullptr;
+ FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
// 1 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<1>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<1>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -82,7 +84,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 2 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<2>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<2>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -106,7 +108,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 3 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<3>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<3>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -130,7 +132,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 4 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<4>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<4>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -154,7 +156,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 5 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<5>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<5>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -178,7 +180,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 6 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<6>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<6>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -202,7 +204,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 7 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<7>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<7>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -226,7 +228,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 8 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<8>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<8>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -250,7 +252,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 9 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<9>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<9>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -274,7 +276,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 10 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<10>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<10>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -298,7 +300,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 11 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<11>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<11>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -322,7 +324,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 12 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<12>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<12>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -523,7 +525,6 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2dAnalytic>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
-
const Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
const Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
const Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp
index d089b4dbddfe4e6b84334c6ddcc2a3b0fc47ff09..e25e1505a3ddaa15ecfa6152fc007bc6009696f9 100644
--- a/test/gtest_factor_autodiff_distance_3d.cpp
+++ b/test/gtest_factor_autodiff_distance_3d.cpp
@@ -55,13 +55,10 @@ class FactorAutodiffDistance3d_Test : public testing::Test
problem = Problem::create("PO", 3);
ceres_manager = std::make_shared<CeresManager>(problem);
- F1 = problem->emplaceKeyFrame (pose1, 1.0);
+ F1 = problem->emplaceFrame (KEY, 1.0, pose1);
- F2 = problem->emplaceKeyFrame (pose2, 2.0);
+ F2 = problem->emplaceFrame (KEY, 2.0, pose2);
C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0);
- f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov);
- c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
-
}
};
@@ -70,6 +67,8 @@ TEST_F(FactorAutodiffDistance3d_Test, ground_truth)
{
double res;
+ f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov);
+ c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
c2->operator ()(pos1.data(), pos2.data(), &res);
ASSERT_NEAR(res, 0.0, 1e-5);
@@ -79,7 +78,8 @@ TEST_F(FactorAutodiffDistance3d_Test, expected_residual)
{
double measurement = 1.400;
- f2->setMeasurement(Vector1d(measurement));
+ f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov);
+ c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
double res_expected = (measurement - (pos2-pos1).norm()) * c2->getMeasurementSquareRootInformationUpper()(0,0);
@@ -92,7 +92,9 @@ TEST_F(FactorAutodiffDistance3d_Test, expected_residual)
TEST_F(FactorAutodiffDistance3d_Test, solve)
{
double measurement = 1.400;
- f2->setMeasurement(Vector1d(measurement));
+
+ f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov);
+ c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET);
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index 51e017e9032955950818cbd3b413d6971673023e..3237da062337d5a26c3c320e63df62c7d2798fb8 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -24,8 +24,8 @@ class FactorBaseTest : public testing::Test
virtual void SetUp()
{
- F0 = std::make_shared<FrameBase>(NON_ESTIMATED, 0.0, nullptr);
- F1 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.0, nullptr);
+ F0 = std::make_shared<FrameBase>(KEY, 0.0, nullptr);
+ F1 = std::make_shared<FrameBase>(KEY, 1.0, nullptr);
C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr);
C1 = std::make_shared<CaptureBase>("Capture", 1.0, nullptr);
f0 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
@@ -39,11 +39,13 @@ class FactorBaseTest : public testing::Test
class FactorDummy : public FactorBase
{
public:
- FactorDummy(const FrameBasePtr& _frame_other,
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtr& _frame_other,
const CaptureBasePtr& _capture_other,
const FeatureBasePtr& _feature_other,
const LandmarkBasePtr& _landmark_other) :
FactorBase("Dummy",
+ _feature,
_frame_other,
_capture_other,
_feature_other,
@@ -53,11 +55,13 @@ class FactorDummy : public FactorBase
{
//
}
- FactorDummy(const FrameBasePtrList& _frame_other_list,
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
const LandmarkBasePtrList& _landmark_other_list) :
FactorBase("Dummy",
+ _feature,
_frame_other_list,
_capture_other_list,
_feature_other_list,
@@ -70,8 +74,12 @@ class FactorDummy : public FactorBase
virtual ~FactorDummy() = default;
virtual std::string getTopology() const override {return "DUMMY";}
- virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _jacobians) const override {return true;}
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& _residual, std::vector<Eigen::MatrixXd>& _jacobians) const override {}
+ virtual bool evaluate(double const* const* _parameters,
+ double* _residuals,
+ double** _jacobians) const override {return true;}
+ virtual void evaluate(const std::vector<const double*>& _states_ptr,
+ Eigen::VectorXd& _residual,
+ std::vector<Eigen::MatrixXd>& _jacobians) const override {}
virtual JacobianMethod getJacobianMethod() const override {return JacobianMethod::JAC_ANALYTIC;}
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {std::vector<StateBlockPtr> v; return v;}
virtual std::vector<unsigned int> getStateSizes() const override {std::vector<unsigned int> v; return v;}
@@ -81,7 +89,7 @@ class FactorDummy : public FactorBase
TEST_F(FactorBaseTest, constructor_from_pointers)
{
- FactorDummy fac(nullptr,C0,f0,nullptr);
+ FactorDummy fac(f0,nullptr,C0,f1,nullptr);
ASSERT_TRUE(fac.getFrameOtherList().empty());
@@ -90,11 +98,15 @@ TEST_F(FactorBaseTest, constructor_from_pointers)
ASSERT_EQ(fac.getFeatureOtherList().size(), 1);
ASSERT_TRUE(fac.getLandmarkOtherList().empty());
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurement(), f0->getMeasurement(), 1e-12);
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurementSquareRootInformationUpper(), f0->getMeasurementSquareRootInformationUpper(), 1e-12);
}
TEST_F(FactorBaseTest, constructor_from_lists)
{
- FactorDummy fac({},{C0},{f0,f1},{});
+ FactorDummy fac(f0,{},{C0},{f0,f1},{});
ASSERT_TRUE(fac.getFrameOtherList().empty());
@@ -103,6 +115,10 @@ TEST_F(FactorBaseTest, constructor_from_lists)
ASSERT_EQ(fac.getFeatureOtherList().size(), 2);
ASSERT_TRUE(fac.getLandmarkOtherList().empty());
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurement(), f0->getMeasurement(), 1e-12);
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurementSquareRootInformationUpper(), f0->getMeasurementSquareRootInformationUpper(), 1e-12);
}
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index bc7b92005a0aa81262599264986ae3497dec7241..bee51d78e417f1f446a5890fa1ead4834dc366bc 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -24,7 +24,7 @@ using namespace wolf;
const Vector6d zero6 = Vector6d::Zero();
const Vector3d zero3 = zero6.head<3>();
-
+const Matrix3d idty02 = Matrix3d::Identity() * 0.02;
class FixtureFactorBlockDifference : public testing::Test
{
@@ -35,6 +35,7 @@ class FixtureFactorBlockDifference : public testing::Test
FrameBasePtr KF1_;
CaptureBasePtr Cap_;
FeatureBasePtr Feat_;
+ FactorBlockDifferencePtr Fac_;
protected:
@@ -48,19 +49,19 @@ class FixtureFactorBlockDifference : public testing::Test
TimeStamp t0(0);
TimeStamp t1(1);
- Vector10d x_origin = problem_->zeroState();
- Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
+ // Vector10d x_origin = problem_->stateZero().vector("POV");
+ VectorComposite x_origin(problem_->stateZero().vector("POV"), "POV", {3, 4, 3});
+ // Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
+ VectorComposite cov_prior("POV", {Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)),Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)),Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1))});
KF0_ = problem_->setPriorFactor(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(), nullptr, false);
- KF1_ = problem_->emplaceKeyFrame(problem_->zeroState(), t1);
+ KF1_ = problem_->emplaceFrame(KEY, t1, problem_->stateZero());
Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
- Eigen::Matrix3d cov = 0.2 * Eigen::Matrix3d::Identity();
- Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, cov);
}
virtual void TearDown() override {}
@@ -68,20 +69,16 @@ class FixtureFactorBlockDifference : public testing::Test
TEST_F(FixtureFactorBlockDifference, CheckFactorType)
{
- // Feat_->setMeasurement()
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getP(), KF1_->getP()
- );
- ASSERT_EQ(Fac->getType(), "FactorBlockDifference");
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP());
+ ASSERT_EQ(Fac_->getType(), "FactorBlockDifference");
}
TEST_F(FixtureFactorBlockDifference, EqualP)
{
- // Feat_->setMeasurement()
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getP(), KF1_->getP()
- );
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP());
// perturbate
KF0_->getP()->setState(Vector3d::Random());
@@ -90,14 +87,12 @@ TEST_F(FixtureFactorBlockDifference, EqualP)
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()
- );
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getV(), KF1_->getV());
// perturbate
KF0_->getV()->setState(Vector3d::Random());
@@ -106,18 +101,14 @@ TEST_F(FixtureFactorBlockDifference, EqualV)
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()
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP());
// perturbate
KF0_->getP()->setState(Vector3d::Random());
@@ -126,18 +117,14 @@ TEST_F(FixtureFactorBlockDifference, DiffP)
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()
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getV(), KF1_->getV());
// perturbate
KF0_->getV()->setState(Vector3d::Random());
@@ -154,13 +141,11 @@ 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(),
- nullptr, nullptr, nullptr, nullptr,
- 0, 1, 0, 1
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, cov_diff);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
+ 0, 1, 0, 1);
// perturbate
KF0_->getP()->setState(Vector3d::Random());
@@ -171,21 +156,16 @@ TEST_F(FixtureFactorBlockDifference, DiffPx)
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(),
- nullptr, nullptr, nullptr, nullptr,
- 0, 2, 0, 2
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, cov_diff);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
+ 0, 2, 0, 2);
// perturbate
KF0_->getP()->setState(Vector3d::Random());
@@ -201,13 +181,11 @@ 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(),
- nullptr, nullptr, nullptr, nullptr,
- 1, 2, 1, 2
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, cov_diff);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
+ 1, 2, 1, 2);
// perturbate
KF0_->getP()->setState(Vector3d::Random());
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 3fb12bb38ca2f3ea4157842aa4c9e6a6348bccf5..0413c21cb2c30bb91b1a03dabac0b595d1843349 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -63,12 +63,9 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
- virtual void statePlusDelta(const Eigen::VectorXd& _x,
- const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _x_plus_delta) const
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const
{
- Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
+ return Base::getCalibration(_capture);
}
virtual Eigen::VectorXd deltaZero() const
@@ -162,16 +159,18 @@ class FactorDiffDriveTest : public testing::Test
processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
// frames
- F0 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory,
- "PO",
- 2,
- 0.0,
- Vector3d(0,0,0));
- F1 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory,
- "PO",
- 2,
- 1.0,
- Vector3d(1,0,0));
+ F0 = FrameBase::emplace<FrameBase>(trajectory,
+ KEY,
+ 0.0,
+ "PO",
+ 2,
+ Vector3d(0,0,0));
+ F1 = FrameBase::emplace<FrameBase>(trajectory,
+ KEY,
+ 1.0,
+ "PO",
+ 2,
+ Vector3d(1,0,0));
// captures
C0 = CaptureBase::emplace<CaptureDiffDrive>(F0,
@@ -199,7 +198,7 @@ class FactorDiffDriveTest : public testing::Test
"FeatureDiffDrive",
delta1,
delta1_cov,
- C0->getCalibration(),
+ processor->getCalibration(C0),
Matrix3d::Identity()); // TODO Jacobian?
@@ -288,7 +287,15 @@ TEST_F(FactorDiffDriveTest, residual_right_turn_90_deg)
ASSERT_MATRIX_APPROX(delta2, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
- f1->setMeasurement(delta2);
+ // modify f1 measurement (remove and emplace - FeatureBase::setMeasurement is private)
+ f1 ->remove();
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "FeatureDiffDrive",
+ delta2,
+ delta1_cov,
+ processor->getCalibration(C0),
+ Matrix3d::Identity()); // TODO Jacobian?
+
F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
@@ -315,7 +322,15 @@ TEST_F(FactorDiffDriveTest, solve_F1)
delta1 .setZero();
processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
- f1->setMeasurement(delta2);
+ // modify f1 measurement (remove and emplace - FeatureBase::setMeasurement is private)
+ f1 ->remove();
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "FeatureDiffDrive",
+ delta2,
+ delta1_cov,
+ processor->getCalibration(C0),
+ Matrix3d::Identity()); // TODO Jacobian?
+
F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
@@ -326,14 +341,14 @@ TEST_F(FactorDiffDriveTest, solve_F1)
// Fix all but F1 ; perturb F1 ; solve ; print and assert values of F1
F0->fix();
sensor->fixIntrinsics();
- F1->setState(F1->getState() + Vector3d::Random() * 0.1);
+ F1->perturb(0.1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
// WOLF_TRACE("\n", report);
problem->print(1,0,1,1);
- ASSERT_MATRIX_APPROX(F1->getState(), delta, 1e-6);
+ ASSERT_MATRIX_APPROX(F1->getStateVector(), delta, 1e-6);
}
@@ -353,7 +368,15 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
delta1 .setZero();
processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
- f1->setMeasurement(delta2);
+ // modify f1 measurement (remove and emplace - FeatureBase::setMeasurement is private)
+ f1 ->remove();
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "FeatureDiffDrive",
+ delta2,
+ delta1_cov,
+ processor->getCalibration(C0),
+ Matrix3d::Identity()); // TODO Jacobian?
+
F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
@@ -365,7 +388,7 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
F0->fix();
F1->fix();
sensor->unfixIntrinsics();
- sensor->getIntrinsic()->setState(calib + Vector3d::Random() * 0.1);
+ sensor->perturb(0.1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -441,13 +464,14 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
TimeStamp t(0.0);
double dt = 1.0;
- Vector3d x0(0,0,0);
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
Vector3d x1(1.5, -1.5, -M_PI/2.0);
Vector3d x2(3.0, -3.0, 0.0);
- Matrix3d P0; P0.setIdentity();
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(0.1,0.1,0.1), "PO", {2,1});
// set prior at t=0 and processor origin
- auto F0 = problem->setPriorFactor(x0, P0, t, 0.1);
+ auto F0 = problem->setPriorFactor(x0, s0, t, 0.1);
processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
@@ -492,16 +516,16 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE("\n", report);
- WOLF_TRACE("x1 : ", problem->getState(N*dt).transpose());
- WOLF_TRACE("x2 : ", F2->getState().transpose());
+ WOLF_TRACE("x1 : ", problem->getState(N*dt).vector("PO").transpose());
+ WOLF_TRACE("x2 : ", F2->getStateVector().transpose());
WOLF_TRACE("calib_preint : ", calib_preint.transpose());
WOLF_TRACE("calib_pert : ", calib_pert.transpose());
WOLF_TRACE("calib_est : ", sensor->getIntrinsic()->getState().transpose());
WOLF_TRACE("calib_gt : ", calib_gt.transpose());
- ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib_gt, 1e-6);
- ASSERT_MATRIX_APPROX(problem->getState( N*dt), x1, 1e-6);
- ASSERT_MATRIX_APPROX(problem->getState(2*N*dt), x2, 1e-6);
+ ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState() , calib_gt , 1e-6 );
+ ASSERT_MATRIX_APPROX(problem->getState( N*dt).vector("PO") , x1 , 1e-6 );
+ ASSERT_MATRIX_APPROX(problem->getState(2*N*dt).vector("PO") , x2 , 1e-6 );
std::cout << "\n\n" << std::endl;
@@ -573,13 +597,15 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
TimeStamp t(0.0);
double dt = 1.0;
- Vector3d x0(0,0,0);
+ // Vector3d x0(0,0,0);
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
Vector3d x1(1.5, -1.5, -M_PI/2.0);
Vector3d x2(3.0, -3.0, 0.0);
- Matrix3d P0; P0.setIdentity();
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
// set prior at t=0 and processor origin
- auto F0 = problem->setPriorFactor(x0, P0, t, 0.1);
+ auto F0 = problem->setPriorFactor(x0, s0, t, 0.1);
processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
@@ -614,7 +640,8 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
auto F2 = problem->getLastKeyFrame();
- F2->setState(x2); // Impose known final state regardless of integrated value.
+// VectorComposite x2c(x2, "PO", {2,1});
+ F2->setState(x2, "PO"); // Impose known final state regardless of integrated value.
// Fix boundaries
F0->fix();
@@ -624,16 +651,16 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE("\n", report);
- WOLF_TRACE("x1 : ", problem->getState(N*dt).transpose());
- WOLF_TRACE("x2 : ", F2->getState().transpose());
+ WOLF_TRACE("x1 : ", problem->getState(N*dt).vector("PO").transpose());
+ WOLF_TRACE("x2 : ", F2->getStateVector().transpose());
WOLF_TRACE("calib_preint : ", calib_preint.transpose());
WOLF_TRACE("calib_est : ", sensor->getIntrinsic()->getState().transpose());
WOLF_TRACE("calib_GT : ", calib_gt.transpose());
ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib_gt, 1e-2);
- ASSERT_MATRIX_APPROX(problem->getState( N*dt), x1, 1e-2);
- ASSERT_MATRIX_APPROX(problem->getState(2*N*dt), x2, 1e-6);
+ ASSERT_MATRIX_APPROX(problem->getState( N*dt).vector("PO"), x1, 1e-2);
+ ASSERT_MATRIX_APPROX(problem->getState(2*N*dt).vector("PO"), x2, 1e-6);
}
diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp
index 954dcc47b185a30f3c1bdaec54c02069a664a505..8c0b11e3b428cfbfa99c91c29bffe53ceeeb1d92 100644
--- a/test/gtest_factor_odom_2d.cpp
+++ b/test/gtest_factor_odom_2d.cpp
@@ -19,13 +19,11 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(Vector3d::Zero(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(Vector3d::Zero(), TimeStamp(1));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), Vector3d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, TimeStamp(1), Vector3d::Zero());
-// Capture, feature and factor from frm1 to frm0
+// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
-auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
-auto fac1 = FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false); // create and add
TEST(FactorOdom2d, check_tree)
{
@@ -49,11 +47,14 @@ TEST(FactorOdom2d, fix_0_solve)
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
x1(2) = pi2pi(x0(2) + delta(2));
- // Set states and measurement
+ // Set states
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false);
// Fix frm0, perturb frm1
frm0->fix();
@@ -63,7 +64,10 @@ TEST(FactorOdom2d, fix_0_solve)
// solve for frm1
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm1->getState(), x1, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
@@ -84,11 +88,14 @@ TEST(FactorOdom2d, fix_1_solve)
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
x1(2) = pi2pi(x0(2) + delta(2));
- // Set states and measurement
+ // Set states
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false);
// Fix frm1, perturb frm0
frm1->fix();
@@ -98,7 +105,10 @@ TEST(FactorOdom2d, fix_1_solve)
// solve for frm0
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm0->getState(), x0, 1e-6);
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
diff --git a/test/gtest_factor_odom_3d.cpp b/test/gtest_factor_odom_3d.cpp
index b24a074a3262884d6364df47c6253e98bd2c41a8..8001bbe70f25f774a55cf0c834950529934bdb36 100644
--- a/test/gtest_factor_odom_3d.cpp
+++ b/test/gtest_factor_odom_3d.cpp
@@ -37,11 +37,11 @@ ProblemPtr problem_ptr = Problem::create("PO", 3);
CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(problem_ptr->zeroState(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(delta, TimeStamp(1));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), problem_ptr->stateZero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, TimeStamp(1), delta);
// Capture, feature and factor from frm1 to frm0
-auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, 7, 6, nullptr);
+auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr);
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
FactorOdom3dPtr ctr1 = FactorBase::emplace<FactorOdom3d>(fea1, fea1, frm0, nullptr, false); // create and add
@@ -68,7 +68,7 @@ TEST(FactorOdom3d, fix_0_solve)
// solve for frm1
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(frm1->getState(), delta, 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector(), delta, 1e-6);
}
@@ -82,7 +82,7 @@ TEST(FactorOdom3d, fix_1_solve)
// solve for frm0
std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(frm0->getState(), problem_ptr->zeroState(), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector(), problem_ptr->stateZero().vector("PO"), 1e-6);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp
index a302bf7721c587762c8566180fa5168ef3bb9095..e3107b7a5f976f5626f22016f64f60695e1fc982 100644
--- a/test/gtest_factor_pose_2d.cpp
+++ b/test/gtest_factor_pose_2d.cpp
@@ -30,10 +30,10 @@ ProblemPtr problem = Problem::create("PO", 2);
CeresManager ceres_mgr(problem);
// Two frames
-FrameBasePtr frm0 = problem->emplaceKeyFrame(problem->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem->emplaceFrame(KEY, TimeStamp(0), problem->stateZero());
// Capture, feature and factor from frm1 to frm0
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2d", 0, nullptr, pose, 3, 3, nullptr);
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2d", 0, nullptr, pose, nullptr);
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom2d", pose, data_cov);
auto ctr0 = FactorBase::emplace<FactorPose2d>(fea0, fea0, nullptr, false);
@@ -59,7 +59,7 @@ TEST(FactorPose2d, solve)
// solve for frm0
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(frm0->getState(), pose, 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector(), pose, 1e-6);
}
diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp
index c9e95cca169bb9db68a82b8742d280dc31b22b40..d57aee85c27b47e41f9610040c3b8bcceb57ad35 100644
--- a/test/gtest_factor_pose_3d.cpp
+++ b/test/gtest_factor_pose_3d.cpp
@@ -36,10 +36,10 @@ ProblemPtr problem = Problem::create("PO", 3);
CeresManager ceres_mgr(problem);
// Two frames
-FrameBasePtr frm0 = problem->emplaceKeyFrame(problem->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem->emplaceFrame(KEY, 0, problem->stateZero() );
// Capture, feature and factor
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3d", 0, nullptr, pose7, 7, 6, nullptr);
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3d", 0, nullptr, pose7, nullptr);
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom3d", pose7, data_cov);
FactorPose3dPtr ctr0 = FactorBase::emplace<FactorPose3d>(fea0, fea0, nullptr, false);
@@ -65,7 +65,7 @@ TEST(FactorPose3d, solve)
// solve for frm0
std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::FULL);
- ASSERT_MATRIX_APPROX(frm0->getState(), pose7, 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector(), pose7, 1e-6);
}
diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_2d_analytic.cpp
index 2a9575ac9e19c72f49fcc9284a47eb9057fe19cd..c461bbb94743c9ee8b3665650fb00686d979fc20 100644
--- a/test/gtest_factor_relative_2d_analytic.cpp
+++ b/test/gtest_factor_relative_2d_analytic.cpp
@@ -19,13 +19,11 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(Vector3d::Zero(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(Vector3d::Zero(), TimeStamp(1));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0.0, Vector3d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 1.0, Vector3d::Zero());
-// Capture, feature and factor from frm1 to frm0
+// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
-auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
-auto fac1 = FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false); // create and add
TEST(FactorRelative2dAnalytic, check_tree)
{
@@ -53,7 +51,10 @@ TEST(FactorRelative2dAnalytic, fix_0_solve)
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false);
// Fix frm0, perturb frm1
frm0->fix();
@@ -63,7 +64,10 @@ TEST(FactorRelative2dAnalytic, fix_0_solve)
// solve for frm1
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm1->getState(), x1, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
@@ -88,7 +92,10 @@ TEST(FactorRelative2dAnalytic, fix_1_solve)
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false);
// Fix frm1, perturb frm0
frm1->fix();
@@ -98,7 +105,10 @@ TEST(FactorRelative2dAnalytic, fix_1_solve)
// solve for frm0
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm0->getState(), x0, 1e-6);
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index 756e1f9ecc4244756cd60d354270e73ec5196f78..b9ad5bbe208d981a4ca69aa3eab8eac806f62e14 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -27,13 +27,11 @@ auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d
auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>());
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(Vector3d::Zero(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(Vector3d::Zero(), TimeStamp(1));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0, Vector3d::Zero() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 1, Vector3d::Zero() );
-// Capture, feature and factor from frm1 to frm0
+// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
-auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
-auto fac1 = FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
TEST(FactorRelativePose2dWithExtrinsics, check_tree)
{
@@ -61,13 +59,16 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
x1(2) = pi2pi(x0(2) + delta(2));
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
- // Set states and measurement
+ // Set states
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
sensor_odom2d->setState(xs);
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+
// Perturb frm1, fix the rest
frm0->fix();
frm1->unfix();
@@ -78,7 +79,10 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
// solve for frm1
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm1->getState(), x1, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
@@ -103,13 +107,16 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
x1(2) = pi2pi(x0(2) + delta(2));
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
- // Set states and measurement
+ // Set states
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
sensor_odom2d->setState(xs);
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+
// Perturb frm0, fix the rest
frm1->fix();
frm0->unfix();
@@ -120,7 +127,10 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
// solve for frm0
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm0->getState(), x0, 1e-6);
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
@@ -145,13 +155,16 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
x1(2) = pi2pi(x0(2) + delta(2));
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
- // Set states and measurement
+ // Set states
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
sensor_odom2d->setState(xs);
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+
// Perturb sensor P, fix the rest
frm1->fix();
frm0->fix();
@@ -162,7 +175,10 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
// solve for frm0
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(sensor_odom2d->getState(), xs, 1e-6);
+ ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
@@ -187,14 +203,15 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
x1(2) = pi2pi(x0(2) + delta(2));
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
- // Set states and measurement
+ // Set states
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
sensor_odom2d->setState(xs);
- //std::cout << "Sensor O: " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
// Perturb sensor O, fix the rest
frm1->fix();
@@ -208,7 +225,10 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
// solve for frm0
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(sensor_odom2d->getState(), xs, 1e-6);
+ ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp
index 2824fe179096eaca65136b5fadb31b4053bd99cc..bbb978af9c69260bb409ee28e0c9da88214609f5 100644
--- a/test/gtest_feature_base.cpp
+++ b/test/gtest_feature_base.cpp
@@ -12,7 +12,7 @@
using namespace wolf;
using namespace Eigen;
-TEST(FeatureBase, Constructor)
+TEST(FeatureBase, ConstructorCov)
{
Vector3d m; m << 1,2,3;
Matrix3d M; M.setRandom(); M = M*M.transpose();
@@ -29,7 +29,7 @@ TEST(FeatureBase, Constructor)
ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6);
}
-TEST(FeatureBase, SetMeasurement)
+TEST(FeatureBase, ConstructorInfo)
{
Vector3d m; m << 1,2,3;
Matrix3d M; M.setRandom(); M = M*M.transpose();
@@ -38,26 +38,9 @@ TEST(FeatureBase, SetMeasurement)
Eigen::MatrixXd U = llt_of_info.matrixU();
Eigen::MatrixXd L = llt_of_info.matrixL();
- FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3d::Zero(), Matrix3d::Identity()));
-
- f->setMeasurement(m);
+ FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", m, I, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
ASSERT_MATRIX_APPROX(f->getMeasurement(), m, 1e-6);
-}
-
-TEST(FeatureBase, SetMeasurementCovariance)
-{
- Vector3d m; m << 1,2,3;
- Matrix3d M; M.setRandom(); M = M*M.transpose();
- Matrix3d I = M.inverse();
- Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A
- Eigen::MatrixXd U = llt_of_info.matrixU();
- Eigen::MatrixXd L = llt_of_info.matrixL();
-
- FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3d::Zero(), Matrix3d::Identity()));
-
- f->setMeasurementCovariance(M);
-
ASSERT_MATRIX_APPROX(f->getMeasurementCovariance(), M, 1e-6);
ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(), U.transpose()*U, 1e-6);
ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6);
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 4845f757c046bd57cea373b4f26a6d4f6e6e5001..df0c7ce316ecef3897f990a24a970a38ef042d11 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -69,9 +69,9 @@ TEST(FrameBase, LinksToTree)
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
- auto F1 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F2 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), 3, 3, nullptr);
+ auto F1 = FrameBase::emplace<FrameBase>(T, KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F2 = FrameBase::emplace<FrameBase>(T, KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>());
WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
@@ -145,15 +145,39 @@ TEST(FrameBase, GetSetState)
ASSERT_MATRIX_APPROX(q, F.getO()->getState(), Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(v, F.getV()->getState(), Constants::EPS_SMALL);
- // Get the state, form 1 by reference
- F.getState(x1);
- ASSERT_MATRIX_APPROX(x1 , x, Constants::EPS_SMALL);
-
// get the state, form 2 by return value
- x2 = F.getState();
+ x2 = F.getStateVector();
ASSERT_MATRIX_APPROX(x2, x, Constants::EPS_SMALL);
}
+TEST(FrameBase, Constructor_composite)
+{
+ FrameBase F = FrameBase(KEY,
+ 0.0,
+ "POV",
+ VectorComposite("POV", {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)}));
+
+ ASSERT_TRUE (F.isKey());
+ ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
+ ASSERT_TRUE (F.getO()->hasLocalParametrization());
+ ASSERT_EQ (F.getStateBlock("V")->getSize(), 3);
+}
+
+TEST(FrameBase, Constructor_vectors)
+{
+ FrameBase F = FrameBase(KEY,
+ 0.0,
+ "POV",
+ {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)});
+
+ ASSERT_TRUE (F.isKey());
+ ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
+ ASSERT_TRUE (F.getO()->hasLocalParametrization());
+ ASSERT_EQ (F.getStateBlock("V")->getSize(), 3);
+}
+
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 84e0e45f3d3ab6953de7841bc80c5f8427af052d..8ef700a29dca74d56baeee8e7869c7fb04f67947 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -92,7 +92,7 @@ void show(const ProblemPtr& problem)
<< F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl;
}
}
- cout << " state: \n" << F->getState().transpose() << endl;
+ cout << " state: \n" << F->getStateVector().transpose() << endl;
Eigen::MatrixXd cov;
problem->getFrameCovariance(F,cov);
cout << " covariance: \n" << cov << endl;
@@ -119,8 +119,8 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
TimeStamp t0(0.0), t = t0;
double dt = .01;
- Vector3d x0 (0,0,0);
- Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
Vector3d delta (2,0,0);
Matrix3d delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
@@ -128,18 +128,18 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
CeresManager ceres_manager(Pr);
// KF0 and absolute prior
- FrameBasePtr F0 = Pr->setPriorFactor(x0, P0,t0, dt/2);
+ FrameBasePtr F0 = Pr->setPriorFactor(x0, s0,t0, dt/2);
// KF1 and motion from KF0
t += dt;
- FrameBasePtr F1 = Pr->emplaceKeyFrame(Vector3d::Zero(), t);
+ FrameBasePtr F1 = Pr->emplaceFrame(KEY, t, Vector3d::Zero());
auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2d", t);
auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2d", delta, delta_cov);
auto c1 = FactorBase::emplace<FactorOdom2d>(f1, f1, F0, nullptr, false);
// KF2 and motion from KF1
t += dt;
- FrameBasePtr F2 = Pr->emplaceKeyFrame(Vector3d::Zero(), t);
+ FrameBasePtr F2 = Pr->emplaceFrame(KEY, t, Vector3d::Zero());
auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2d", t);
auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2d", delta, delta_cov);
auto c2 = FactorBase::emplace<FactorOdom2d>(f2, f2, F1, nullptr, false);
@@ -170,11 +170,12 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
ASSERT_TRUE(Pr->getFrameCovariance(F1, P1_solver));
ASSERT_TRUE(Pr->getFrameCovariance(F2, P2_solver));
- ASSERT_POSE2d_APPROX(F0->getState(), Vector3d(0,0,0), 1e-6);
- ASSERT_MATRIX_APPROX(P0_solver, P0, 1e-6);
- ASSERT_POSE2d_APPROX(F1->getState(), Vector3d(2,0,0), 1e-6);
+ ASSERT_POSE2d_APPROX(F0->getStateVector(), Vector3d(0,0,0), 1e-6);
+ auto s0_vector = s0.vector("PO");
+ ASSERT_MATRIX_APPROX(P0_solver, MatrixXd((s0_vector.array() * s0_vector.array()).matrix().asDiagonal()), 1e-6);
+ ASSERT_POSE2d_APPROX(F1->getStateVector(), Vector3d(2,0,0), 1e-6);
ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6);
- ASSERT_POSE2d_APPROX(F2->getState(), Vector3d(4,0,0), 1e-6);
+ ASSERT_POSE2d_APPROX(F2->getStateVector(), Vector3d(4,0,0), 1e-6);
ASSERT_MATRIX_APPROX(P2_solver, P2, 1e-6);
}
@@ -187,8 +188,10 @@ TEST(Odom2d, VoteForKfAndSolve)
TimeStamp t0(0.0), t = t0;
double dt = .01;
// Origin frame:
- Vector3d x0(0,0,0);
- Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
+ // Vector3d x0(0,0,0);
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
+ VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
// motion data
VectorXd data(Vector2d(1, M_PI_4) ); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn
Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
@@ -217,8 +220,10 @@ TEST(Odom2d, VoteForKfAndSolve)
// Ceres wrapper
CeresManager ceres_manager(problem);
- // Origin Key Frame (in t1 to let processor motion to join the KF)
- problem->setPriorFactor(x0, P0, t+dt, dt/2);
+ // Origin Key Frame
+ auto KF = problem->setPriorFactor(x0, s0, t, dt/2);
+ processor_odom2d->setOrigin(KF);
+
ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
@@ -227,8 +232,8 @@ TEST(Odom2d, VoteForKfAndSolve)
// std::cout << "Motion data : " << data.transpose() << std::endl;
// Check covariance values
- Eigen::Vector3d integrated_pose = x0;
- Eigen::Matrix3d integrated_cov = P0;
+ Eigen::Vector3d integrated_pose = Vector3d(0,0,0);
+ Eigen::Matrix3d integrated_cov = Eigen::Matrix3d::Identity() * 0.1;
Eigen::Vector3d integrated_delta = Eigen::Vector3d::Zero();
Eigen::Matrix3d integrated_delta_cov = Eigen::Matrix3d::Zero();
Eigen::MatrixXd Ju(3, 2);
@@ -283,7 +288,7 @@ TEST(Odom2d, VoteForKfAndSolve)
integrated_pose = plus(integrated_pose, data);
integrated_cov = Jx * integrated_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
- ASSERT_POSE2d_APPROX(processor_odom2d->getCurrentState(), integrated_pose, 1e-6);
+ ASSERT_POSE2d_APPROX(processor_odom2d->getState().vector("PO"), integrated_pose, 1e-6);
integrated_pose_vector.push_back(integrated_pose);
integrated_cov_vector.push_back(integrated_cov);
@@ -296,6 +301,8 @@ TEST(Odom2d, VoteForKfAndSolve)
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+ problem->print(4,1,1,1);
+
// Check the 3 KFs' states and covariances
MatrixXd computed_cov;
int n = 0;
@@ -303,7 +310,7 @@ TEST(Odom2d, VoteForKfAndSolve)
{
if (!F->isKey()) break;
- ASSERT_POSE2d_APPROX(F->getState(), integrated_pose_vector[n] , 1e-6);
+ ASSERT_POSE2d_APPROX(F->getStateVector("PO"), integrated_pose_vector[n] , 1e-6);
ASSERT_TRUE (F->getCovariance(computed_cov));
ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n] , 1e-6);
n += 3;
@@ -321,8 +328,8 @@ TEST(Odom2d, KF_callback)
// time
TimeStamp t0(0.0), t = t0;
double dt = .01;
- Vector3d x0(0,0,0);
- Eigen::Matrix3d x0_cov = Eigen::Matrix3d::Identity() * 0.1;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite x0_cov(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
VectorXd data(Vector2d(1, M_PI/4) ); // advance 1m
Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
int N = 8; // number of process() steps
@@ -353,13 +360,13 @@ TEST(Odom2d, KF_callback)
// Ceres wrapper
CeresManager ceres_manager(problem);
- // Origin Key Frame (in t1 to let processor motion to join the KF)
- FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0+dt, dt/2);
+ // Origin Key Frame
+ FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0, dt/2);
processor_odom2d->setOrigin(keyframe_0);
// Check covariance values
- Eigen::Vector3d integrated_pose = x0;
- Eigen::Matrix3d integrated_cov = x0_cov;
+ Eigen::Vector3d integrated_pose = Vector3d(0,0,0);
+ Eigen::Matrix3d integrated_cov = Eigen::Matrix3d::Identity() * 0.1;
Eigen::Vector3d integrated_delta = Eigen::Vector3d::Zero();
Eigen::Matrix3d integrated_delta_cov = Eigen::Matrix3d::Zero();
Eigen::MatrixXd Ju(3, 2);
@@ -415,8 +422,8 @@ TEST(Odom2d, KF_callback)
std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl;
- Vector3d x_split = processor_odom2d->getState(t_split);
- FrameBasePtr keyframe_2 = problem->emplaceKeyFrame(x_split, t_split);
+ Vector3d x_split = processor_odom2d->getState(t_split).vector("PO");
+ FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, t_split, x_split);
// there must be 2KF and one F
ASSERT_EQ(problem->getTrajectory()->getFrameList().size(), 2);
@@ -439,7 +446,7 @@ TEST(Odom2d, KF_callback)
// std::cout << report << std::endl;
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getStateVector() , integrated_pose_vector[n_split], 1e-6);
MatrixXd computed_cov;
ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6);
@@ -452,8 +459,8 @@ TEST(Odom2d, KF_callback)
problem->print(4,1,1,1);
- x_split = processor_odom2d->getState(t_split);
- FrameBasePtr keyframe_1 = problem->emplaceKeyFrame(x_split, t_split);
+ x_split = processor_odom2d->getState(t_split).vector("PO");
+ FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY, t_split, x_split);
ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
@@ -480,13 +487,13 @@ TEST(Odom2d, KF_callback)
// check the split KF
MatrixXd KF1_cov;
ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, KF1_cov));
- ASSERT_POSE2d_APPROX(keyframe_1->getState(), integrated_pose_vector[m_split], 1e-6);
- ASSERT_MATRIX_APPROX(KF1_cov, integrated_cov_vector [m_split], 1e-6);
+ ASSERT_POSE2d_APPROX(keyframe_1->getStateVector(), integrated_pose_vector[m_split], 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_cov, integrated_cov_vector [m_split], 1e-6);
// check other KF in the future of the split KF
MatrixXd KF2_cov;
ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov));
- ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getState(), integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getStateVector(), integrated_pose_vector[n_split], 1e-6);
ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6);
// Check full trajectory
@@ -494,9 +501,9 @@ TEST(Odom2d, KF_callback)
for (int n=1; n<=N; n++)
{
t += dt;
- WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).transpose());
+ WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).vector("PO").transpose());
WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose());
- ASSERT_POSE2d_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getState(t).vector("PO"), integrated_pose_vector[n], 1e-6);
}
}
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 625ea44beae19a6d91e3d1307f447e0a48c1d331..7778f7b46b98a94c204e67f65bc3ef72442388d8 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -6,7 +6,7 @@
*/
#include "core/utils/utils_gtest.h"
-#include "core/utils/logging.h"
+//#include "core/utils/logging.h"
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
@@ -45,7 +45,7 @@ TEST(Problem, create)
ASSERT_EQ(P, P->getMap()->getProblem());
// check frame structure through the state size
- ASSERT_EQ(P->getFrameStructureSize(), 10);
+ ASSERT_EQ(P->getFrameStructure(), "POV");
}
TEST(Problem, Sensors)
@@ -106,10 +106,12 @@ TEST(Problem, SetOrigin_PO_2d)
{
ProblemPtr P = Problem::create("PO", 2);
TimeStamp t0(0);
- Eigen::VectorXd x0(3); x0 << 1,2,3;
- Eigen::MatrixXd P0(Eigen::MatrixXd::Identity(3,3) * 0.1); // P0 is 0.1*Id
+ // Eigen::VectorXd x0(3); x0 << 1,2,3;
+ VectorComposite x0(Vector3d(1,2,3), "PO", {2,1});
+ // Eigen::MatrixXd P0(Eigen::MatrixXd::Identity(3,3) * 0.1); // P0 is 0.1*Id
+ VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
- P->setPriorFactor(x0, P0, t0, 1.0);
+ P->setPriorFactor(x0, s0, t0, 1.0);
P->print(4,1,1,1);
@@ -117,14 +119,14 @@ TEST(Problem, SetOrigin_PO_2d)
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
- ASSERT_MATRIX_APPROX(x0, P->getCurrentState(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0.vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL );
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
FrameBasePtr F = P->getLastFrame();
CaptureBasePtr C = F->getCaptureList().front();
- FeatureBasePtr fp = C->getFeatureList().front();
- FeatureBasePtr fo = C->getFeatureList().back();
+ FeatureBasePtr fo = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
+ FeatureBasePtr fp = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
FactorBasePtrList fac_list;
F->getFactorList(fac_list);
@@ -145,12 +147,15 @@ TEST(Problem, SetOrigin_PO_2d)
ASSERT_FALSE(fac->getCaptureOther());
ASSERT_FALSE(fac->getFeatureOther());
}
+ auto x0_vector = x0.vector("PO");
+ auto s0_vector = s0.vector("PO");
+ MatrixXd P0_matrix = (s0_vector.array() * s0_vector.array()).matrix().asDiagonal();
// check that the Feature measurement and covariance are the ones provided
- ASSERT_MATRIX_APPROX(x0.head<2>(), fp->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(x0.tail<1>(), fo->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0.topLeftCorner(2,2), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0.bottomRightCorner(1,1), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.head<2>(), fp->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.tail<1>(), fo->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.topLeftCorner(2,2), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.bottomRightCorner(1,1), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
@@ -159,23 +164,28 @@ TEST(Problem, SetOrigin_PO_3d)
{
ProblemPtr P = Problem::create("PO", 3);
TimeStamp t0(0);
- Eigen::VectorXd x0(7); x0 << 1,2,3,4,5,6,7; // not normalized quaternion, this is not what's tested
- Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
+ Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7; // not normalized quaternion, this is not what's tested
+ VectorComposite x0(vec7, "PO", {3,4});
+ // Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
+ Eigen::VectorXd vec6(6); vec6 << sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1);
+ VectorComposite s0(vec6, "PO", {3,3});
- P->setPriorFactor(x0, P0, t0, 1.0);
+ P->setPriorFactor(x0, s0, t0, 1.0);
// check that no sensor has been added
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
- ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_TRUE((x0.vector("PO") - P->getState().vector("PO")).isMuchSmallerThan(1, Constants::EPS_SMALL));
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
FrameBasePtr F = P->getLastFrame();
CaptureBasePtr C = F->getCaptureList().front();
- FeatureBasePtr fp = C->getFeatureList().front();
- FeatureBasePtr fo = C->getFeatureList().back();
+ // FeatureBasePtr fo = C->getFeatureList().front();
+ // FeatureBasePtr fp = C->getFeatureList().back();
+ FeatureBasePtr fo = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
+ FeatureBasePtr fp = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
FactorBasePtrList fac_list;
F->getFactorList(fac_list);
@@ -197,11 +207,14 @@ TEST(Problem, SetOrigin_PO_3d)
ASSERT_FALSE(fac->getFeatureOther());
}
+ auto x0_vector = x0.vector("PO");
+ auto s0_vector = s0.vector("PO");
+ MatrixXd P0_matrix = (s0_vector.array() * s0_vector.array()).matrix().asDiagonal();
// check that the Feature measurement and covariance are the ones provided
- ASSERT_MATRIX_APPROX(x0.head<3>(), fp->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(x0.tail<4>(), fo->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0.topLeftCorner(3,3), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0.bottomRightCorner(3,3), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.head<3>(), fp->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.tail<4>(), fo->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.topLeftCorner(3,3), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.bottomRightCorner(3,3), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
@@ -210,9 +223,9 @@ TEST(Problem, emplaceFrame_factory)
{
ProblemPtr P = Problem::create("PO", 2);
- FrameBasePtr f0 = P->emplaceKeyFrame("PO", 2, VectorXd(3), TimeStamp(0.0));
- FrameBasePtr f1 = P->emplaceKeyFrame("PO", 3, VectorXd(7), TimeStamp(1.0));
- FrameBasePtr f2 = P->emplaceKeyFrame("POV", 3, VectorXd(10), TimeStamp(2.0));
+ FrameBasePtr f0 = P->emplaceFrame(KEY, 0, "PO" , 2, VectorXd(3) );
+ FrameBasePtr f1 = P->emplaceFrame(KEY, 1, "PO" , 3, VectorXd(7) );
+ FrameBasePtr f2 = P->emplaceFrame(KEY, 2, "POV", 3, VectorXd(10) );
// std::cout << "f0: type PO 2d? " << f0->getType() << std::endl;
// std::cout << "f1: type PO 3d? " << f1->getType() << std::endl;
@@ -251,7 +264,7 @@ TEST(Problem, StateBlocks)
auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// 2 state blocks, estimated
- auto KF = P->emplaceKeyFrame("PO", 3, xs3d, 0);
+ auto KF = P->emplaceFrame(KEY, 0, "PO", 3, xs3d );
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
// Notifications
@@ -310,7 +323,7 @@ TEST(Problem, Covariances)
// 4 state blocks, estimated
St->unfixExtrinsics();
- FrameBasePtr F = P->emplaceKeyFrame("PO", 3, xs3d, 0);
+ FrameBasePtr F = P->emplaceFrame(KEY, 0, "PO", 3, xs3d );
// set covariance (they are not computed without a solver)
P->addCovarianceBlock(F->getP(), Eigen::Matrix3d::Identity());
@@ -346,7 +359,7 @@ TEST(Problem, perturb)
int i = 0;
for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
{
- auto F = problem->emplaceKeyFrame(pose, t);
+ auto F = problem->emplaceFrame(KEY, t, pose );
if (i==0) F->fix();
for (int j = 0; j< 2 ; j++)
@@ -436,7 +449,7 @@ TEST(Problem, check)
int i = 0;
for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
{
- auto F = problem->emplaceKeyFrame(pose, t);
+ auto F = problem->emplaceFrame(KEY, t, pose);
if (i==0) F->fix();
for (int j = 0; j< 2 ; j++)
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 25da38b6b7452d810cb29c8dca795b8edcd27c18..a47822ae81f8df9f6682a91807aaaf9837e31a02 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -96,8 +96,10 @@ TEST(ProcessorBase, KeyFrameCallback)
// initialize
TimeStamp t(0.0);
- Vector3d x(0,0,0);
- Matrix3d P = Matrix3d::Identity() * 0.1;
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P = Matrix3d::Identity() * 0.1;
+ VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
problem->setPriorFactor(x, P, t, dt/2); // KF1
CaptureOdom2dPtr capt_odo = make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5,0));
@@ -132,7 +134,7 @@ TEST(ProcessorBase, KeyFrameCallback)
std::cout << "6\n";
// Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2d")==0 );
+ ASSERT_TRUE( problem->getLastKeyFrame()->getStructure().compare("PO")==0 );
}
}
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 7704ec8c4e720ae0060f22dfdccaed512383692e..ee908013b9eadd216363d528d7f60757ea331f29 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -58,10 +58,10 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
- virtual void statePlusDelta(const Eigen::VectorXd& _x,
+ virtual void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _dt,
- Eigen::VectorXd& _x_plus_delta) const
+ VectorComposite& _x_plus_delta) const
{
Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
}
@@ -268,7 +268,8 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
Matrix2d data_cov; data_cov . setIdentity();
Vector3d calib(1,1,1);
double dt = 1.0;
- VectorXd delta(3), x1(3), x2(3);
+ VectorXd delta(3);
+ VectorComposite x1("PO", {2,1}), x2("PO", {2,1});
MatrixXd delta_cov(3,3), J_delta_calib(3,3);
// 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2)
@@ -283,7 +284,7 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
x1 = x2;
processor->statePlusDelta(x1, delta, dt, x2); // 90
- ASSERT_MATRIX_APPROX(x2, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
+ ASSERT_MATRIX_APPROX(x2.vector("PO"), Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
data(0) = 0.50*intr->ticks_per_wheel_revolution / 4;
@@ -299,7 +300,7 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
x1 = x2;
processor->statePlusDelta(x1, delta, dt, x2); // 90
- ASSERT_MATRIX_APPROX(x2, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
+ ASSERT_MATRIX_APPROX(x2.vector("PO"), Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
}
TEST_F(ProcessorDiffDriveTest, process)
@@ -308,8 +309,10 @@ TEST_F(ProcessorDiffDriveTest, process)
Matrix2d data_cov; data_cov . setIdentity();
TimeStamp t = 0.0;
double dt = 1.0;
- Vector3d x(0,0,0);
- Matrix3d P; P.setIdentity();
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P; P.setIdentity();
+ VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
auto F0 = problem->setPriorFactor(x, P, t, 0.1);
processor->setOrigin(F0);
@@ -326,7 +329,7 @@ TEST_F(ProcessorDiffDriveTest, process)
t += dt;
C->setTimeStamp(t);
C->process();
- WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
+ WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose());
}
problem->print(4,1,1,1);
@@ -337,8 +340,10 @@ TEST_F(ProcessorDiffDriveTest, linear)
Vector2d data;
Matrix2d data_cov; data_cov . setIdentity();
TimeStamp t = 0.0;
- Vector3d x(0,0,0);
- Matrix3d P; P.setIdentity();
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P; P.setIdentity();
+ VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
auto F0 = problem->setPriorFactor(x, P, t, 0.1);
processor->setOrigin(F0);
@@ -350,12 +355,12 @@ TEST_F(ProcessorDiffDriveTest, linear)
auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
C->process();
- WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
+ WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose());
// radius is 1.0m, 100 ticks per revolution, so advanced distance is
double distance = 2 * M_PI * 1.0;
- ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3d(distance,0,0), 1e-6)
+ ASSERT_MATRIX_APPROX(processor->getState().vector("PO"), Vector3d(distance,0,0), 1e-6)
}
TEST_F(ProcessorDiffDriveTest, angular)
@@ -363,8 +368,10 @@ TEST_F(ProcessorDiffDriveTest, angular)
Vector2d data;
Matrix2d data_cov; data_cov . setIdentity();
TimeStamp t = 0.0;
- Vector3d x(0,0,0);
- Matrix3d P; P.setIdentity();
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P; P.setIdentity();
+ VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
auto F0 = problem->setPriorFactor(x, P, t, 0.1);
processor->setOrigin(F0);
@@ -376,7 +383,7 @@ TEST_F(ProcessorDiffDriveTest, angular)
auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
C->process();
- WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
+ WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose());
// this is a turn in place, so distance = 0;
double distance = 0.0;
@@ -384,7 +391,7 @@ TEST_F(ProcessorDiffDriveTest, angular)
// radius is 1.0m, 100 ticks per revolution, and wheel separation is 1m, so turn angle is
double angle = pi2pi(2 * M_PI * 1.0 / 0.5 / 5);
- ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3d(distance,0,angle), 1e-6)
+ ASSERT_MATRIX_APPROX(processor->getState().vector("PO"), Vector3d(distance,0,angle), 1e-6)
}
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index 1e8246ee2de40852e7ce3a3df31fec55c32d8619..9ca6f9850a22cafef2fc58168c1351910b07ebe2 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -29,10 +29,10 @@ public:
std::pair<FrameBasePtr,CaptureBasePtr> public_selectPairKC(){ return selectPairKC();};
protected:
- virtual bool voteComputeFeatures() { return true;};
- virtual bool voteSearchLoopClosure() { return true;};
- virtual bool detectFeatures(CaptureBasePtr cap) { return true;};
- virtual CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture)
+ virtual bool voteComputeFeatures() override { return true;};
+ virtual bool voteSearchLoopClosure() override { return true;};
+ virtual bool detectFeatures(CaptureBasePtr cap) override { return true;};
+ virtual CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) override
{
for (FrameBasePtr kf : *getProblem()->getTrajectory())
if (kf->isKey())
@@ -41,7 +41,8 @@ protected:
return cap;
return nullptr;
};
- virtual void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) {
+ virtual void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) override
+ {
std::cout << "factor created\n";
*factor_created = true;
};
@@ -77,14 +78,16 @@ TEST(ProcessorLoopClosure, installProcessor)
// initialize
TimeStamp t(0.0);
- Vector3d x(0,0,0);
- Matrix3d P = Matrix3d::Identity() * 0.1;
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P = Matrix3d::Identity() * 0.1;
+ VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
problem->setPriorFactor(x, P, t, dt/2); // KF1
// new KF
t += dt;
- auto kf = problem->emplaceKeyFrame(x, t); //KF2
+ auto kf = problem->emplaceFrame(KEY, t, x); //KF2
// emplace a capture in KF
auto capt_lc = CaptureBase::emplace<CaptureVoid>(kf, t, sens_lc);
proc_lc->captureCallback(capt_lc);
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index fa989be48e578e94046d10702197db0447f24094..839c8d1ffb27e04a58340b4221b4571e383aa4ec 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -71,7 +71,7 @@ class ProcessorMotion_test : public testing::Test{
params->cov_det = 100;
params->unmeasured_perturbation_std = 0.001;
processor = ProcessorBase::emplace<ProcessorOdom2dPublic>(sensor, params);
- capture = std::make_shared<CaptureMotion>("CaptureOdom2d", 0.0, sensor, data, data_cov, 3, 3, nullptr);
+ capture = std::make_shared<CaptureMotion>("CaptureOdom2d", 0.0, sensor, data, data_cov, nullptr);
}
virtual void TearDown(){}
@@ -95,10 +95,10 @@ TEST_F(ProcessorMotion_test, IntegrateStraightAutoPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<9,0,0).finished(), 1e-8);
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
}
@@ -106,9 +106,11 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
{
// Prior
TimeStamp t(0.0);
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
- auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01);
+ // Vector3d x0; x0 << 0, 0, 0;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
processor->setOrigin(KF_0);
data << 1, 0; // advance straight
@@ -121,18 +123,20 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<9,0,0).finished(), 1e-8);
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
{
// Prior
TimeStamp t(0.0);
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ // Vector3d x0; x0 << 0, 0, 0;
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
auto KF_0 = problem->setPriorFix(x0, t, 0.01);
processor->setOrigin(KF_0);
@@ -146,10 +150,10 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<9,0,0).finished(), 1e-8);
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateCircleAutoPrior)
@@ -169,19 +173,21 @@ TEST_F(ProcessorMotion_test, IntegrateCircleAutoPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<0,0,0).finished(), 1e-8);
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
{
// Prior
TimeStamp t(0.0);
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
- auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01);
+ // Vector3d x0; x0 << 0, 0, 0;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
@@ -194,18 +200,20 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<0,0,0).finished(), 1e-8);
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
{
// Prior
TimeStamp t(0.0);
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ // Vector3d x0; x0 << 0, 0, 0;
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
auto KF_0 = problem->setPriorFix(x0, t, 0.01);
processor->setOrigin(KF_0);
@@ -219,10 +227,10 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<0,0,0).finished(), 1e-8);
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
@@ -242,7 +250,7 @@ TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
SensorBasePtr S = processor->getSensor();
@@ -255,8 +263,6 @@ TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
t_target,
sensor,
data,
- 3,
- 3,
nullptr);
processor->splitBuffer(C_source,
@@ -272,9 +278,11 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
{
// Prior
TimeStamp t(0.0);
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
- auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01);
+ // Vector3d x0; x0 << 0, 0, 0;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
@@ -287,7 +295,7 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
SensorBasePtr S = processor->getSensor();
@@ -300,8 +308,6 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
t_target,
sensor,
data,
- 3,
- 3,
nullptr);
processor->splitBuffer(C_source,
@@ -317,8 +323,10 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
{
// Prior
TimeStamp t(0.0);
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ // Vector3d x0; x0 << 0, 0, 0;
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
auto KF_0 = problem->setPriorFix(x0, t, 0.01);
processor->setOrigin(KF_0);
@@ -332,7 +340,7 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
SensorBasePtr S = processor->getSensor();
@@ -345,8 +353,6 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
t_target,
sensor,
data,
- 3,
- 3,
nullptr);
processor->splitBuffer(C_source,
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 6210ff07938b29e0dbd30e8d8b2092c82e932366..382f08108aa7fcf503fddd53730382e11bae0b93 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -431,7 +431,7 @@ TEST(SolverManager, AddFactor)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
@@ -458,7 +458,7 @@ TEST(SolverManager, RemoveFactor)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
@@ -491,7 +491,7 @@ TEST(SolverManager, AddRemoveFactor)
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
@@ -525,8 +525,8 @@ TEST(SolverManager, DoubleRemoveFactor)
Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
- // Create (and add) factor pose 2d
- FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
+ // Create (and add) factor point 2d
+ FrameBasePtr F = P->emplaceFrame(KEY, TimeStamp(0), P->stateZero() );
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
@@ -572,7 +572,7 @@ TEST(SolverManager, MultiThreadingTruncatedNotifications)
while (true)
{
// Emplace Frame, Capture, feature and factor pose 2d
- FrameBasePtr F = P->emplaceKeyFrame(P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, TimeStamp(0), P->stateZero());
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4d5a420119526c2fc1984aecfc22e01b40d0084b
--- /dev/null
+++ b/test/gtest_state_composite.cpp
@@ -0,0 +1,886 @@
+/*
+ * gtest_state_composite.cpp
+ *
+ * Created on: Apr 6, 2020
+ * Author: jsola
+ */
+
+#include "core/state_block/state_composite.h"
+#include "core/state_block/state_quaternion.h"
+
+#include "core/utils/utils_gtest.h"
+
+using namespace wolf;
+using namespace std;
+
+
+class StateBlockCompositeInit : public testing::Test
+{
+ public:
+
+ StateBlockPtr sbp, sbv, sbx;
+ StateQuaternionPtr sbq;
+
+ StateBlockComposite states;
+
+ void SetUp() override
+ {
+ sbp = states.emplace("P", Vector3d(1,2,3));
+ sbv = states.emplace("V", Vector3d(4,5,6));
+ sbq = states.emplace<StateQuaternion>("Q", Vector4d(.5,.5,.5,.5));
+
+ sbx = std::make_shared<StateBlock>(Vector3d(7,8,9));
+ }
+};
+
+TEST_F(StateBlockCompositeInit, stateSize)
+{
+ ASSERT_EQ( states.stateSize() , 10 );
+ ASSERT_EQ( states.stateSize("PQ"), 7 );
+ ASSERT_EQ( states.stateSize("PV"), 6 );
+}
+
+TEST_F(StateBlockCompositeInit, stateVector)
+{
+ ASSERT_EQ( states.stateVector("PQ") , (VectorXd( 7) << 1,2,3,.5,.5,.5,.5) .finished() );
+ ASSERT_EQ( states.stateVector("PVQ"), (VectorXd(10) << 1,2,3,4,5,6,.5,.5,.5,.5).finished() );
+ ASSERT_EQ( states.stateVector("PQV"), (VectorXd(10) << 1,2,3,.5,.5,.5,.5,4,5,6).finished() );
+}
+
+TEST_F(StateBlockCompositeInit, emplace)
+{
+ // Emplaces called in SetUp()
+
+ // check 3 elements
+ ASSERT_EQ(states.getStateBlockMap().size(), 3);
+}
+
+TEST_F(StateBlockCompositeInit, has_key)
+{
+ ASSERT_TRUE(states.has("P"));
+ ASSERT_FALSE(states.has("X"));
+}
+
+TEST_F(StateBlockCompositeInit, has_sb)
+{
+ ASSERT_TRUE(states.has(sbp));
+ ASSERT_FALSE(states.has(sbx));
+}
+
+TEST_F(StateBlockCompositeInit, at)
+{
+ ASSERT_EQ(states.at("P"), sbp);
+
+ ASSERT_EQ(states.at("X"), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, set_sb)
+{
+ states.set("P", sbx);
+
+ ASSERT_EQ(states.at("P"), sbx);
+
+ states.set("P", sbp);
+
+ ASSERT_DEATH(states.set("X", sbx), "");
+
+ ASSERT_EQ(states.at("X"), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, set_vec)
+{
+ Vector3d p(11,22,33);
+ Vector3d x(55,66,77);
+
+ states.set("P", p);
+
+ ASSERT_MATRIX_APPROX(states.at("P")->getState(), p, 1e-20);
+
+ ASSERT_DEATH(states.set("X", x), "");
+
+ ASSERT_EQ(states.at("X"), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, set_vectors)
+{
+ Vector3d p(11,22,33);
+ Vector4d q(11,22,33,44); q.normalize();
+ Vector3d x(55,66,77);
+
+ states.setVectors("PQ", {p, q});
+
+ ASSERT_MATRIX_APPROX(states.at("P")->getState(), p, 1e-20);
+ ASSERT_MATRIX_APPROX(states.at("Q")->getState(), q, 1e-20);
+
+ ASSERT_DEATH(states.setVectors("PX", {p,x}), "");
+
+ ASSERT_EQ(states.at("X"), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, key_ref)
+{
+ std::string key;
+ ASSERT_TRUE(states.key(sbp, key));
+ ASSERT_EQ(key, "P");
+
+ ASSERT_FALSE(states.key(sbx, key));
+ ASSERT_EQ(key, "");
+}
+
+TEST_F(StateBlockCompositeInit, key_return)
+{
+ // existing key
+ ASSERT_EQ(states.key(sbp), "P");
+
+ // non existing key returns empty string
+ ASSERT_EQ(states.key(sbx), "");
+}
+
+TEST_F(StateBlockCompositeInit, find)
+{
+ auto it = states.find(sbp);
+ ASSERT_NE(it, states.getStateBlockMap().end());
+
+ it = states.find(sbx);
+ ASSERT_EQ(it, states.getStateBlockMap().end());
+}
+
+TEST_F(StateBlockCompositeInit, add)
+{
+ states.add("X", sbx);
+
+ ASSERT_EQ(states.at("X"), sbx);
+}
+
+TEST_F(StateBlockCompositeInit, remove)
+{
+ // remove existing block
+ states.remove("V");
+ ASSERT_EQ(states.getStateBlockMap().size(), 2);
+
+ // remove non existing block -- no effect
+ states.remove("X");
+ ASSERT_EQ(states.getStateBlockMap().size(), 2);
+}
+
+TEST_F(StateBlockCompositeInit, perturb)
+{
+ ASSERT_TRUE(states.at("P")->getState().isApprox(sbp->getState(), 1e-3));
+ ASSERT_TRUE(states.at("V")->getState().isApprox(sbv->getState(), 1e-3));
+ ASSERT_TRUE(states.at("Q")->getState().isApprox(sbq->getState(), 1e-3));
+
+ states.perturb(0.1);
+
+ // values have moved wrt original
+ ASSERT_FALSE(states.at("P")->getState().isApprox(Vector3d(1,2,3), 1e-3));
+ ASSERT_FALSE(states.at("V")->getState().isApprox(Vector3d(4,5,6), 1e-3));
+ ASSERT_FALSE(states.at("Q")->getState().isApprox(Vector4d(.5,.5,.5,.5), 1e-3));
+}
+
+TEST_F(StateBlockCompositeInit, setIdentity)
+{
+ ASSERT_TRUE(states.at("P")->getState().isApprox(sbp->getState(), 1e-3));
+ ASSERT_TRUE(states.at("V")->getState().isApprox(sbv->getState(), 1e-3));
+ ASSERT_TRUE(states.at("Q")->getState().isApprox(sbq->getState(), 1e-3));
+
+ states.setIdentity();
+
+ // values have moved wrt original
+ ASSERT_TRUE(states.at("P")->getState().isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(states.at("V")->getState().isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(states.at("Q")->getState().isApprox(Vector4d(0,0,0,1), 1e-10));
+}
+
+TEST_F(StateBlockCompositeInit, identity)
+{
+ VectorComposite v = states.identity();
+
+ ASSERT_TRUE(v.at("P").isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(v.at("V").isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(v.at("Q").isApprox(Vector4d(0,0,0,1), 1e-10));
+}
+
+TEST_F(StateBlockCompositeInit, fix)
+{
+ states.fix();
+
+ ASSERT_TRUE(states.at("P")->isFixed());
+ ASSERT_TRUE(states.at("V")->isFixed());
+ ASSERT_TRUE(states.at("Q")->isFixed());
+}
+
+TEST_F(StateBlockCompositeInit, unfix)
+{
+ states.fix();
+
+ ASSERT_TRUE(states.at("P")->isFixed());
+ ASSERT_TRUE(states.at("V")->isFixed());
+ ASSERT_TRUE(states.at("Q")->isFixed());
+
+ states.unfix();
+
+ ASSERT_FALSE(states.at("P")->isFixed());
+ ASSERT_FALSE(states.at("V")->isFixed());
+ ASSERT_FALSE(states.at("Q")->isFixed());
+}
+
+TEST_F(StateBlockCompositeInit, isFixed)
+{
+ states.fix();
+
+ ASSERT_TRUE(states.isFixed());
+
+ states.at("P")->unfix();
+
+ ASSERT_FALSE(states.isFixed());
+}
+
+
+TEST(VectorComposite, constructor_empty)
+{
+ VectorComposite v;
+ ASSERT_TRUE(v.empty());
+}
+
+TEST(VectorComposite, constructor_copy)
+{
+ VectorComposite u;
+ u.emplace("a", Vector2d(1,2));
+ u.emplace("b", Vector3d(3,4,5));
+
+ VectorComposite v(u);
+
+ ASSERT_FALSE(v.empty());
+
+ ASSERT_MATRIX_APPROX(u["a"], v["a"], 1e-20);
+ ASSERT_MATRIX_APPROX(u["b"], v["b"], 1e-20);
+}
+
+TEST(VectorComposite, constructor_from_list)
+{
+ VectorComposite v(Vector4d(1,2,3,4), "ab", {3,1});
+
+ ASSERT_MATRIX_APPROX(v.at("a"), Vector3d(1,2,3), 1e-20);
+ ASSERT_MATRIX_APPROX(v.at("b"), Vector1d(4), 1e-20);
+}
+
+TEST(VectorComposite, set)
+{
+ VectorComposite v;
+
+ v.set(Vector4d(1,2,3,4), "ab", {3,1});
+
+ ASSERT_MATRIX_APPROX(v.at("a"), Vector3d(1,2,3), 1e-20);
+ ASSERT_MATRIX_APPROX(v.at("b"), Vector1d(4), 1e-20);
+}
+
+TEST(VectorComposite, operatorStream)
+{
+ using namespace Eigen;
+
+ VectorComposite x;
+
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+
+ WOLF_DEBUG("X = ", x);
+}
+
+TEST(VectorComposite, operatorPlus)
+{
+ VectorComposite x, y;
+
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+ y.emplace("P", Vector2d(1,1));
+ y.emplace("O", Vector3d(2,2,2));
+
+ WOLF_DEBUG("x + y = ", x + y);
+
+ ASSERT_MATRIX_APPROX((x+y).at("P"), Vector2d(2,2), 1e-20);
+ ASSERT_MATRIX_APPROX((x+y).at("O"), Vector3d(4,4,4), 1e-20);
+}
+
+TEST(VectorComposite, operatorMinus)
+{
+ VectorComposite x, y;
+
+ x.emplace("P", Vector2d(3,3));
+ x.emplace("O", Vector3d(6,6,6));
+ y.emplace("P", Vector2d(1,1));
+ y.emplace("O", Vector3d(2,2,2));
+
+ WOLF_DEBUG("x = ", x);
+ WOLF_DEBUG("y = ", y);
+ WOLF_DEBUG("x - y = ", x - y);
+
+ ASSERT_MATRIX_APPROX((x-y).at("P"), Vector2d(2,2), 1e-20);
+ ASSERT_MATRIX_APPROX((x-y).at("O"), Vector3d(4,4,4), 1e-20);
+}
+
+TEST(VectorComposite, unary_Minus)
+{
+ VectorComposite x, y;
+
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+
+ WOLF_DEBUG("-x = ", -x);
+
+ ASSERT_MATRIX_APPROX((-x).at("P"), Vector2d(-1,-1), 1e-20);
+ ASSERT_MATRIX_APPROX((-x).at("O"), Vector3d(-2,-2,-2), 1e-20);
+}
+
+TEST(VectorComposite, size)
+{
+ VectorComposite x;
+
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+ x.emplace("V", Vector4d(3,3,3,3));
+
+ ASSERT_EQ(x.size("PO"), 5);
+ ASSERT_EQ(x.size("VO"), 7);
+ ASSERT_EQ(x.size("PVO"), 9);
+ ASSERT_EQ(x.size("OPV"), 9);
+}
+
+TEST(VectorComposite, stateVector)
+{
+ VectorComposite x;
+
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+ x.emplace("V", Vector4d(3,3,3,3));
+
+ VectorXd y(5); y<<1,1,2,2,2;
+ ASSERT_MATRIX_APPROX(x.vector("PO"), y, 1e-20);
+
+ y.resize(7);
+ y << 3,3,3,3,2,2,2;
+ ASSERT_MATRIX_APPROX(x.vector("VO"), y, 1e-20);
+
+ y.resize(9);
+ y << 1,1,3,3,3,3,2,2,2;
+ ASSERT_MATRIX_APPROX(x.vector("PVO"), y, 1e-20);
+}
+
+TEST(MatrixComposite, Constructor_empty)
+{
+ MatrixComposite M;
+
+ ASSERT_EQ(M.size() , 0);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Constructor_structure)
+{
+ MatrixComposite M("PO", "POV");
+
+ ASSERT_EQ(M.size() , 2);
+ ASSERT_EQ(M.at("P").size() , 3);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Constructor_structure_sizes)
+{
+ MatrixComposite M("PO", {3,4}, "POV", {3,4,3});
+
+ ASSERT_EQ(M.size() , 2);
+ ASSERT_EQ(M.at("P").size() , 3);
+
+ ASSERT_EQ(M.at("P","O").rows() , 3);
+ ASSERT_EQ(M.at("P","O").cols() , 4);
+
+ ASSERT_EQ(M.matrix("PO","POV").rows() , 7);
+ ASSERT_EQ(M.matrix("PO","POV").cols() , 10);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Constructor_eigenMatrix_structure_sizes)
+{
+ MatrixXd m ( MatrixXd::Random(7,10) );
+
+ MatrixComposite M(m, "PO", {3,4}, "POV", {3,4,3});
+
+ ASSERT_EQ(M.size() , 2);
+ ASSERT_EQ(M.at("P").size() , 3);
+
+ ASSERT_EQ(M.at("P","O").rows() , 3);
+ ASSERT_EQ(M.at("P","O").cols() , 4);
+
+ ASSERT_EQ(M.matrix("PO","POV").rows() , 7);
+ ASSERT_EQ(M.matrix("PO","POV").cols() , 10);
+
+ ASSERT_MATRIX_APPROX(M.at("P","O"), m.block(0,3,3,4), 1e-20);
+ ASSERT_MATRIX_APPROX(M.at("O","V"), m.block(3,7,4,3), 1e-20);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Constructor_eigenMatrix_structure_sizes_mismatches)
+{
+ MatrixXd m;
+
+// // input m has too few rows
+// m.setRandom(6,10);
+// MatrixComposite M1(m, "PO", {3,4}, "POV", {3,4,3});
+
+// // input m has too many rows
+// m.setRandom(8,10);
+// MatrixComposite M2(m, "PO", {3,4}, "POV", {3,4,3});
+
+// // input m has too few cols
+// m.setRandom(7,9) ;
+// MatrixComposite M3(m, "PO", {3,4}, "POV", {3,4,3});
+
+// // input m has too many cols
+// m.setRandom(7,11) ;
+// MatrixComposite M4(m, "PO", {3,4}, "POV", {3,4,3});
+
+}
+
+TEST(MatrixComposite, Zero)
+{
+ MatrixComposite M = MatrixComposite::zero("PO", {3,4}, "POV", {3,4,3});
+
+ ASSERT_MATRIX_APPROX(M.matrix("PO", "POV"), MatrixXd::Zero(7,10), 1e-20);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Identity)
+{
+ MatrixComposite M = MatrixComposite::identity("POV", {3,4,3});
+
+ ASSERT_MATRIX_APPROX(M.matrix("POV", "POV"), MatrixXd::Identity(10,10), 1e-20);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, emplace_operatorStream)
+{
+ MatrixComposite M;
+
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ ASSERT_TRUE(M.emplace("P", "P", Mpp));
+ ASSERT_TRUE(M.emplace("P", "O", Mpo));
+ ASSERT_TRUE(M.emplace("O", "P", Mop));
+ ASSERT_TRUE(M.emplace("O", "O", Moo));
+
+ cout << "M = " << M << endl;
+}
+
+//TEST(MatrixComposite, operatorBrackets)
+//{
+// MatrixComposite M;
+//
+// unsigned int psize, osize;
+// psize = 2;
+// osize = 3;
+//
+// MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+//
+// Mpp.setOnes();
+// Mpo.setOnes(); Mpo *= 2;
+// Mop.setOnes(); Mop *= 3;
+// Moo.setOnes(); Moo *= 4;
+//
+// M.emplace("P", "P", Mpp);
+// ASSERT_MATRIX_APPROX( M["P"]["P"], Mpp, 1e-20);
+//
+// M.emplace("P", "O", Mpo);
+// ASSERT_MATRIX_APPROX( M["P"]["O"], Mpo, 1e-20);
+//
+// // return default empty matrix if block not present
+// MatrixXd N = M["O"]["O"];
+// ASSERT_EQ(N.rows(), 0);
+// ASSERT_EQ(N.cols(), 0);
+//}
+
+//TEST(MatrixComposite, operatorParenthesis)
+//{
+// MatrixComposite M;
+//
+// unsigned int psize, osize;
+// psize = 2;
+// osize = 3;
+//
+// MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+//
+// Mpp.setOnes();
+// Mpo.setOnes(); Mpo *= 2;
+// Mop.setOnes(); Mop *= 3;
+// Moo.setOnes(); Moo *= 4;
+//
+// M.emplace("P", "P", Mpp);
+// ASSERT_MATRIX_APPROX( M("P", "P"), Mpp, 1e-20);
+//
+// M.emplace("P", "O", Mpo);
+// ASSERT_MATRIX_APPROX( M("P", "O"), Mpo, 1e-20);
+//
+// // return default empty matrix if block not present
+// MatrixXd N = M("O", "O");
+// ASSERT_EQ(N.rows(), 0);
+// ASSERT_EQ(N.cols(), 0);
+//}
+
+TEST(MatrixComposite, operatorAt)
+{
+ MatrixComposite M;
+
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ ASSERT_MATRIX_APPROX( M.at("P", "P"), Mpp, 1e-20);
+
+ M.emplace("P", "O", Mpo);
+ ASSERT_MATRIX_APPROX( M.at("P", "O"), Mpo, 1e-20);
+
+ // error if block not present
+ ASSERT_DEATH(MatrixXd N = M.at("O", "O"), "");
+}
+
+TEST(MatrixComposite, productScalar)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ MatrixComposite M;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+
+
+ // right-multiply by scalar
+ MatrixComposite R = M * 1.2;
+ ASSERT_MATRIX_APPROX(R.matrix("PO","PO"), 1.2 * M.matrix("PO","PO"), 1e-20);
+
+
+ // left-multiply by scalar
+ MatrixComposite L = 1.2 * M;
+ ASSERT_MATRIX_APPROX(L.matrix("PO","PO"), 1.2 * M.matrix("PO","PO"), 1e-20);
+
+
+}
+
+TEST(MatrixComposite, productVector)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ VectorComposite x;
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+
+ cout << "x= " << x << endl;
+
+// WOLF_DEBUG("x = " , x);
+
+ MatrixComposite M;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+
+ VectorComposite y;
+
+ y = M * x;
+
+// WOLF_DEBUG("y = M * x = " , y);
+
+ /* M * x = y
+ * p o
+ * p [1 1 2 2 2] p [1] p [14]
+ * [1 1 2 2 2] [1] [14]
+ * [3 3 4 4 4] * [2] = [30]
+ * o [3 3 4 4 4] o [2] o [30]
+ * [3 3 4 4 4] [2] [30]
+ */
+
+ Vector2d yp(14,14);
+ Vector3d yo(30,30,30);
+
+ ASSERT_MATRIX_APPROX(y.at("P"), yp, 1e-20);
+ ASSERT_MATRIX_APPROX(y.at("O"), yo, 1e-20);
+
+ // throw if x has extra blocks
+ // x.emplace("V", Vector2d(3,3));
+ // ASSERT_DEATH(y = M * x , ""); // M * x --> does not die if x has extra blocks wrt M
+
+ // throw if x has missing blocks
+ // x.erase("O");
+ // cout << "x = " << x << endl;
+ // ASSERT_DEATH(y = M * x , ""); // M * x --> exception if x has missing blocks wrt M, not caught by ASSERT_DEATH
+
+}
+
+TEST(MatrixComposite, product)
+{
+ unsigned int psize, osize, vsize;
+ psize = 2;
+ osize = 1;
+ vsize = 2;
+
+ MatrixComposite M, N;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+
+ MatrixXd Noo(osize,osize), Nov(osize, vsize), Npo(psize,osize), Npv(psize,vsize);
+ Noo.setOnes();
+ Nov.setOnes(); Nov *= 2;
+ Npo.setOnes(); Npo *= 3;
+ Npv.setOnes(); Npv *= 4;
+
+ N.emplace("O", "O", Noo);
+ N.emplace("O", "V", Nov);
+ N.emplace("P", "O", Npo);
+ N.emplace("P", "V", Npv);
+ WOLF_DEBUG("N = " , N);
+
+ MatrixComposite MN;
+
+ MN = M * N;
+
+ WOLF_DEBUG("MN = M * N = " , MN);
+
+ /* M * N = MN
+ * p o o v o v
+ * p [1 1 2] p [3 4 4] p [ 8 12 12]
+ * [1 1 2] * [3 4 4] = [ 8 12 12]
+ * o [3 3 4] o [1 2 2] o [22 32 32]
+ */
+
+ MatrixXd MNpo(MatrixXd::Ones(psize,osize) * 8);
+ MatrixXd MNpv(MatrixXd::Ones(psize,vsize) * 12);
+ MatrixXd MNoo(MatrixXd::Ones(osize,osize) * 22);
+ MatrixXd MNov(MatrixXd::Ones(osize,vsize) * 32);
+
+ ASSERT_MATRIX_APPROX(MN.at("P","O"), MNpo, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at("P","V"), MNpv, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at("O","O"), MNoo, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at("O","V"), MNov, 1e-20);
+
+ ASSERT_TRUE(MN.check());
+}
+
+TEST(MatrixComposite, propagate)
+{
+ unsigned int psize, osize, vsize, wsize;
+ psize = 2;
+ osize = 3;
+ vsize = 4;
+ wsize = 1;
+
+ MatrixComposite Q, J;
+
+ MatrixXd Qpp(psize,psize), Qpo(psize, osize), Qop(osize,psize), Qoo(osize,osize);
+ Qpp.setOnes();
+ Qpo.setOnes(); Qpo *= 2;
+ Qop.setOnes(); Qop *= 2;
+ Qoo.setOnes(); Qoo *= 3;
+
+ Q.emplace("P", "P", Qpp);
+ Q.emplace("P", "O", Qpo);
+ Q.emplace("O", "P", Qop);
+ Q.emplace("O", "O", Qoo);
+ WOLF_DEBUG("Q = " , Q);
+
+ MatrixXd Jvp(vsize,psize), Jvo(vsize, osize), Jwp(wsize,psize), Jwo(wsize,osize);
+ Jvp.setOnes();
+ Jvo.setOnes(); Jvo *= 2;
+ Jwp.setOnes(); Jwp *= 3;
+ Jwo.setOnes(); Jwo *= 4;
+
+ J.emplace("V", "P", Jvp);
+ J.emplace("V", "O", Jvo);
+ J.emplace("W", "P", Jwp);
+ J.emplace("W", "O", Jwo);
+ WOLF_DEBUG("J = " , J);
+
+ MatrixComposite JQJt;
+
+ JQJt = J.propagate(Q);
+
+ WOLF_DEBUG("JQJt = J * Q * J.tr = " , JQJt);
+
+ WOLF_DEBUG("JQJt = J * Q * J.tr = \n" , JQJt.matrix("VW", "VW"));
+
+ ASSERT_MATRIX_APPROX(JQJt.matrix("VW", "VW"), J.matrix("VW","PO") * Q.matrix("PO","PO") * J.matrix("VW","PO").transpose(), 1e-8);
+
+ ASSERT_TRUE(JQJt.check());
+
+}
+
+TEST(MatrixComposite, sum)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 1;
+
+ MatrixComposite M, N;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+ N.emplace("P", "P", Mpp);
+ N.emplace("P", "O", Mpo);
+ N.emplace("O", "P", Mop);
+ N.emplace("O", "O", Moo);
+ WOLF_DEBUG("N = " , N);
+
+ MatrixComposite MpN;
+
+ MpN = M + N;
+
+ WOLF_DEBUG("MpN = M + N = " , MpN);
+
+ ASSERT_MATRIX_APPROX(MpN.at("P","P"), 2 * Mpp, 1e-10);
+ ASSERT_MATRIX_APPROX(MpN.at("P","O"), 2 * Mpo, 1e-10);
+ ASSERT_MATRIX_APPROX(MpN.at("O","P"), 2 * Mop, 1e-10);
+ ASSERT_MATRIX_APPROX(MpN.at("O","O"), 2 * Moo, 1e-10);
+
+}
+
+TEST(MatrixComposite, difference)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 1;
+
+ MatrixComposite M, N;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+ N.emplace("P", "P", Mpp);
+ N.emplace("P", "O", Mpo);
+ N.emplace("O", "P", Mop);
+ N.emplace("O", "O", Moo);
+ WOLF_DEBUG("N = " , N);
+
+ MatrixComposite MmN;
+
+ MmN = M - N;
+
+ WOLF_DEBUG("MmN = M - N = " , MmN);
+
+ ASSERT_MATRIX_APPROX(MmN.at("P","P"), Mpp * 0, 1e-10);
+ ASSERT_MATRIX_APPROX(MmN.at("P","O"), Mpo * 0, 1e-10);
+ ASSERT_MATRIX_APPROX(MmN.at("O","P"), Mop * 0, 1e-10);
+ ASSERT_MATRIX_APPROX(MmN.at("O","O"), Moo * 0, 1e-10);
+
+}
+
+TEST(MatrixComposite, unary_minus)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 1;
+
+ MatrixComposite M, N;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+
+ MatrixComposite m;
+
+ m = - M;
+
+ WOLF_DEBUG("m = - M = " , m);
+
+ ASSERT_MATRIX_APPROX(m.at("P","P"), - M.at("P","P"), 1e-10);
+ ASSERT_MATRIX_APPROX(m.at("P","O"), - M.at("P","O"), 1e-10);
+ ASSERT_MATRIX_APPROX(m.at("O","P"), - M.at("O","P"), 1e-10);
+ ASSERT_MATRIX_APPROX(m.at("O","O"), - M.at("O","O"), 1e-10);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ // restrict to a group of tests only
+ // ::testing::GTEST_FLAG(filter) = "VectorComposite.*";
+
+ // restrict to this test only
+ // ::testing::GTEST_FLAG(filter) = "MatrixComposite.propagate";
+// ::testing::GTEST_FLAG(filter) = "MatrixComposite.*";
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index fed1010e8d193c15e4bf247e24a98464340081e4..689c167522f7884d27b3e9fdde5e88fb01ba7b27 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -37,11 +37,11 @@ class TrackMatrixTest : public testing::Test
// unlinked frames
// Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
- F0 = FrameBase::createNonKeyFrame<FrameBase>(0.0, nullptr);
- F1 = FrameBase::createNonKeyFrame<FrameBase>(1.0, nullptr);
- F2 = FrameBase::createNonKeyFrame<FrameBase>(2.0, nullptr);
- F3 = FrameBase::createNonKeyFrame<FrameBase>(3.0, nullptr);
- F4 = FrameBase::createNonKeyFrame<FrameBase>(4.0, nullptr);
+ F0 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 0.0, nullptr);
+ F1 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 1.0, nullptr);
+ F2 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 2.0, nullptr);
+ F3 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 3.0, nullptr);
+ F4 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 4.0, nullptr);
// unlinked features
// Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 0fe2717c4ba795e215d7be8f36d8d6979c36f8d8..b8fc8608ccc1118bce118b28a7a9957ad688e16a 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -32,11 +32,10 @@ TEST(TrajectoryBase, ClosestKeyFrame)
// 1 2 3 4 time stamps
// --+-----+-----+-----+---> time
- FrameBasePtr F1 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 1);
- FrameBasePtr F2 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 2);
- //WARNING! MIGHT NEED TO ROLLBACK THIS TO AUXILIARY, FELLA
- FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 3, Eigen::Vector3d::Zero());
- FrameBasePtr F4 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 4, Eigen::Vector3d::Zero());
+ FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero() );
+ FrameBasePtr F2 = P->emplaceFrame(KEY, 2, Eigen::Vector3d::Zero() );
+ FrameBasePtr F3 = P->emplaceFrame(AUXILIARY, 3, Eigen::Vector3d::Zero() );
+ FrameBasePtr F4 = P->emplaceFrame(NON_ESTIMATED, 4, Eigen::Vector3d::Zero() );
FrameBasePtr KF; // closest key-frame queried
@@ -76,24 +75,21 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
// add F1
- FrameBasePtr F1 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 1); // 2 non-fixed
+ FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero()); // 2 non-fixed
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
// add F2
- FrameBasePtr F2 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 2); // 1 fixed, 1 not
+ FrameBasePtr F2 = P->emplaceFrame(KEY, 2, Eigen::Vector3d::Zero()); // 1 fixed, 1 not
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
// add F3
- FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(),
- P->getDim(),
- 3,
- Eigen::Vector3d::Zero()); // 1 fixed, 1 not
+ FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, 3, Eigen::Vector3d::Zero());
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 08cd9f8cfb240fe58bf94a8c52fb19aad365635f..507d2104e14bf43f57d563dc4873481322f57090 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -94,7 +94,7 @@ TEST(TreeManager, keyFrameCallback)
ASSERT_EQ(GM->n_KF_, 0);
- auto F0 = P->emplaceKeyFrame("PO", 3, VectorXd(7), TimeStamp(0.0));
+ auto F0 = P->emplaceFrame(KEY, 0, "PO", 3, VectorXd(7) );
P->keyFrameCallback(F0, nullptr, 0);
ASSERT_EQ(GM->n_KF_, 1);
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index f171dbcff79a31e07f71b1045f44ec71f9e62a8f..8909987f091831fddc686c585e827035ce629ebd 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -82,12 +82,12 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
auto F1 = P->getTrajectory()->getLastKeyFrame();
ASSERT_TRUE(F1 != nullptr);
- Vector7d state = F1->getState();
+ Vector7d state = F1->getStateVector();
Vector7d zero_disp(state);
Matrix6d cov = Matrix6d::Identity();
// FRAME 2 ----------------------------------------------------------
- auto F2 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(2));
+ auto F2 = P->emplaceFrame(KEY, TimeStamp(2), "PO", 3, state);
P->keyFrameCallback(F2, nullptr, 0);
// absolute factor
@@ -103,7 +103,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
ASSERT_FALSE(F1->isRemoving());
// FRAME 3 ----------------------------------------------------------
- auto F3 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(3));
+ auto F3 = P->emplaceFrame(KEY, TimeStamp(3), "PO", 3, state);
P->keyFrameCallback(F3, nullptr, 0);
// absolute factor
@@ -119,7 +119,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
ASSERT_FALSE(F1->isRemoving());
// FRAME 4 ----------------------------------------------------------
- auto F4 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(4));
+ auto F4 = P->emplaceFrame(KEY, TimeStamp(4), "PO", 3, state);
P->keyFrameCallback(F4, nullptr, 0);
// absolute factor
@@ -139,7 +139,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
ASSERT_TRUE(F2->isFixed()); //Fixed
// FRAME 5 ----------------------------------------------------------
- auto F5 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(5));
+ auto F5 = P->emplaceFrame(KEY, TimeStamp(5), "PO", 3, state);
P->keyFrameCallback(F5, nullptr, 0);
// absolute factor
@@ -178,12 +178,12 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
auto F1 = P->getTrajectory()->getLastKeyFrame();
ASSERT_TRUE(F1 != nullptr);
- Vector7d state = F1->getState();
+ Vector7d state = F1->getStateVector();
Vector7d zero_disp(state);
Matrix6d cov = Matrix6d::Identity();
// FRAME 2 ----------------------------------------------------------
- auto F2 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(2));
+ auto F2 = P->emplaceFrame(KEY, TimeStamp(2), "PO", 3, state);
P->keyFrameCallback(F2, nullptr, 0);
// absolute factor
@@ -199,7 +199,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
ASSERT_FALSE(F1->isRemoving());
// FRAME 3 ----------------------------------------------------------
- auto F3 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(3));
+ auto F3 = P->emplaceFrame(KEY, TimeStamp(3), "PO", 3, state);
P->keyFrameCallback(F3, nullptr, 0);
// absolute factor
@@ -215,7 +215,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
ASSERT_FALSE(F1->isRemoving());
// FRAME 4 ----------------------------------------------------------
- auto F4 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(4));
+ auto F4 = P->emplaceFrame(KEY, TimeStamp(4), "PO", 3, state);
P->keyFrameCallback(F4, nullptr, 0);
// absolute factor
@@ -235,7 +235,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
ASSERT_FALSE(F2->isFixed()); //Not fixed
// FRAME 5 ----------------------------------------------------------
- auto F5 = P->emplaceKeyFrame("PO", 3, state, TimeStamp(5));
+ auto F5 = P->emplaceFrame(KEY, TimeStamp(5), "PO", 3, state);
P->keyFrameCallback(F5, nullptr, 0);
// absolute factor
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index 085015b89c62f913f7bbf6ab3d6241ddde132dff..d8f443220bf71786c1e95ca77ae9e68705d1d576 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/test/yaml/params_tree_manager1.yaml
@@ -4,8 +4,16 @@ config:
dimension: 3
prior:
mode: "factor"
- state: [0,0,0,0,0,0,1,0,0,0]
- cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
+ # state: [0,0,0,0,0,0,1,0,0,0]
+ # cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [0.31, 0.31, 0.31]
time_tolerance: 0.1
tree_manager:
type: "TreeManagerDummy"
@@ -38,4 +46,4 @@ config:
dist_traveled: 0.5 # meters
angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
- unmeasured_perturbation_std: 0.00111
\ No newline at end of file
+ unmeasured_perturbation_std: 0.00111
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
index 80b5af47ac509b0746ffb8f66775d87742e1e927..b61f9936d4ccb8fc705d2ca92717f2a2ad67c402 100644
--- a/test/yaml/params_tree_manager2.yaml
+++ b/test/yaml/params_tree_manager2.yaml
@@ -4,8 +4,16 @@ config:
dimension: 3
prior:
mode: "factor"
- state: [0,0,0,0,0,0,1,0,0,0]
- cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
+ # state: [0,0,0,0,0,0,1,0,0,0]
+ # cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [0.31, 0.31, 0.31]
time_tolerance: 0.1
tree_manager:
type: "None"
@@ -37,4 +45,4 @@ config:
dist_traveled: 0.5 # meters
angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
- unmeasured_perturbation_std: 0.00111
\ No newline at end of file
+ unmeasured_perturbation_std: 0.00111
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index 3a0f421c5c0048125d0a8afa742e873f79e703f6..96ad94f72af7e8de1b9200a8e5ea44e3bc952a85 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -4,8 +4,14 @@ config:
dimension: 3
prior:
mode: "factor"
- state: [0,0,0,0,0,0,1]
- cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
+ # state: [0,0,0,0,0,0,1]
+ # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindow"
@@ -40,4 +46,4 @@ config:
dist_traveled: 0.5 # meters
angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
- unmeasured_perturbation_std: 0.00111
\ No newline at end of file
+ unmeasured_perturbation_std: 0.00111
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index 894dadf651dfe18919881ca44a296f3df447246a..9ae33a2af5ab0695bd44948d33d2f9aa104cbc74 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -4,8 +4,14 @@ config:
dimension: 3
prior:
mode: "factor"
- state: [0,0,0,0,0,0,1]
- cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
+ # state: [0,0,0,0,0,0,1]
+ # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindow"
@@ -40,4 +46,4 @@ config:
dist_traveled: 0.5 # meters
angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
- unmeasured_perturbation_std: 0.00111
\ No newline at end of file
+ unmeasured_perturbation_std: 0.00111