Skip to content
Snippets Groups Projects
Commit 093d7435 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Rename a few variables for consistency

parent 076a41a3
No related branches found
No related tags found
1 merge request!318CaptureMotion: replace origin_frame_ptr_ by capture_origin_ptr_.
Pipeline #4265 passed
Showing with 53 additions and 53 deletions
......@@ -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);
......
......@@ -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
......
......@@ -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();
......
......@@ -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();
......
......@@ -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);
......
......@@ -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
......
......@@ -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;
......
......@@ -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)
......
......@@ -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)
{
//
}
......
......@@ -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)
{
//
}
......
......@@ -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)
{
//
}
......
......@@ -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)
{
//
......
......@@ -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);
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment