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

Use 'auto' instead of pre-defined types

parent e43f41e2
No related branches found
No related tags found
No related merge requests found
...@@ -25,15 +25,15 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture) ...@@ -25,15 +25,15 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
CaptureRangeBearingPtr capture = std::static_pointer_cast<CaptureRangeBearing>(_capture); CaptureRangeBearingPtr capture = std::static_pointer_cast<CaptureRangeBearing>(_capture);
// 1. get KF -- we assume a KF is available to hold this _capture (checked in assert below) // 1. get KF -- we assume a KF is available to hold this _capture (checked in assert below)
FrameBasePtr kf = getProblem()->closestKeyFrameToTimeStamp(capture->getTimeStamp()); auto kf = getProblem()->closestKeyFrameToTimeStamp(capture->getTimeStamp());
assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < time_tolerance_) && "Could not find a KF close enough to _capture!"); assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < time_tolerance_) && "Could not find a KF close enough to _capture!");
// 2. create Capture // 2. create Capture
CaptureRangeBearingPtr cap = std::make_shared<CaptureRangeBearing>(capture->getTimeStamp(), auto cap = std::make_shared<CaptureRangeBearing>(capture->getTimeStamp(),
getSensorPtr(), getSensorPtr(),
capture->getIds(), capture->getIds(),
capture->getRanges(), capture->getRanges(),
capture->getBearings()); capture->getBearings());
kf->addCapture(cap); kf->addCapture(cap);
// 3. explore all observations in the capture // 3. explore all observations in the capture
...@@ -66,17 +66,17 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture) ...@@ -66,17 +66,17 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
// 5. create feature // 5. create feature
Vector2s rb(range,bearing); Vector2s rb(range,bearing);
FeatureRangeBearingPtr ftr = std::make_shared<FeatureRangeBearing>(rb, auto ftr = std::make_shared<FeatureRangeBearing>(rb,
getSensorPtr()->getNoiseCov()); getSensorPtr()->getNoiseCov());
cap->addFeature(ftr); cap->addFeature(ftr);
// 6. create constraint // 6. create constraint
ProcessorBasePtr prc = shared_from_this(); auto prc = shared_from_this();
ConstraintRangeBearingPtr ctr = std::make_shared<ConstraintRangeBearing>(cap, auto ctr = std::make_shared<ConstraintRangeBearing>(cap,
lmk, lmk,
prc, prc,
false, false,
CTR_ACTIVE); CTR_ACTIVE);
ftr->addConstraint(ctr); ftr->addConstraint(ctr);
lmk->addConstrainedBy(ctr); lmk->addConstrainedBy(ctr);
} }
......
...@@ -52,7 +52,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) ...@@ -52,7 +52,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
if (origin_ptr_ == nullptr) if (origin_ptr_ == nullptr)
{ {
FrameBasePtr frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t0); auto frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t0);
if (frm && fabs(frm->getTimeStamp() - t0) < time_tolerance_) if (frm && fabs(frm->getTimeStamp() - t0) < time_tolerance_)
{ {
std::cout << "PM: join KF" << std::endl; std::cout << "PM: join KF" << std::endl;
...@@ -88,30 +88,30 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) ...@@ -88,30 +88,30 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
if (voteForKeyFrame() && permittedKeyFrame()) if (voteForKeyFrame() && permittedKeyFrame())
{ {
// Set the frame of last_ptr as key // Set the frame of last_ptr as key
FrameBasePtr key_frame_ptr = last_ptr_->getFramePtr(); auto key_frame_ptr = last_ptr_->getFramePtr();
key_frame_ptr->setState(getCurrentState()); key_frame_ptr->setState(getCurrentState());
key_frame_ptr->setTimeStamp(getCurrentTimeStamp()); key_frame_ptr->setTimeStamp(getCurrentTimeStamp());
key_frame_ptr->setKey(); key_frame_ptr->setKey();
// create motion feature and add it to the key_capture // create motion feature and add it to the key_capture
FeatureBasePtr key_feature_ptr = emplaceFeature(last_ptr_); auto key_feature_ptr = emplaceFeature(last_ptr_);
// create motion constraint and link it to parent feature and other frame (which is origin's frame) // create motion constraint and link it to parent feature and other frame (which is origin's frame)
auto ctr_ptr = emplaceConstraint(key_feature_ptr, origin_ptr_); auto ctr_ptr = emplaceConstraint(key_feature_ptr, origin_ptr_);
// create a new frame // create a new frame
FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME,
getCurrentState(), getCurrentState(),
getCurrentTimeStamp()); getCurrentTimeStamp());
// create a new capture // create a new capture
CaptureMotionPtr new_capture_ptr = emplaceCapture(new_frame_ptr, auto new_capture_ptr = emplaceCapture(new_frame_ptr,
getSensorPtr(), getSensorPtr(),
key_frame_ptr->getTimeStamp(), key_frame_ptr->getTimeStamp(),
Eigen::VectorXs::Zero(data_size_), Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_), Eigen::MatrixXs::Zero(data_size_, data_size_),
last_ptr_->getCalibration(), last_ptr_->getCalibration(),
last_ptr_->getCalibration(), last_ptr_->getCalibration(),
key_frame_ptr); key_frame_ptr);
// reset the new buffer // reset the new buffer
new_capture_ptr->getBuffer().get().clear(); new_capture_ptr->getBuffer().get().clear();
new_capture_ptr->getBuffer().get().push_back( motionZero(key_frame_ptr->getTimeStamp()) ) ; new_capture_ptr->getBuffer().get().push_back( motionZero(key_frame_ptr->getTimeStamp()) ) ;
...@@ -167,7 +167,7 @@ void ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x) ...@@ -167,7 +167,7 @@ void ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
{ {
//std::cout << "ProcessorMotion::findCaptureContainingTimeStamp: ts = " << _ts.getSeconds() << "." << _ts.getNanoSeconds() << std::endl; //std::cout << "ProcessorMotion::findCaptureContainingTimeStamp: ts = " << _ts.getSeconds() << "." << _ts.getNanoSeconds() << std::endl;
auto capture_ptr = last_ptr_; CaptureMotionPtr capture_ptr = last_ptr_;
while (capture_ptr != nullptr) while (capture_ptr != nullptr)
{ {
// capture containing motion previous than the ts found: // capture containing motion previous than the ts found:
...@@ -221,9 +221,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) ...@@ -221,9 +221,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ---------- // ---------- LAST ----------
// Make non-key-frame for last Capture // Make non-key-frame for last Capture
FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME,
_origin_frame->getState(), _origin_frame->getState(),
_origin_frame->getTimeStamp()); _origin_frame->getTimeStamp());
// emplace (emtpy) last Capture // emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(new_frame_ptr, last_ptr_ = emplaceCapture(new_frame_ptr,
getSensorPtr(), getSensorPtr(),
...@@ -259,22 +259,22 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar& ...@@ -259,22 +259,22 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar&
TimeStamp new_ts = _new_keyframe->getTimeStamp(); TimeStamp new_ts = _new_keyframe->getTimeStamp();
// find the capture whose buffer is affected by the new keyframe // find the capture whose buffer is affected by the new keyframe
CaptureMotionPtr existing_capture = findCaptureContainingTimeStamp(new_ts); auto existing_capture = findCaptureContainingTimeStamp(new_ts);
assert(existing_capture != nullptr assert(existing_capture != nullptr
&& "ProcessorMotion::keyFrameCallback: no motion capture containing the required TimeStamp found"); && "ProcessorMotion::keyFrameCallback: no motion capture containing the required TimeStamp found");
// Find the frame acting as the capture's origin // Find the frame acting as the capture's origin
FrameBasePtr keyframe_origin = existing_capture->getOriginFramePtr(); auto keyframe_origin = existing_capture->getOriginFramePtr();
// emplace a new motion capture to the new keyframe // emplace a new motion capture to the new keyframe
CaptureMotionPtr new_capture = emplaceCapture(_new_keyframe, auto new_capture = emplaceCapture(_new_keyframe,
getSensorPtr(), getSensorPtr(),
new_ts, new_ts,
Eigen::VectorXs::Zero(data_size_), Eigen::VectorXs::Zero(data_size_),
existing_capture->getDataCovariance(), existing_capture->getDataCovariance(),
existing_capture->getCalibration(), existing_capture->getCalibration(),
existing_capture->getCalibration(), existing_capture->getCalibration(),
keyframe_origin); keyframe_origin);
// split the buffer // split the buffer
// and give the part of the buffer before the new keyframe to the key_capture // and give the part of the buffer before the new keyframe to the key_capture
...@@ -292,7 +292,7 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar& ...@@ -292,7 +292,7 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar&
} }
// create motion feature and add it to the capture // create motion feature and add it to the capture
FeatureBasePtr new_feature = emplaceFeature(new_capture); auto new_feature = emplaceFeature(new_capture);
// create motion constraint and add it to the feature, and constrain to the other capture (origin) // create motion constraint and add it to the feature, and constrain to the other capture (origin)
emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) ); // XXX it was new_keyframe_origin emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) ); // XXX it was new_keyframe_origin
......
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