diff --git a/CMakeLists.txt b/CMakeLists.txt
index 42622856d59417ed225d4950f9bb0a87f62e5ae7..b9f3ad7358ad2dad416d6cf0c8923f23d5a90ece 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -227,6 +227,7 @@ SET(HDRS_FACTOR
include/core/factor/factor_relative_pose_2d.h
include/core/factor/factor_relative_pose_2d_with_extrinsics.h
include/core/factor/factor_relative_pose_3d.h
+ include/core/factor/factor_pose_3d_with_extrinsics.h
)
SET(HDRS_FEATURE
include/core/feature/feature_base.h
@@ -250,6 +251,7 @@ SET(HDRS_PROCESSOR
include/core/processor/processor_motion.h
include/core/processor/processor_odom_2d.h
include/core/processor/processor_odom_3d.h
+ include/core/processor/processor_pose.h
include/core/processor/processor_tracker.h
include/core/processor/processor_tracker_feature.h
include/core/processor/processor_tracker_landmark.h
@@ -261,6 +263,7 @@ SET(HDRS_SENSOR
include/core/sensor/factory_sensor.h
include/core/sensor/sensor_odom_2d.h
include/core/sensor/sensor_odom_3d.h
+ include/core/sensor/sensor_pose.h
)
SET(HDRS_SOLVER
include/core/solver/solver_manager.h
@@ -344,6 +347,7 @@ SET(SRCS_PROCESSOR
src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp
src/processor/processor_odom_3d.cpp
+ src/processor/processor_pose.cpp
src/processor/processor_tracker.cpp
src/processor/processor_tracker_feature.cpp
src/processor/processor_tracker_landmark.cpp
@@ -354,6 +358,7 @@ SET(SRCS_SENSOR
src/sensor/sensor_diff_drive.cpp
src/sensor/sensor_odom_2d.cpp
src/sensor/sensor_odom_3d.cpp
+ src/sensor/sensor_pose.cpp
)
SET(SRCS_SOLVER
src/solver/solver_manager.cpp
@@ -367,6 +372,7 @@ SET(SRCS_YAML
src/yaml/processor_odom_3d_yaml.cpp
src/yaml/sensor_odom_2d_yaml.cpp
src/yaml/sensor_odom_3d_yaml.cpp
+ src/yaml/sensor_pose_yaml.cpp
)
#OPTIONALS
#optional HDRS and SRCS
diff --git a/include/core/capture/capture_pose.h b/include/core/capture/capture_pose.h
index 79298fe4d63b03b3c0ad830f4265c106ca30ae74..07db9e0351ddb1e73ea8877c1a31ddc456e7dacd 100644
--- a/include/core/capture/capture_pose.h
+++ b/include/core/capture/capture_pose.h
@@ -28,6 +28,8 @@ class CapturePose : public CaptureBase
~CapturePose() override;
virtual void emplaceFeatureAndFactor();
+ Eigen::VectorXd getData(){ return data_;}
+ Eigen::MatrixXd getDataCovariance(){ return data_covariance_;}
};
diff --git a/include/core/factor/factor_pose_3d_with_extrinsics.h b/include/core/factor/factor_pose_3d_with_extrinsics.h
new file mode 100644
index 0000000000000000000000000000000000000000..f726291a9a0480dd2edc71d137511c543d4d3886
--- /dev/null
+++ b/include/core/factor/factor_pose_3d_with_extrinsics.h
@@ -0,0 +1,88 @@
+#ifndef FACTOR_POSE_3D_WITH_EXTRINSICS_THETA_H_
+#define FACTOR_POSE_3D_WITH_EXTRINSICS_THETA_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorPose3dWithExtrinsics);
+
+//class
+class FactorPose3dWithExtrinsics : public FactorAutodiff<FactorPose3dWithExtrinsics, 6, 3, 4, 3, 4>
+{
+ public:
+ FactorPose3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorPose3dWithExtrinsics, 6, 3, 4, 3, 4>("FactorPose3dWithExtrinsics",
+ _top,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
+ {
+ assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
+ //
+ }
+
+ ~FactorPose3dWithExtrinsics() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _p,
+ const T* const _q,
+ const T* const _sp,
+ const T* const _sq,
+ T* _residuals) const;
+};
+
+template<typename T>
+inline bool FactorPose3dWithExtrinsics::operator ()(const T* const _p,
+ const T* const _q,
+ const T* const _sp,
+ const T* const _sq,
+ T* _residuals) const
+{
+ // MAPS
+ Eigen::Map<const Eigen::Matrix<T,3,1> > w_p_wb(_p);
+ Eigen::Map<const Eigen::Quaternion<T> > w_q_b(_q);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > b_p_bm(_sp);
+ Eigen::Map<const Eigen::Quaternion<T> > b_q_m(_sq);
+
+ // measurements
+ Eigen::Vector3d w_p_wm(getMeasurement().data() + 0); // measurements
+ Eigen::Quaterniond w_q_m (getMeasurement().data() + 3); // measurements
+
+ // Transformation definitions:
+ // w_p_wm, w_q_m: world to measurement frame (robot attached mocap frame)
+ // w_p_wb, w_q_b: world to robot base frame
+ // b_p_bm, b_q_m: base to measurement frame (extrinsics)
+ // Error function:
+ // err = w_T_m |-| (w_T_b*b_T_m)
+
+ Eigen::Matrix<T, 6, 1> err; // error
+ err.head(3) = w_p_wm - (w_p_wb + w_q_b*b_p_bm);
+ err.tail(3) = q2v((w_q_b * b_q_m).conjugate() * w_q_m.cast<T>());
+
+ // Residuals
+ Eigen::Map<Eigen::Matrix<T,6,1> > res(_residuals);
+ res = getMeasurementSquareRootInformationUpper().cast<T>() * err;
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index c8945c8036ff54dc2e0fe2243b3ec0919c66eba4..bbb9d109f4ea76919f4c9ddac9c5960eb94de141 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -549,11 +549,11 @@ T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_t
return nullptr;
// Checking on rbegin() since packs are ordered in time
- // Return last pack if is older than time stamp
+ // Return last pack if is newer than time stamp
if (container_.rbegin()->first > _time_stamp)
return container_.rbegin()->second;
- // Return last pack if despite being newer, it is within the time tolerance
+ // Return last pack if despite being older, it is within the time tolerance
if (simpleCheckTimeTolerance(container_.rbegin()->first, _time_stamp, _time_tolerance))
return container_.rbegin()->second;
diff --git a/include/core/processor/processor_pose.h b/include/core/processor/processor_pose.h
new file mode 100644
index 0000000000000000000000000000000000000000..f8d4833807714dd5303db309e4f13e4710f24e7f
--- /dev/null
+++ b/include/core/processor/processor_pose.h
@@ -0,0 +1,93 @@
+#ifndef PROCESSOR_POSE_NOMOVE_H
+#define PROCESSOR_POSE_NOMOVE_H
+
+// Wolf
+#include "core/sensor/sensor_base.h"
+#include "core/processor/processor_base.h"
+
+#include "core/capture/capture_pose.h"
+#include "core/sensor/sensor_pose.h"
+#include "core/factor/factor_pose_3d_with_extrinsics.h"
+
+namespace wolf {
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorPose);
+
+struct ParamsProcessorPose : public ParamsProcessorBase
+{
+ ParamsProcessorPose() = default;
+ ParamsProcessorPose(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server)
+ {
+ }
+ ~ParamsProcessorPose() override = default;
+ std::string print() const override
+ {
+ return "\n" + ParamsProcessorBase::print() + "\n";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorPose);
+
+//class
+class ProcessorPose : public ProcessorBase{
+ public:
+ ProcessorPose(ParamsProcessorPosePtr _params_pfnomove);
+ ~ProcessorPose() override = default;
+ WOLF_PROCESSOR_CREATE(ProcessorPose, ParamsProcessorPose);
+
+ void configure(SensorBasePtr _sensor) override;
+
+ void processCapture(CaptureBasePtr _incoming) override;
+ void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
+ bool triggerInCapture(CaptureBasePtr) const override;
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override;
+ bool storeKeyFrame(FrameBasePtr) override;
+ bool storeCapture(CaptureBasePtr) override;
+ bool voteForKeyFrame() const override;
+
+ void createFactorIfNecessary();
+
+ protected:
+ ParamsProcessorPosePtr params_pfnomove_;
+};
+
+
+
+inline bool ProcessorPose::triggerInKeyFrame(FrameBasePtr _keyframe_ptr,
+ const double& _time_tolerance) const
+{
+ return true;
+}
+
+inline bool ProcessorPose::triggerInCapture(CaptureBasePtr _capture) const
+{
+ return true;
+}
+
+inline bool ProcessorPose::storeKeyFrame(FrameBasePtr)
+{
+ return true;
+}
+
+inline bool ProcessorPose::storeCapture(CaptureBasePtr)
+{
+ return true;
+}
+
+inline bool ProcessorPose::voteForKeyFrame() const
+{
+ return false;
+}
+
+} /* namespace wolf */
+
+/////////////////////////////////////////////////////////
+// IMPLEMENTATION. Put your implementation includes here
+/////////////////////////////////////////////////////////
+
+
+namespace wolf{
+
+} // namespace wolf
+
+#endif // PROCESSOR_POSE_NOMOVE_H
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
new file mode 100644
index 0000000000000000000000000000000000000000..63f16d1e42b1994632478f293bd2d9c0a9ba2fe1
--- /dev/null
+++ b/include/core/sensor/sensor_pose.h
@@ -0,0 +1,79 @@
+/**
+ * \file sensor_pose.h
+ *
+ * Created on: Feb 18, 2020
+ * \author: mfourmy
+ */
+
+#ifndef SRC_SENSOR_POSE_H_
+#define SRC_SENSOR_POSE_H_
+
+//wolf includes
+#include "core/sensor/sensor_base.h"
+#include "core/utils/params_server.h"
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPose);
+
+struct ParamsSensorPose : public ParamsSensorBase
+{
+ double std_p = 1; // m
+ double std_o = 1; // rad
+ ParamsSensorPose()
+ {
+ //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
+ }
+ ParamsSensorPose(std::string _unique_name, const ParamsServer& _server):
+ ParamsSensorBase(_unique_name, _server)
+ {
+ std_p = _server.getParam<double>(prefix + _unique_name + "/std_p");
+ std_o = _server.getParam<double>(prefix + _unique_name + "/std_o");
+ }
+ std::string print() const override
+ {
+ return ParamsSensorBase::print() + "\n"
+ + "std_p: " + std::to_string(std_p) + "\n"
+ + "std_o: " + std::to_string(std_o) + "\n";
+ }
+ ~ParamsSensorPose() override = default;
+};
+
+WOLF_PTR_TYPEDEFS(SensorPose);
+
+class SensorPose : public SensorBase
+{
+ protected:
+ double std_p_; // standard deviation of translation measurements
+ double std_o_; // standard deviation of orientation measurements
+
+ public:
+ SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& params);
+ SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr params);
+ WOLF_SENSOR_CREATE(SensorPose, ParamsSensorPose, 7);
+
+ ~SensorPose() override;
+
+ double getStdP() const;
+ double getStdO() const;
+
+};
+
+inline double SensorPose::getStdP() const
+{
+ return std_p_;
+}
+
+inline double SensorPose::getStdO() const
+{
+ return std_o_;
+}
+
+// inline Matrix6d SensorPose::computeDataCov() const
+// {
+// return (Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal();
+// }
+
+} /* namespace wolf */
+
+#endif /* SRC_SENSOR_POSE_H_ */
diff --git a/src/processor/processor_pose.cpp b/src/processor/processor_pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..806021722dd5ad3b25220315a71cc578f02ed0bf
--- /dev/null
+++ b/src/processor/processor_pose.cpp
@@ -0,0 +1,113 @@
+/**
+ * \file processor_pose.cpp
+ *
+ * Created on: Feb 19, 2020
+ * \author: mfourmy
+ */
+
+#include "core/processor/processor_pose.h"
+
+
+namespace wolf{
+
+
+inline ProcessorPose::ProcessorPose(ParamsProcessorPosePtr _params_pfnomove) :
+ ProcessorBase("ProcessorPose", 3, _params_pfnomove),
+ params_pfnomove_(std::make_shared<ParamsProcessorPose>(*_params_pfnomove))
+{
+
+}
+
+void ProcessorPose::configure(SensorBasePtr _sensor)
+{
+}
+
+void ProcessorPose::createFactorIfNecessary(){
+ auto sensor_pose = std::static_pointer_cast<SensorPose>(getSensor());
+
+ while (buffer_pack_kf_.size() >= 1)
+ {
+ auto kf_it = buffer_pack_kf_.getContainer().begin();
+ TimeStamp t = kf_it->first;
+ double time_tolerance = std::min(getTimeTolerance(), kf_it->second->time_tolerance);
+ if (getTimeTolerance() == 0.0){
+ WOLF_WARN("Time tolerance set to zero -> value not used");
+ time_tolerance = kf_it->second->time_tolerance;
+ }
+ auto cap_it = buffer_capture_.selectIterator(t, time_tolerance);
+
+ // if capture with corresponding timestamp is not found, stop and assume you will get it later
+ if (cap_it == buffer_capture_.getContainer().end())
+ {
+ return;
+ }
+ else
+ {
+ // if a corresponding capture exists, link it to the KF and create a factor
+ auto cap = std::static_pointer_cast<CapturePose>(cap_it->second);
+ cap->link(kf_it->second->key_frame);
+ FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "Pose", cap->getData(), cap->getDataCovariance());
+ FactorPose3dWithExtrinsicsPtr fac = FactorBase::emplace<FactorPose3dWithExtrinsics>(feat, feat, nullptr, false, TOP_MOTION);
+
+ // erase removes range [first, last): it does not removes last
+ // so increment the iterator so that it points to the next element in the container
+ buffer_pack_kf_.getContainer().erase(buffer_pack_kf_.getContainer().begin(), std::next(kf_it));
+ buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), std::next(cap_it));
+ }
+ }
+}
+
+
+inline void ProcessorPose::processCapture(CaptureBasePtr _capture)
+{
+ if (!_capture)
+ {
+ WOLF_ERROR("Received capture is nullptr.");
+ return;
+ }
+ // nothing to do if any of the two buffer is empty
+ if(buffer_pack_kf_.empty()){
+ WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
+ return;
+ }
+ if(buffer_pack_kf_.empty()){
+ WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
+ return;
+ }
+
+ createFactorIfNecessary();
+
+}
+
+inline void ProcessorPose::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+{
+ if (!_keyframe_ptr)
+ {
+ WOLF_ERROR("Received KF is nullptr.");
+ return;
+ }
+ // nothing to do if any of the two buffer is empty
+ if(buffer_pack_kf_.empty()){
+ WOLF_DEBUG("ProcessorPose: KF pack buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ return;
+ }
+ if(buffer_pack_kf_.empty()){
+ WOLF_DEBUG("ProcessorPose: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ return;
+ }
+
+ createFactorIfNecessary();
+}
+
+
+
+
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorPose);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorPose);
+} // namespace wolf
diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9cd65ccc8b5e95b88234ccd5ef5e2a5f4683fca0
--- /dev/null
+++ b/src/sensor/sensor_pose.cpp
@@ -0,0 +1,46 @@
+/**
+ * \file sensor_pose.cpp
+ *
+ * Created on: Feb 18, 2020
+ * \author: mfourmy
+ */
+
+#include "core/sensor/sensor_pose.h"
+
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/math/rotations.h"
+
+namespace wolf {
+
+SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& _intrinsics) :
+ SensorBase("SensorPose", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
+ std_p_(_intrinsics.std_p),
+ std_o_(_intrinsics.std_o)
+{
+ assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
+
+ noise_cov_ = Matrix6d::Identity();
+ // noise_cov_ = (Eigen::Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal();
+ setNoiseCov(noise_cov_); // sets also noise_std_
+}
+
+SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr _intrinsics) :
+ SensorPose(_extrinsics_pq, *_intrinsics)
+{
+ //
+}
+
+SensorPose::~SensorPose()
+{
+ //
+}
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorPose);
+WOLF_REGISTER_SENSOR_AUTO(SensorPose);
+}
diff --git a/src/yaml/sensor_pose_yaml.cpp b/src/yaml/sensor_pose_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4b9521595c7d5ac100e7d6dd72b266dc884d7336
--- /dev/null
+++ b/src/yaml/sensor_pose_yaml.cpp
@@ -0,0 +1,47 @@
+/**
+ * \file sensor_pose_yaml.cpp
+ *
+ * Created on: Feb 18, 2020
+ * \author: mfourmy
+ */
+
+// wolf yaml
+#include "core/yaml/yaml_conversion.h"
+
+// wolf
+#include "core/sensor/sensor_pose.h"
+#include "core/common/factory.h"
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+
+namespace
+{
+static ParamsSensorBasePtr createParamsSensorPose(const std::string & _filename_dot_yaml)
+{
+ YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+ if (config["type"].as<std::string>() == "SensorPose")
+ {
+ ParamsSensorPosePtr params = std::make_shared<ParamsSensorPose>();
+
+ params->std_p = config["std_p"].as<double>();
+ params->std_o = config["std_o"].as<double>();
+
+ return params;
+ }
+
+ std::cout << "Bad configuration file. No sensor type found." << std::endl;
+ return nullptr;
+}
+
+// Register in the FactorySensor
+const bool WOLF_UNUSED registered_odom_3d_intr = FactoryParamsSensor::registerCreator("SensorPose", createParamsSensorPose);
+
+} // namespace [unnamed]
+
+} // namespace wolf
+
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 8deec25dbf4bbb80a78298e77301a4c24ecfac35..885c1f53a456deb4b74392d011d560cb7216bafe 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -241,6 +241,11 @@ target_link_libraries(gtest_odom_2d ${PLUGIN_NAME})
wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
target_link_libraries(gtest_processor_odom_3d ${PLUGIN_NAME})
+# FactorPose3dWithExtrinsics class test
+wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
+target_link_libraries(gtest_processor_and_factor_pose_3d_with_extrinsics ${PLUGIN_NAME})
+
+
# ProcessorTrackerFeatureDummy class test
wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
target_link_libraries(gtest_processor_tracker_feature_dummy ${PLUGIN_NAME} dummy)
@@ -253,6 +258,10 @@ target_link_libraries(gtest_processor_tracker_landmark_dummy ${PLUGIN_NAME} dumm
wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
target_link_libraries(gtest_sensor_diff_drive ${PLUGIN_NAME})
+# SensorDiffDriveSelfcalib class test
+wolf_add_gtest(gtest_sensor_pose gtest_sensor_pose.cpp)
+target_link_libraries(gtest_sensor_pose ${PLUGIN_NAME})
+
IF (Ceres_FOUND)
# SolverCeres test
wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)
diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..444fc643caa29559441c9c64911048285a344989
--- /dev/null
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -0,0 +1,318 @@
+/**
+ * \file gtest_factor_pose_with_extrinsics.cpp
+ *
+ * Created on: Feb 19, 2020
+ * \author: mfourmy
+ */
+
+#include "core/utils/utils_gtest.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/capture/capture_base.h"
+#include "core/capture/capture_pose.h"
+#include "core/sensor/sensor_pose.h"
+#include "core/processor/processor_pose.h"
+#include "core/factor/factor_relative_pose_3d.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/factor/factor_pose_3d_with_extrinsics.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+const Vector3d zero3 = Vector3d::Zero();
+
+
+
+class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
+{
+ /**
+ Factor graph implemented common for all the tests
+ 2 KF is not enough since the SensorPose extrinsics translation would be unobservable along the first KF1-KF2 transformation axis of rotation
+ (KF1)----|Odom3d|----(KF2)----|Odom3d|----(KF3)
+ | | |
+ | | |
+ |Pose3dWE| |Pose3dWE| |Pose3dWE|
+ \ /
+ \ /
+ \____________(Extrinsics)_______________/
+ */
+
+ // In this class, the poses of the 3 KF are chosen randomly and then measurement data for the factors are
+ // derived from random extrinsics
+ // The problem and Pose Sensor/Processor are implemented
+
+ public:
+
+ ProblemPtr problem_;
+ SolverCeresPtr solver_;
+ SensorBasePtr sensor_pose_;
+ FrameBasePtr KF1_;
+ FrameBasePtr KF2_;
+ FrameBasePtr KF3_;
+
+ Vector7d pose1_;
+ Vector7d pose2_;
+ Vector7d pose3_;
+ Vector7d pose_12_;
+ Vector7d pose_23_;
+ Vector3d b_p_bm_;
+ Quaterniond b_q_m_;
+
+ Vector7d pose1_meas_;
+ Vector7d pose2_meas_;
+ Vector7d pose3_meas_;
+
+ void SetUp() override
+ {
+ // 2 random 3d positions
+ Vector3d w_p_wb1 = Vector3d::Random();
+ Quaterniond w_q_b1 = Quaterniond::UnitRandom();
+ Vector3d w_p_wb2 = Vector3d::Random();
+ Quaterniond w_q_b2 = Quaterniond::UnitRandom();
+ Vector3d w_p_wb3 = Vector3d::Random();
+ Quaterniond w_q_b3 = Quaterniond::UnitRandom();
+
+ // pose vectors
+ pose1_ << w_p_wb1, w_q_b1.coeffs();
+ pose2_ << w_p_wb2, w_q_b2.coeffs();
+ pose3_ << w_p_wb3, w_q_b3.coeffs();
+
+ /////////////////////////////////////////
+ // extrinsics ground truth and mocap data
+ // b_p_bm_ << 0.1,0.2,0.3;
+ b_p_bm_ << Vector3d::Random();
+ b_q_m_ = Quaterniond::UnitRandom();
+
+ Vector3d w_p_wb1_meas = w_p_wb1 + w_q_b1 * b_p_bm_;
+ Vector3d w_p_wb2_meas = w_p_wb2 + w_q_b2 * b_p_bm_;
+ Vector3d w_p_wb3_meas = w_p_wb3 + w_q_b3 * b_p_bm_;
+ Quaterniond w_q_b1_meas = w_q_b1 * b_q_m_;
+ Quaterniond w_q_b2_meas = w_q_b2 * b_q_m_;
+ Quaterniond w_q_b3_meas = w_q_b3 * b_q_m_;
+ pose1_meas_ << w_p_wb1_meas, w_q_b1_meas.coeffs();
+ pose2_meas_ << w_p_wb2_meas, w_q_b2_meas.coeffs();
+ pose3_meas_ << w_p_wb3_meas, w_q_b3_meas.coeffs();
+
+ /////////////////////
+ // relative odom data
+ Vector3d b1_p_b1b2 = w_q_b1.conjugate()*(w_p_wb2 - w_p_wb1);
+ Quaterniond b1_q_b2 = w_q_b1.conjugate()*w_q_b2;
+ pose_12_ << b1_p_b1b2, b1_q_b2.coeffs();
+
+ Vector3d b2_p_b2b3 = w_q_b2.conjugate()*(w_p_wb3 - w_p_wb2);
+ Quaterniond b2_q_b3 = w_q_b2.conjugate()*w_q_b3;
+ pose_23_ << b2_p_b2b3, b2_q_b3.coeffs();
+
+ // Problem and solver_
+ problem_ = Problem::create("PO", 3);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+
+ // pose sensor and proc (to get extrinsics in the prb)
+ auto intr_sp = std::make_shared<ParamsSensorPose>();
+ intr_sp->std_p = 1;
+ intr_sp->std_o = 1;
+ Vector7d extr; extr << b_p_bm_, b_q_m_.coeffs();
+ sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
+ auto params_proc = std::make_shared<ParamsProcessorPose>();
+ params_proc->time_tolerance = 0.5;
+ auto proc_pose = problem_->installProcessor("ProcessorPose", "pose", sensor_pose_, params_proc);
+ // somehow by default the sensor extrinsics is fixed
+ sensor_pose_->unfixExtrinsics();
+ }
+
+ void TearDown() override{};
+};
+
+class FactorPose3dWithExtrinsicsMANUAL_Test : public FactorPose3dWithExtrinsicsBase_Test
+{
+ public:
+
+ void SetUp() override
+ {
+ FactorPose3dWithExtrinsicsBase_Test::SetUp();
+ // Two frames
+ KF1_ = problem_->emplaceFrame(1, pose1_);
+ KF2_ = problem_->emplaceFrame(2, pose2_);
+ KF3_ = problem_->emplaceFrame(3, pose3_);
+
+ ///////////////////
+ // Create factor graph
+ Matrix6d cov6d = sensor_pose_->getNoiseCov();
+ // Odometry capture, feature and factor
+ auto cap_odom1 = CaptureBase::emplace<CapturePose>(KF2_, 2, nullptr, pose_12_, cov6d);
+ auto fea_odom1 = FeatureBase::emplace<FeatureBase>(cap_odom1, "odom", pose_12_, cov6d);
+ FactorRelativePose3dPtr fac_odomA = FactorBase::emplace<FactorRelativePose3d>(fea_odom1, fea_odom1, KF1_, nullptr, false, TOP_MOTION);
+
+ auto cap_odom2 = CaptureBase::emplace<CapturePose>(KF3_, 3, nullptr, pose_23_, cov6d);
+ auto fea_odom2 = FeatureBase::emplace<FeatureBase>(cap_odom2, "odom", pose_23_, cov6d);
+ FactorRelativePose3dPtr fac_odom2 = FactorBase::emplace<FactorRelativePose3d>(fea_odom2, fea_odom2, KF2_, nullptr, false, TOP_MOTION);
+
+ // Captures mocap
+ auto cap_mocap1 = CaptureBase::emplace<CapturePose>(KF1_, 1, sensor_pose_, pose1_meas_, cov6d);
+ auto fea_mocap1 = FeatureBase::emplace<FeatureBase>(cap_mocap1, "pose", pose1_meas_, cov6d);
+ FactorPose3dWithExtrinsicsPtr fac_mocap1 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap1, fea_mocap1, nullptr, false, TOP_MOTION);
+
+ auto cap_mocap2 = CaptureBase::emplace<CapturePose>(KF2_, 2, sensor_pose_, pose2_meas_, cov6d);
+ auto fea_mocap2 = FeatureBase::emplace<FeatureBase>(cap_mocap2, "pose", pose2_meas_, cov6d);
+ FactorPose3dWithExtrinsicsPtr fac_mocap2 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap2, fea_mocap2, nullptr, false, TOP_MOTION);
+
+ auto cap_mocap3 = CaptureBase::emplace<CapturePose>(KF3_, 3, sensor_pose_, pose3_meas_, cov6d);
+ auto fea_mocap3 = FeatureBase::emplace<FeatureBase>(cap_mocap3, "pose", pose3_meas_, cov6d);
+ FactorPose3dWithExtrinsicsPtr fac_mocap3 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap3, fea_mocap3, nullptr, false, TOP_MOTION);
+
+
+ }
+
+ void TearDown() override{};
+};
+
+
+class FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test : public FactorPose3dWithExtrinsicsBase_Test
+{
+ public:
+
+ void SetUp() override
+ {
+ FactorPose3dWithExtrinsicsBase_Test::SetUp();
+ // Two frames
+ KF1_ = problem_->emplaceFrame(1, pose1_);
+ KF2_ = problem_->emplaceFrame(2, pose2_);
+ KF3_ = problem_->emplaceFrame(3, pose3_);
+ problem_->keyFrameCallback(KF1_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF2_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF3_, nullptr, 0.5);
+
+ ///////////////////
+ // Create factor graph
+ Matrix6d cov6d = sensor_pose_->getNoiseCov();
+ // Odometry capture, feature and factor
+ auto cap_odom1 = CaptureBase::emplace<CapturePose>(KF2_, 2, nullptr, pose_12_, cov6d);
+ auto fea_odom1 = FeatureBase::emplace<FeatureBase>(cap_odom1, "odom", pose_12_, cov6d);
+ FactorRelativePose3dPtr fac_odomA = FactorBase::emplace<FactorRelativePose3d>(fea_odom1, fea_odom1, KF1_, nullptr, false, TOP_MOTION);
+
+ auto cap_odom2 = CaptureBase::emplace<CapturePose>(KF3_, 3, nullptr, pose_23_, cov6d);
+ auto fea_odom2 = FeatureBase::emplace<FeatureBase>(cap_odom2, "odom", pose_23_, cov6d);
+ FactorRelativePose3dPtr fac_odom2 = FactorBase::emplace<FactorRelativePose3d>(fea_odom2, fea_odom2, KF2_, nullptr, false, TOP_MOTION);
+
+ // process mocap data
+ auto cap_mocap1 = std::make_shared<CapturePose>(1, sensor_pose_, pose1_meas_, cov6d);
+ auto cap_mocap2 = std::make_shared<CapturePose>(2, sensor_pose_, pose2_meas_, cov6d);
+ auto cap_mocap3 = std::make_shared<CapturePose>(3, sensor_pose_, pose3_meas_, cov6d);
+ cap_mocap1->process();
+ cap_mocap2->process();
+ cap_mocap3->process();
+
+ }
+
+ void TearDown() override{};
+};
+
+
+class FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test : public FactorPose3dWithExtrinsicsBase_Test
+{
+ public:
+
+ void SetUp() override
+ {
+ FactorPose3dWithExtrinsicsBase_Test::SetUp();
+ // Two frames
+ KF1_ = problem_->emplaceFrame(1, pose1_);
+ KF2_ = problem_->emplaceFrame(2, pose2_);
+ KF3_ = problem_->emplaceFrame(3, pose3_);
+
+ Matrix6d cov6d = sensor_pose_->getNoiseCov();
+ ///////////////////
+ // Create factor graph
+ // Odometry capture, feature and factor
+ auto cap_odom1 = CaptureBase::emplace<CapturePose>(KF2_, 2, nullptr, pose_12_, cov6d);
+ auto fea_odom1 = FeatureBase::emplace<FeatureBase>(cap_odom1, "odom", pose_12_, cov6d);
+ FactorRelativePose3dPtr fac_odomA = FactorBase::emplace<FactorRelativePose3d>(fea_odom1, fea_odom1, KF1_, nullptr, false, TOP_MOTION);
+
+ auto cap_odom2 = CaptureBase::emplace<CapturePose>(KF3_, 3, nullptr, pose_23_, cov6d);
+ auto fea_odom2 = FeatureBase::emplace<FeatureBase>(cap_odom2, "odom", pose_23_, cov6d);
+ FactorRelativePose3dPtr fac_odom2 = FactorBase::emplace<FactorRelativePose3d>(fea_odom2, fea_odom2, KF2_, nullptr, false, TOP_MOTION);
+
+ // process mocap data
+ auto cap_mocap1 = std::make_shared<CapturePose>(1, sensor_pose_, pose1_meas_, cov6d);
+ auto cap_mocap2 = std::make_shared<CapturePose>(2, sensor_pose_, pose2_meas_, cov6d);
+ auto cap_mocap3 = std::make_shared<CapturePose>(3, sensor_pose_, pose3_meas_, cov6d);
+ cap_mocap1->process();
+ cap_mocap2->process();
+ cap_mocap3->process();
+
+ // keyframe callback called after all mocap captures have been processed
+ problem_->keyFrameCallback(KF1_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF2_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF3_, nullptr, 0.5);
+ }
+
+ void TearDown() override{};
+};
+
+
+
+
+TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, check_tree)
+{
+ ASSERT_TRUE(problem_->check(0));
+}
+
+TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, solve)
+{
+ problem_->perturb();
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+
+ SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
+ ASSERT_MATRIX_APPROX(KF1_->getStateVector(), pose1_, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+
+}
+
+
+TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, check_tree)
+{
+ ASSERT_TRUE(problem_->check(0));
+}
+
+TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, solve)
+{
+ problem_->perturb();
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+
+ SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
+ ASSERT_MATRIX_APPROX(KF1_->getStateVector(), pose1_, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+
+}
+
+
+TEST_F(FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test, check_tree)
+{
+ ASSERT_TRUE(problem_->check(0));
+}
+
+TEST_F(FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test, solve)
+{
+ problem_->perturb();
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+
+ SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
+ ASSERT_MATRIX_APPROX(KF1_->getStateVector(), pose1_, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+}
+
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..64d6b73a1a77ddf4a1ee7a85f72164d4174c8ff6
--- /dev/null
+++ b/test/gtest_sensor_pose.cpp
@@ -0,0 +1,69 @@
+/**
+ * \file gtest_sensor_pose.cpp
+ *
+ * Created on: Feb 18, 2020
+ * \author: mfourmy
+ */
+
+#include "core/sensor/sensor_pose.h"
+#include "core/utils/utils_gtest.h"
+
+#include <cstdio>
+
+using namespace wolf;
+using namespace Eigen;
+
+TEST(Pose, constructor)
+{
+
+ auto intr = std::make_shared<ParamsSensorPose>();
+ Vector7d extr; extr << 1,2,3,4,5,6,7;
+
+ auto sen = std::make_shared<SensorPose>(extr, intr);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), extr.head<3>(), 1e-12);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), extr.tail<4>(), 1e-12);
+}
+
+TEST(Pose, getParams)
+{
+ auto intr = std::make_shared<ParamsSensorPose>();
+ intr->std_p = 2;
+ intr->std_o = 3;
+
+ Vector7d extr; extr << 1,2,3,4,5,6,7;
+
+ auto sen = std::make_shared<SensorPose>(extr, intr);
+
+ ASSERT_EQ(sen->getStdP(), intr->std_p);
+ ASSERT_EQ(sen->getStdO(), intr->std_o);
+ // ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished());
+ // ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished());
+}
+
+TEST(Pose, create)
+{
+ auto intr = std::make_shared<ParamsSensorPose>();
+ intr->std_p = 2;
+ intr->std_o = 3;
+
+ Vector7d extr; extr << 1,2,3,4,5,6,7;
+
+ auto sen_base = SensorPose::create("name", extr, intr);
+
+ auto sen = std::static_pointer_cast<SensorPose>(sen_base);
+
+ ASSERT_EQ(sen->getStdP(), intr->std_p);
+ ASSERT_EQ(sen->getStdO(), intr->std_o);
+ // ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished());
+ // ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished());
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+