diff --git a/CMakeLists.txt b/CMakeLists.txt index 42622856d59417ed225d4950f9bb0a87f62e5ae7..9ee01c7dce630f0afd6f5522c89ed640e951f830 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -250,6 +250,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 +262,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 +346,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 +357,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 +371,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..cecb1c32f5db2dff405b874e4a7306beece32b1a --- /dev/null +++ b/include/core/factor/factor_pose_3d_with_extrinsics.h @@ -0,0 +1,94 @@ +#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>()); + + std::cout << "\n\n\n\nFAC" << std::endl; + std::cout << "w_p_wm: " << w_p_wm.transpose() << std::endl; + std::cout << "(w_p_wb + w_q_b*b_p_bm): " << (w_p_wb + w_q_b*b_p_bm).transpose() << std::endl; + std::cout << "b_p_bm: " << b_p_bm.transpose() << std::endl; + std::cout << "err: " << err.transpose() << std::endl; + + // 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_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..b3cc090dfe29d9d875af68ed51810a56093b9efb --- /dev/null +++ b/src/processor/processor_pose.cpp @@ -0,0 +1,108 @@ +/** + * \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; + auto cap_it = buffer_capture_.selectIterator(t, kf_it->second->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..e62e51d0acafc4efed5b0d4eff606ec508531779 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -209,6 +209,10 @@ target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAM wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp) target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME}) +# FactorPose3dWithExtrinsics class test +wolf_add_gtest(gtest_factor_pose_3d_with_extrinsics gtest_factor_pose_3d_with_extrinsics.cpp) +target_link_libraries(gtest_factor_pose_3d_with_extrinsics ${PLUGIN_NAME}) + # MakePosDef function test wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp) target_link_libraries(gtest_make_posdef ${PLUGIN_NAME}) @@ -253,6 +257,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_factor_pose_3d_with_extrinsics.cpp b/test/gtest_factor_pose_3d_with_extrinsics.cpp new file mode 100644 index 0000000000000000000000000000000000000000..aeeba81efd6feb7674f6efe47289b831ccfcd297 --- /dev/null +++ b/test/gtest_factor_pose_3d_with_extrinsics.cpp @@ -0,0 +1,136 @@ +/** + * \file gtest_factor_pose_with_extrinsics.cpp + * + * Created on: Feb a9, 2020 + * \author: mfourmy + */ + +#include "core/utils/utils_gtest.h" +#include "core/ceres_wrapper/solver_ceres.h" +#include "core/factor/factor_relative_pose_3d.h" +#include "core/sensor/sensor_pose.h" +#include "core/capture/capture_odom_3d.h" +#include "core/processor/processor_pose.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(); + + +// 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_wb1 = Vector3d::Zero(); +// Quaterniond w_q_b1 = Quaterniond::Identity(); +// Vector3d w_p_wb2 = Vector3d::Ones(); +// Quaterniond w_q_b2 = Quaterniond::Identity(); +// Quaterniond w_q_b2(0,0,0,1); +// Quaterniond w_q_b2 = Quaterniond::UnitRandom(); + +// pose vectors +Vector7d pose1((Vector7d() << w_p_wb1, w_q_b1.coeffs()).finished()); +Vector7d pose2((Vector7d() << w_p_wb2, w_q_b2.coeffs()).finished()); +// unitary covariance +Matrix6d cov_odom = pow(0.01,2)*Matrix6d::Identity(); +Matrix6d cov_mocap = pow(0.0001,2)*Matrix6d::Identity(); + +///////////////////////////////////////// +// extrinsics ground truth and mocap data +// Vector3d b_p_bm = Vector3d::Zero(); +// Vector3d b_p_bm = 0.1*Vector3d::Ones(); +Vector3d b_p_bm((Vector3d() << 0.1,0,0).finished()); +// Vector3d b_p_bm = Vector3d::Random(); +// Quaterniond b_q_m = Quaterniond::Identity(); +Quaterniond b_q_m(0,0,0,1); +// Quaterniond 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; +Quaterniond w_q_b1_meas = w_q_b1 * b_q_m; +Quaterniond w_q_b2_meas = w_q_b2 * b_q_m; +Vector7d pose1_meas((Vector7d() << w_p_wb1_meas, w_q_b1_meas.coeffs()).finished()); +Vector7d pose2_meas((Vector7d() << w_p_wb2_meas, w_q_b2_meas.coeffs()).finished()); + +///////////////////// +// 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; +Vector7d pose_12((Vector7d() << b1_p_b1b2, b1_q_b2.coeffs()).finished()); + +// Problem and solver +ProblemPtr problem = Problem::create("PO", 3); +SolverCeres solver(problem); + +// pose sensor and proc (to get extrinsics in the prb) +auto intr_sp = std::make_shared<ParamsSensorPose>(); +Vector7d extr = (Vector7d() << b_p_bm, b_q_m.coeffs()).finished(); +auto sensor_pose = problem->installSensor("SensorPose", "pose", extr, intr_sp); +auto params_proc = std::make_shared<ParamsProcessorPose>(); +auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc); + +// Two frames +FrameBasePtr KF1 = problem->emplaceFrame(1, pose1); +FrameBasePtr KF2 = problem->emplaceFrame(2, pose2); + + +/////////////////// +// Create factor graph +// Odometry capture, feature and factor +auto cap_odom = CaptureBase::emplace<CapturePose>(KF2, 2, nullptr, pose_12, cov_odom); +auto fea_odom = FeatureBase::emplace<FeatureBase>(cap_odom, "odom", pose_12, cov_odom); +FactorRelativePose3dPtr fac_odom = FactorBase::emplace<FactorRelativePose3d>(fea_odom, fea_odom, KF1, nullptr, false, TOP_MOTION); + +// Captures mocap +auto cap_mocap1 = CaptureBase::emplace<CapturePose>(KF1, 1, sensor_pose, pose1_meas, cov_mocap); +auto fea_mocap1 = FeatureBase::emplace<FeatureBase>(cap_mocap1, "pose", pose1_meas, cov_mocap); +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, cov_mocap); +auto fea_mocap2 = FeatureBase::emplace<FeatureBase>(cap_mocap2, "pose", pose2_meas, cov_mocap); +FactorPose3dWithExtrinsicsPtr fac_mocap2 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap2, fea_mocap2, nullptr, false, TOP_MOTION); + + + +TEST(FactorPose3dWithExtrinsics, check_tree) +{ + ASSERT_TRUE(problem->check(0)); +} + +TEST(FactorPose3dWithExtrinsics, solve) +{ + + // Fix frame 0, perturb frm1 + sensor_pose->unfixExtrinsics(); + // sensor_pose->fixExtrinsics(); + // frm0->setState(x0); + + + problem->print(4, true, true, true); + problem->perturb(); + + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + + std::cout << report << std::endl; + + problem->print(4, true, true, true); + + // ASSERT_MATRIX_APPROX(KF1->getStateVector(), pose1, 1e-6); + ASSERT_MATRIX_APPROX(KF2->getStateVector(), pose2, 1e-6); + ASSERT_MATRIX_APPROX(sensor_pose->getP()->getState(), b_p_bm, 1e-6); + ASSERT_MATRIX_APPROX(sensor_pose->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..4884ae79a4db2e4d32bf4c34ba51dd3a47500b1f --- /dev/null +++ b/test/gtest_sensor_pose.cpp @@ -0,0 +1,65 @@ +/** + * \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); +} + +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); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +