diff --git a/include/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h index d50a728160c9d1d393fd552501a1d3f58a5dbe8f..0f785cdd3d8178a5fb5b194841e76d46547f58cf 100644 --- a/include/core/capture/capture_diff_drive.h +++ b/include/core/capture/capture_diff_drive.h @@ -32,7 +32,7 @@ public: const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov, - FrameBasePtr _origin_frame_ptr, + CaptureBasePtr _capture_origin_ptr, StateBlockPtr _p_ptr = nullptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _intr_ptr = nullptr); diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h index 4ea8cb172cd8503788125d441c79569a9cf15f75..7fa7e38eb1dbad2579222c2dff247b9614322594 100644 --- a/include/core/capture/capture_motion.h +++ b/include/core/capture/capture_motion.h @@ -52,7 +52,7 @@ class CaptureMotion : public CaptureBase const Eigen::VectorXs& _data, SizeEigen _delta_size, SizeEigen _delta_cov_size, - FrameBasePtr _origin_frame_ptr); + CaptureBasePtr _capture_origin_ptr); CaptureMotion(const std::string & _type, const TimeStamp& _ts, @@ -61,7 +61,7 @@ class CaptureMotion : public CaptureBase const Eigen::MatrixXs& _data_cov, SizeEigen _delta_size, SizeEigen _delta_cov_size, - FrameBasePtr _origin_frame_ptr, + CaptureBasePtr _capture_origin_ptr, StateBlockPtr _p_ptr = nullptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _intr_ptr = nullptr); @@ -96,9 +96,9 @@ class CaptureMotion : public CaptureBase MatrixXs getDeltaPreintCov(const TimeStamp& _ts) const; virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const; - // Origin frame - FrameBasePtr getOriginFrame(); - void setOriginFrame(FrameBasePtr _frame_ptr); + // Origin frame and capture + CaptureBasePtr getOriginCapture(); + void setOriginCapture(CaptureBasePtr _capture_origin_ptr); // member data: private: @@ -106,7 +106,7 @@ class CaptureMotion : public CaptureBase Eigen::MatrixXs data_cov_; ///< Motion data covariance Eigen::VectorXs calib_preint_; ///< Calibration parameters used during pre-integration MotionBuffer buffer_; ///< Buffer of motions between this Capture and the next one. - FrameBaseWPtr origin_frame_ptr_; ///< Pointer to the origin frame of the motion + CaptureBaseWPtr capture_origin_ptr_; ///< Pointer to the origin capture of the motion }; inline const Eigen::VectorXs& CaptureMotion::getData() const @@ -158,14 +158,14 @@ inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const return _delta + _delta_error; } -inline FrameBasePtr CaptureMotion::getOriginFrame() +inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture() { - return origin_frame_ptr_.lock(); + return capture_origin_ptr_.lock(); } -inline void CaptureMotion::setOriginFrame(FrameBasePtr _frame_ptr) +inline void CaptureMotion::setOriginCapture(CaptureBasePtr _capture_origin_ptr) { - origin_frame_ptr_ = _frame_ptr; + capture_origin_ptr_ = _capture_origin_ptr; } inline VectorXs CaptureMotion::getCalibrationPreint() const diff --git a/include/core/capture/capture_odom_2D.h b/include/core/capture/capture_odom_2D.h index 6b6258cea271baf716324a3888a2725557fd4027..06a507aa8368474460e93b7b6d64c059180d41d4 100644 --- a/include/core/capture/capture_odom_2D.h +++ b/include/core/capture/capture_odom_2D.h @@ -23,13 +23,13 @@ class CaptureOdom2D : public CaptureMotion CaptureOdom2D(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, - FrameBasePtr _origin_frame_ptr = nullptr); + CaptureBasePtr _capture_origin_ptr = nullptr); CaptureOdom2D(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov, - FrameBasePtr _origin_frame_ptr = nullptr); + CaptureBasePtr _capture_origin_ptr = nullptr); virtual ~CaptureOdom2D(); diff --git a/include/core/capture/capture_odom_3D.h b/include/core/capture/capture_odom_3D.h index c7e0eecb59e5fc500c5a67c07e1557d65681f5cb..cc810448af0a68d7287483b0afe538bda0e0d2b2 100644 --- a/include/core/capture/capture_odom_3D.h +++ b/include/core/capture/capture_odom_3D.h @@ -22,14 +22,14 @@ class CaptureOdom3D : public CaptureMotion public: CaptureOdom3D(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, - const Eigen::Vector6s& _data, - FrameBasePtr _origin_frame_ptr = nullptr); + const Eigen::VectorXs& _data, + CaptureBasePtr _capture_origin_ptr = nullptr); CaptureOdom3D(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, - const Eigen::Vector6s& _data, + const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov, - FrameBasePtr _origin_frame_ptr = nullptr); + CaptureBasePtr _capture_origin_ptr = nullptr); virtual ~CaptureOdom3D(); diff --git a/include/core/capture/capture_velocity.h b/include/core/capture/capture_velocity.h index 98bc3b3e395af54388f03777dfe876bf6a8a8429..f665c181311494bf0bbc154b3f1e030c7f1ecad2 100644 --- a/include/core/capture/capture_velocity.h +++ b/include/core/capture/capture_velocity.h @@ -35,14 +35,14 @@ public: const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _velocity, SizeEigen _delta_size, SizeEigen _delta_cov_size, - FrameBasePtr _origin_frame_ptr); + CaptureBasePtr _capture_origin_ptr); CaptureVelocity(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _velocity, const Eigen::MatrixXs& _velocity_cov, SizeEigen _delta_size, SizeEigen _delta_cov_size, - FrameBasePtr _origin_frame_ptr, + CaptureBasePtr _capture_origin_ptr, StateBlockPtr _p_ptr = nullptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _intr_ptr = nullptr); diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h index 1192230afa33cbb5814f6fc599e1df17c8290b9b..6c8bf6a34605dd5da4c9d4766f619b3c7cbc6941 100644 --- a/include/core/processor/processor_diff_drive.h +++ b/include/core/processor/processor_diff_drive.h @@ -56,7 +56,7 @@ class ProcessorDiffDrive : public ProcessorOdom2D const MatrixXs& _data_cov, const VectorXs& _calib, const VectorXs& _calib_preint, - const FrameBasePtr& _frame_origin) override; + const CaptureBasePtr& _capture_origin) override; virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override; virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override; diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 0de934bca5dd2ef91eb77522c9db20a5eadfbc19..9bc5027fd8e8cbcc0729aed9b897765b21862ec7 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -418,7 +418,7 @@ class ProcessorMotion : public ProcessorBase const MatrixXs& _data_cov, const VectorXs& _calib, const VectorXs& _calib_preint, - const FrameBasePtr& _frame_origin) = 0; + const CaptureBasePtr& _capture_origin_ptr) = 0; /** \brief emplace a feature corresponding to given capture and add the feature to this capture * \param _capture_motion: the parent capture diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h index 2957ec7de8cd0647a45fc1f72c15629b9f42a089..a4f6e3358fc0f71745a3df1ce15cf48cc880a7f6 100644 --- a/include/core/processor/processor_odom_2D.h +++ b/include/core/processor/processor_odom_2D.h @@ -80,7 +80,7 @@ class ProcessorOdom2D : public ProcessorMotion const MatrixXs& _data_cov, const VectorXs& _calib, const VectorXs& _calib_preint, - const FrameBasePtr& _frame_origin) override; + const CaptureBasePtr& _capture_origin_ptr) override; virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override; virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin) override; diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h index 1512b996e89a5e5d5c7c6bc294d1dfa495219d41..22f4903c209d11bf31f0e08f302c0ce7a6d8d77f 100644 --- a/include/core/processor/processor_odom_3D.h +++ b/include/core/processor/processor_odom_3D.h @@ -99,7 +99,7 @@ class ProcessorOdom3D : public ProcessorMotion const MatrixXs& _data_cov, const VectorXs& _calib, const VectorXs& _calib_preint, - const FrameBasePtr& _frame_origin) override; + const CaptureBasePtr& _capture_origin) override; virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override; diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h index 9367347e07da6a0f9aaa0708c5db3b939a0deba3..47b0b7886ee9a123e39a5b8cb90576d8a2562ea6 100644 --- a/include/core/sensor/sensor_diff_drive.h +++ b/include/core/sensor/sensor_diff_drive.h @@ -49,10 +49,10 @@ class SensorDiffDrive : public SensorBase { public: SensorDiffDrive(const Eigen::VectorXs& _extrinsics, - const IntrinsicsDiffDrivePtr& _intrinsics); + const IntrinsicsDiffDrivePtr& _intrinsics); WOLF_SENSOR_CREATE(SensorDiffDrive, IntrinsicsDiffDrive, 3); virtual ~SensorDiffDrive(); - IntrinsicsDiffDriveConstPtr getParams() const {return params_diff_drive_;} + IntrinsicsDiffDriveConstPtr getParams() const; Scalar getRadiansPerTick() const { @@ -65,6 +65,11 @@ class SensorDiffDrive : public SensorBase }; +inline wolf::IntrinsicsDiffDriveConstPtr SensorDiffDrive::getParams() const +{ + return params_diff_drive_; +} + } /* namespace wolf */ #endif /* SENSOR_SENSOR_DIFF_DRIVE_H_ */ diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp index 02d9eab183156528ffe991b5f01a7867fd5d5568..cc81850c8447040d21f7e6cffa4f079c5884ca47 100644 --- a/src/capture/capture_diff_drive.cpp +++ b/src/capture/capture_diff_drive.cpp @@ -9,7 +9,7 @@ CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov, - FrameBasePtr _origin_frame_ptr, + CaptureBasePtr _capture_origin_ptr, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr) : @@ -20,7 +20,7 @@ CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts, _data_cov, 3, 3, - _origin_frame_ptr, + _capture_origin_ptr, _p_ptr, _o_ptr, _intr_ptr) diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp index 47d26a131c25cffb0014ca5472ad744070929eaf..a2330905ad8be9ff5f871f8e5fc0c11a01b9ccde 100644 --- a/src/capture/capture_motion.cpp +++ b/src/capture/capture_motion.cpp @@ -16,12 +16,12 @@ CaptureMotion::CaptureMotion(const std::string & _type, const Eigen::VectorXs& _data, SizeEigen _delta_size, SizeEigen _delta_cov_size, - FrameBasePtr _origin_frame_ptr) : + CaptureBasePtr _capture_origin_ptr) : CaptureBase(_type, _ts, _sensor_ptr), data_(_data), data_cov_(_sensor_ptr ? _sensor_ptr->getNoiseCov() : Eigen::MatrixXs::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly buffer_(), - origin_frame_ptr_(_origin_frame_ptr) + capture_origin_ptr_(_capture_origin_ptr) { // } @@ -33,7 +33,7 @@ CaptureMotion::CaptureMotion(const std::string & _type, const Eigen::MatrixXs& _data_cov, SizeEigen _delta_size, SizeEigen _delta_cov_size, - FrameBasePtr _origin_frame_ptr, + CaptureBasePtr _capture_origin_ptr, StateBlockPtr _p_ptr , StateBlockPtr _o_ptr , StateBlockPtr _intr_ptr ) : @@ -41,7 +41,7 @@ CaptureMotion::CaptureMotion(const std::string & _type, data_(_data), data_cov_(_data_cov), buffer_(), - origin_frame_ptr_(_origin_frame_ptr) + capture_origin_ptr_(_capture_origin_ptr) { // } diff --git a/src/capture/capture_odom_2D.cpp b/src/capture/capture_odom_2D.cpp index 1e073b2810e92eaa7c19affe4dc0bb4ad9ef74fe..36826540a27804e14c61102260bbec24a36df58f 100644 --- a/src/capture/capture_odom_2D.cpp +++ b/src/capture/capture_odom_2D.cpp @@ -13,8 +13,8 @@ namespace wolf CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, - FrameBasePtr _origin_frame_ptr): - CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, 3, 3, _origin_frame_ptr) + CaptureBasePtr _capture_origin_ptr): + CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, 3, 3, _capture_origin_ptr) { // } @@ -23,8 +23,8 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov, - FrameBasePtr _origin_frame_ptr): - CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _origin_frame_ptr) + CaptureBasePtr _capture_origin_ptr): + CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _capture_origin_ptr) { // } diff --git a/src/capture/capture_odom_3D.cpp b/src/capture/capture_odom_3D.cpp index d3236a2ab89c312320d0f3c9a6caf830733d3ef2..84f263d2cfbd6c97e647b8504322352696d4332d 100644 --- a/src/capture/capture_odom_3D.cpp +++ b/src/capture/capture_odom_3D.cpp @@ -12,19 +12,19 @@ namespace wolf CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, - const Eigen::Vector6s& _data, - FrameBasePtr _origin_frame_ptr): - CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, 7, 6, _origin_frame_ptr) + const Eigen::VectorXs& _data, + CaptureBasePtr _capture_origin_ptr): + CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, 7, 6, _capture_origin_ptr) { // } CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, - const Eigen::Vector6s& _data, + const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov, - FrameBasePtr _origin_frame_ptr): - CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _origin_frame_ptr) + CaptureBasePtr _capture_origin_ptr): + CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _capture_origin_ptr) { // } diff --git a/src/capture/capture_velocity.cpp b/src/capture/capture_velocity.cpp index 0785e8bae4918c1f8406bc379123dd56f4120730..00a6b796bbba9135ee8fbba1150ff353c29b72ec 100644 --- a/src/capture/capture_velocity.cpp +++ b/src/capture/capture_velocity.cpp @@ -6,9 +6,9 @@ CaptureVelocity::CaptureVelocity(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _velocity, SizeEigen _delta_size, SizeEigen _delta_cov_size, - FrameBasePtr _origin_frame_ptr) : + CaptureBasePtr _capture_origin_ptr) : CaptureMotion("VELOCITY", _ts, _sensor_ptr, _velocity, - _delta_size, _delta_cov_size, _origin_frame_ptr) + _delta_size, _delta_cov_size, _capture_origin_ptr) { // } @@ -18,12 +18,12 @@ CaptureVelocity::CaptureVelocity(const TimeStamp& _ts, const Eigen::VectorXs& _velocity, const Eigen::MatrixXs& _velocity_cov, SizeEigen _delta_size, SizeEigen _delta_cov_size, - FrameBasePtr _origin_frame_ptr, + CaptureBasePtr _capture_origin_ptr, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr) : CaptureMotion("VELOCITY", _ts, _sensor_ptr, _velocity, _velocity_cov, - _delta_size, _delta_cov_size, _origin_frame_ptr, + _delta_size, _delta_cov_size, _capture_origin_ptr, _p_ptr, _o_ptr, _intr_ptr) { // diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 37a841abeeaf8caf6daf56f12d3f00689bd3aa6d..49a9fab6bf8450592dd78e8b6ac69fd4bfe7a0a3 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -939,8 +939,12 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c if (C->isMotion()) { auto CM = std::static_pointer_cast<CaptureMotion>(C); - if (CM->getOriginFrame()) - cout << " -> OF" << CM->getOriginFrame()->id(); + if (auto OC = CM->getOriginCapture()) + { + cout << " -> OC" << OC->id(); + if (auto OF = OC->getFrame()) + cout << " ; OF" << OF->id(); + } } cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : ""); diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp index d0afacb45a6fb235e6d01e7c893ffc612449e6c0..d4605c6f0ab27ef247dd64885de922eece4b0fc6 100644 --- a/src/processor/processor_diff_drive.cpp +++ b/src/processor/processor_diff_drive.cpp @@ -130,14 +130,14 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o const MatrixXs& _data_cov, const VectorXs& _calib, const VectorXs& _calib_preint, - const FrameBasePtr& _frame_origin) + const CaptureBasePtr& _capture_origin) { auto cap_motion = CaptureBase::emplace<CaptureDiffDrive>(_frame_own, _ts, _sensor, _data, _data_cov, - _frame_origin); + _capture_origin); cap_motion->setCalibration (_calib); cap_motion->setCalibrationPreint(_calib_preint); diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 2d6d5db588913577dc7e0a88c7989b6ffc69ccda..d54036d2d3a9b4a00c80daeceddb836920daed10 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -52,10 +52,10 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source, _capture_source->getBuffer().get().push_front(motionZero(t_zero)); // Update the existing capture - _capture_source->setOriginFrame(_keyframe_target); + _capture_source->setOriginCapture(_capture_target); // Get optimized calibration params from 'origin' keyframe - VectorXs calib_preint_optim = _capture_source->getOriginFrame()->getCaptureOf(getSensor())->getCalibration(); + VectorXs calib_preint_optim = _capture_source->getOriginCapture()->getCalibration(); // Write the calib params into the capture before re-integration _capture_source->setCalibrationPreint(calib_preint_optim); @@ -123,12 +123,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) FrameBasePtr keyframe_from_callback = pack->key_frame; // find the capture whose buffer is affected by the new keyframe - auto existing_capture = findCaptureContainingTimeStamp(keyframe_from_callback->getTimeStamp()); // k + auto capture_existing = findCaptureContainingTimeStamp(keyframe_from_callback->getTimeStamp()); // k - // Find the frame acting as the capture's origin - auto keyframe_origin = existing_capture->getOriginFrame(); // i - - auto capture_origin = std::static_pointer_cast<CaptureMotion>(keyframe_origin->getCaptureOf(getSensor())); + // Find the capture acting as the buffer's origin + auto capture_origin = capture_existing->getOriginCapture(); // Get calibration params for preintegration from origin, since it has chances to have optimized values VectorXs calib_origin = capture_origin->getCalibration(); @@ -142,31 +140,31 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) getSensor()->getNoiseCov(), calib_origin, calib_origin, - keyframe_origin); + capture_origin); // split the buffer // and give the part of the buffer before the new keyframe to the capture for the KF callback - splitBuffer(existing_capture, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback); + splitBuffer(capture_existing, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback); // create motion feature and add it to the capture - auto new_feature = emplaceFeature(capture_for_keyframe_callback); + auto feature_new = emplaceFeature(capture_for_keyframe_callback); // create motion factor and add it to the feature, and constrain to the other capture (origin) - emplaceFactor(new_feature, keyframe_origin->getCaptureOf(getSensor()) ); + emplaceFactor(feature_new, capture_origin ); // modify existing feature and factor (if they exist in the existing capture) - if (!existing_capture->getFeatureList().empty()) + if (!capture_existing->getFeatureList().empty()) { - auto existing_feature = existing_capture->getFeatureList().back(); // there is only one feature! + auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature! // Modify existing feature -------- - existing_feature->setMeasurement (existing_capture->getBuffer().get().back().delta_integr_); - existing_feature->setMeasurementCovariance(existing_capture->getBuffer().get().back().delta_integr_cov_); + feature_existing->setMeasurement (capture_existing->getBuffer().get().back().delta_integr_); + feature_existing->setMeasurementCovariance(capture_existing->getBuffer().get().back().delta_integr_cov_); // Modify existing factor -------- // Instead of modifying, we remove one ctr, and create a new one. - auto fac_to_remove = existing_feature->getFactorList().back(); // there is only one factor! - auto new_ctr = emplaceFactor(existing_feature, capture_for_keyframe_callback); + 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; @@ -197,7 +195,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) * k * * ========= * === * === * * x ----------o ----n ----l - * \____f____/ + * \____f____/ \_f_/ * * Trajectory after the KF, and after reset * @@ -210,11 +208,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) // extract pack elements FrameBasePtr keyframe_from_callback = pack->key_frame; - auto & existing_capture = last_ptr_; - - // Find the frame acting as the capture's origin - auto keyframe_origin = existing_capture->getOriginFrame(); + auto & capture_existing = last_ptr_; + // Find the capture acting as the buffer's origin auto & capture_origin = origin_ptr_; // Get calibration params for preintegration from origin, since it has chances to have optimized values @@ -229,17 +225,17 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) getSensor()->getNoiseCov(), calib_origin, calib_origin, - keyframe_origin); + capture_origin); // split the buffer // and give the part of the buffer before the new keyframe to the capture for the KF callback - splitBuffer(existing_capture, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback); + splitBuffer(capture_existing, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback); // create motion feature and add it to the capture auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback); // create motion factor and add it to the feature, and constrain to the other capture (origin) - emplaceFactor(feature_for_keyframe_callback, keyframe_origin->getCaptureOf(getSensor()) ); + emplaceFactor(feature_for_keyframe_callback, capture_origin ); // reset processor origin origin_ptr_ = capture_for_keyframe_callback; @@ -270,10 +266,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) * + : last frame * x : any capture * o : origin capture - * l : last capture + * l : last capture -> we'll make a KF here * i : incoming capture - * e : existing capture -> new KF splits its buffer in two parts - * n : new capture -> part of the split buffer will be given to this + * n : new capture -> * --- : buffer history * * Trajectory before the KF @@ -282,6 +277,12 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) * x ----------o ----------l i * \____f____/ * + * Trajectory after creating KF at last + * + * * ========= * ========= * = + + * x ----------o ----------l --n + * \____f____/ \____f____/ + * * Trajectory after creating KF at last and reset * * * ========= * ========= * = + @@ -302,27 +303,27 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) auto factor = emplaceFactor(key_feature, origin_ptr_); // create a new frame - auto new_frame = getProblem()->emplaceFrame(NON_ESTIMATED, + auto frame_new = getProblem()->emplaceFrame(NON_ESTIMATED, getCurrentState(), getCurrentTimeStamp()); // create a new capture - auto new_capture = emplaceCapture(new_frame, + auto capture_new = emplaceCapture(frame_new, getSensor(), key_frame->getTimeStamp(), Eigen::VectorXs::Zero(data_size_), getSensor()->getNoiseCov(), last_ptr_->getCalibration(), last_ptr_->getCalibration(), - key_frame); + last_ptr_); // reset the new buffer - new_capture->getBuffer().get().push_back( motionZero(key_frame->getTimeStamp()) ) ; + capture_new->getBuffer().get().push_back( motionZero(key_frame->getTimeStamp()) ) ; // reset derived things resetDerived(); - // Update pointers + // Reset pointers origin_ptr_ = last_ptr_; - last_ptr_ = new_capture; + last_ptr_ = capture_new; // callback to other processors getProblem()->keyFrameCallback(key_frame, shared_from_this(), params_motion_->time_tolerance); @@ -347,8 +348,8 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x) if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp { // Get origin state and calibration - VectorXs state_0 = capture_motion->getOriginFrame()->getState(); - CaptureBasePtr cap_orig = capture_motion->getOriginFrame()->getCaptureOf(getSensor()); + CaptureBasePtr cap_orig = capture_motion->getOriginCapture(); + VectorXs state_0 = cap_orig->getFrame()->getState(); VectorXs calib = cap_orig->getCalibration(); // Get delta and correct it with new calibration params @@ -411,7 +412,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) getSensor()->getNoiseCov(), getSensor()->getCalibration(), getSensor()->getCalibration(), - _origin_frame); + origin_ptr_); // clear and reset buffer getBuffer().get().push_back(motionZero(_origin_frame->getTimeStamp())); diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp index dda3af4ddaa964537b95d85eac1a8cccf1fb7167..0f094674afb3c902dbb3bac9e983929a170f1839 100644 --- a/src/processor/processor_odom_2D.cpp +++ b/src/processor/processor_odom_2D.cpp @@ -127,9 +127,9 @@ CaptureMotionPtr ProcessorOdom2D::emplaceCapture(const FrameBasePtr& _frame_own, const MatrixXs& _data_cov, const VectorXs& _calib, const VectorXs& _calib_preint, - const FrameBasePtr& _frame_origin) + const CaptureBasePtr& _capture_origin_ptr) { - auto cap_motion = CaptureBase::emplace<CaptureOdom2D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin); + auto cap_motion = CaptureBase::emplace<CaptureOdom2D>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin_ptr); cap_motion->setCalibration(_calib); cap_motion->setCalibrationPreint(_calib_preint); diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp index 27659e3eeb6deca00ed09794cc1059f53b007aba..c2821646896d2573f7bcaa1ed828f63c0d7813d2 100644 --- a/src/processor/processor_odom_3D.cpp +++ b/src/processor/processor_odom_3D.cpp @@ -194,9 +194,9 @@ CaptureMotionPtr ProcessorOdom3D::emplaceCapture(const FrameBasePtr& _frame_own, const MatrixXs& _data_cov, const VectorXs& _calib, const VectorXs& _calib_preint, - const FrameBasePtr& _frame_origin) + const CaptureBasePtr& _capture_origin) { - auto cap_motion = CaptureBase::emplace<CaptureOdom3D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin); + auto cap_motion = CaptureBase::emplace<CaptureOdom3D>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin); cap_motion->setCalibration(_calib); cap_motion->setCalibrationPreint(_calib_preint); diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp index 369e32919a84d20ab3e7396b67bfcb5e68b514c5..d19ab8aa2e4d2ba5f9146bbdad2d45e5d855afc2 100644 --- a/src/sensor/sensor_diff_drive.cpp +++ b/src/sensor/sensor_diff_drive.cpp @@ -12,13 +12,13 @@ namespace wolf { SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXs& _extrinsics, - const IntrinsicsDiffDrivePtr& _intrinsics) : - SensorBase("DIFF DRIVE", - std::make_shared<StateBlock>(_extrinsics.head(2), true), - std::make_shared<StateAngle>(_extrinsics(2), true), - std::make_shared<StateBlock>(3, false), - 2), - params_diff_drive_(_intrinsics) + const IntrinsicsDiffDrivePtr& _intrinsics) : + SensorBase("DIFF DRIVE", + std::make_shared<StateBlock>(_extrinsics.head(2), true), + std::make_shared<StateAngle>(_extrinsics(2), true), + std::make_shared<StateBlock>(3, false), + 2), + params_diff_drive_(_intrinsics) { radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution; getIntrinsic()->setState(Eigen::Vector3s(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation)); diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp index f8901a4d9e75edc815913289b697d2a39dd455fe..ce15f0b0343b3a58321f0971f1b85f817628988e 100644 --- a/test/gtest_emplace.cpp +++ b/test/gtest_emplace.cpp @@ -123,7 +123,7 @@ TEST(Emplace, EmplaceDerived) // LandmarkBase::emplace<LandmarkBase>(MapBaseWPtr(P->getMap()),"Dummy", nullptr, nullptr); auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXs(3), IntrinsicsOdom2D()); auto cov = Eigen::MatrixXs::Identity(2,2); - auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXs(2), cov, frm); + auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXs(2), cov, nullptr); // auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt); auto m = Eigen::Matrix<Scalar,9,6>(); for(int i = 0; i < 9; i++) diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp index 6a9e0ddb95cc05413d8c960912256bb0ff908b04..7302ae4833a94a7aeb0ed9ed304b4ea3026b453c 100644 --- a/test/gtest_factor_diff_drive.cpp +++ b/test/gtest_factor_diff_drive.cpp @@ -83,9 +83,9 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive const MatrixXs& _data_cov, const VectorXs& _calib, const VectorXs& _calib_preint, - const FrameBasePtr& _frame_origin) + const CaptureBasePtr& _capture_origin) { - return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _frame_origin); + return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin); } virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) @@ -456,7 +456,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt) data(0) = 0.50*intr->ticks_per_wheel_revolution / N; data(1) = 0.25*intr->ticks_per_wheel_revolution / N; - auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0); + auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front()); for (int n = 0; n < N; n++) { @@ -588,7 +588,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics) data(0) = 0.50*intr->ticks_per_wheel_revolution / N; data(1) = 0.25*intr->ticks_per_wheel_revolution / N; - auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0); + auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front()); for (int n = 0; n < N; n++) { diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp index 3c15a924d782d331db4b6ef268a2ea123b27db7d..f697a948afecf8f83b8275220f30497bd6e75130 100644 --- a/test/gtest_processor_diff_drive.cpp +++ b/test/gtest_processor_diff_drive.cpp @@ -78,9 +78,9 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive const MatrixXs& _data_cov, const VectorXs& _calib, const VectorXs& _calib_preint, - const FrameBasePtr& _frame_origin) + const CaptureBasePtr& _capture_origin) { - return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _frame_origin); + return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin); } virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) @@ -318,7 +318,7 @@ TEST_F(ProcessorDiffDriveTest, process) data(0) = 0.25*intr->ticks_per_wheel_revolution / N; data(1) = 0.50*intr->ticks_per_wheel_revolution / N; - auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0); + auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front()); WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose()); for (int n = 1; n <= N; n++) @@ -347,7 +347,7 @@ TEST_F(ProcessorDiffDriveTest, linear) data(0) = 100.0 ; // one turn of the wheels data(1) = 100.0 ; - auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0); + auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front()); C->process(); WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose()); @@ -372,7 +372,7 @@ TEST_F(ProcessorDiffDriveTest, angular) data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse data(1) = 20.0 ; // one fifth of a turn of the right wheel, forward --> we'll turn left --> positive angle - auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0); + auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front()); C->process(); WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose()); diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index 93bf18911e2daadcabdd5ad7df66a9161fdb679c..0977b8f87f3ba2fab515098a2afd0942f8fb6714 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -140,9 +140,7 @@ TEST_F(ProcessorMotion_test, splitBuffer) TimeStamp t_target = 8.5; FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target); - CaptureBasePtr C_last = processor->getLast(); - CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(C_last); - FrameBasePtr F_origin = C_source->getOriginFrame(); + CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast()); CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target, "ODOM 2D", t_target, @@ -150,7 +148,7 @@ TEST_F(ProcessorMotion_test, splitBuffer) data, 3, 3, - F_origin); + nullptr); processor->splitBuffer(C_source, t_target,