diff --git a/include/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h
index 86c4b3ee167096edfce0cfffd6a0a07c71bed840..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,
- CaptureBasePtr _origin_capture,
+ 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 3af1069eed05a7cd1f35123d5b5d915118d97112..e44f6b6e98fa82390c5e4a344a374f898653edbf 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,
- CaptureBasePtr _origin_capture);
+ 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,
- CaptureBasePtr _origin_capture,
+ CaptureBasePtr _capture_origin_ptr,
StateBlockPtr _p_ptr = nullptr,
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _intr_ptr = nullptr);
@@ -98,7 +98,7 @@ class CaptureMotion : public CaptureBase
// Origin frame and capture
CaptureBasePtr getOriginCapture();
- void setOriginCapture(CaptureBasePtr _origin_capture);
+ 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.
- CaptureBaseWPtr origin_capture_ptr_; ///< Pointer to the origin capture of the motion
+ CaptureBaseWPtr capture_origin_ptr_; ///< Pointer to the origin capture of the motion
};
inline const Eigen::VectorXs& CaptureMotion::getData() const
@@ -160,12 +160,12 @@ inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const
inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture()
{
- return origin_capture_ptr_.lock();
+ return capture_origin_ptr_.lock();
}
-inline void CaptureMotion::setOriginCapture(CaptureBasePtr _origin_capture)
+inline void CaptureMotion::setOriginCapture(CaptureBasePtr _capture_origin_ptr)
{
- origin_capture_ptr_ = _origin_capture;
+ 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 0c9997ff63084855488ee5980eb9daa34c1a7c4a..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,
- CaptureBasePtr _origin_capture = nullptr);
+ CaptureBasePtr _capture_origin_ptr = nullptr);
CaptureOdom2D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXs& _data,
const Eigen::MatrixXs& _data_cov,
- CaptureBasePtr _origin_capture = 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 a9d4e69a9252c2a7a42e34e903f34ec06c4fd907..197dd0c5eaf44fdefb7101767a03007546a90932 100644
--- a/include/core/capture/capture_odom_3D.h
+++ b/include/core/capture/capture_odom_3D.h
@@ -23,13 +23,13 @@ class CaptureOdom3D : public CaptureMotion
CaptureOdom3D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::Vector6s& _data,
- CaptureBasePtr _origin_capture = nullptr);
+ CaptureBasePtr _capture_origin_ptr = nullptr);
CaptureOdom3D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::Vector6s& _data,
const Eigen::MatrixXs& _data_cov,
- CaptureBasePtr _origin_capture = 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 92fa221c63c82e5b7571830817b9aab6e63e27e1..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,
- CaptureBasePtr _origin_capture);
+ 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,
- CaptureBasePtr _origin_capture,
+ CaptureBasePtr _capture_origin_ptr,
StateBlockPtr _p_ptr = nullptr,
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _intr_ptr = nullptr);
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 21c9343463ee03fb631865e6b90492c14eee7543..f3dfae1a7c69fc2c12e295cd35a22afd6bdbf39e 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 CaptureBasePtr& _origin_capture) = 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 ca7a3318fa7669a42023c8da33b69dbb16bfbd2a..de27f227ba3f89b7c8cf4472a88050aa183c6540 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 CaptureBasePtr& _origin_capture) 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/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp
index 0e9d0d22a0c3518592baa0a46f7b8bda0405af1e..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,
- CaptureBasePtr _origin_capture,
+ 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_capture,
+ _capture_origin_ptr,
_p_ptr,
_o_ptr,
_intr_ptr)
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index 551175b953642b60808ac19eb2366891fff846c4..cbc19727e70d352bc996c17a210f34cd280f423e 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,
- CaptureBasePtr _origin_capture) :
+ 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_capture_ptr_(_origin_capture)
+ 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,
- CaptureBasePtr _origin_capture,
+ 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_capture_ptr_(_origin_capture)
+ capture_origin_ptr_(_capture_origin_ptr)
{
//
}
diff --git a/src/capture/capture_odom_2D.cpp b/src/capture/capture_odom_2D.cpp
index 6070767d33c82cf2ac079c8837bd2d7fe22f2f29..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,
- CaptureBasePtr _origin_capture):
- CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, 3, 3, _origin_capture)
+ 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,
- CaptureBasePtr _origin_capture):
- CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _origin_capture)
+ 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 4024119ad9c860459ebcb3484ff54118e6ef643a..7ad7a5e60f9c7d56eddd75c14338cb52801632de 100644
--- a/src/capture/capture_odom_3D.cpp
+++ b/src/capture/capture_odom_3D.cpp
@@ -13,8 +13,8 @@ namespace wolf
CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::Vector6s& _data,
- CaptureBasePtr _origin_capture):
- CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, 7, 6, _origin_capture)
+ CaptureBasePtr _capture_origin_ptr):
+ CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, 7, 6, _capture_origin_ptr)
{
//
}
@@ -23,8 +23,8 @@ CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::Vector6s& _data,
const Eigen::MatrixXs& _data_cov,
- CaptureBasePtr _origin_capture):
- CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _origin_capture)
+ 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 33c43824960440fa6fd31ad522f08b03e926e543..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,
- CaptureBasePtr _origin_capture) :
+ CaptureBasePtr _capture_origin_ptr) :
CaptureMotion("VELOCITY", _ts, _sensor_ptr, _velocity,
- _delta_size, _delta_cov_size, _origin_capture)
+ _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,
- CaptureBasePtr _origin_capture,
+ 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_capture,
+ _delta_size, _delta_cov_size, _capture_origin_ptr,
_p_ptr, _o_ptr, _intr_ptr)
{
//
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 4e381ca5db3127a47256f20f5ce80f40631fdd2c..2c1bc8053bb46ef1fcaeadb026ae3b7037a2a5be 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -123,10 +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 capture acting as the buffer's origin
- auto capture_origin = existing_capture->getOriginCapture();
+ 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();
@@ -144,27 +144,27 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// 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, capture_origin );
+ 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;
@@ -208,7 +208,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// extract pack elements
FrameBasePtr keyframe_from_callback = pack->key_frame;
- auto & existing_capture = last_ptr_;
+ auto & capture_existing = last_ptr_;
// Find the capture acting as the buffer's origin
auto & capture_origin = origin_ptr_;
@@ -229,7 +229,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// 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);
@@ -303,11 +303,11 @@ 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_),
@@ -316,14 +316,14 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_->getCalibration(),
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();
// 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);
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index b7cee661cf008809187c99930f8c68ceb9eeccec..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 CaptureBasePtr& _origin_capture)
+ const CaptureBasePtr& _capture_origin_ptr)
{
- auto cap_motion = CaptureBase::emplace<CaptureOdom2D>(_frame_own, _ts, _sensor, _data, _data_cov, _origin_capture);
+ 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);