Skip to content
Snippets Groups Projects
Commit 284305ca authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Revert "Merge branch '21-adapt-to-const-nonconst-new-api' into 'devel'"

This reverts merge request !30
parent 38f96f29
No related branches found
No related tags found
No related merge requests found
...@@ -110,14 +110,10 @@ class SensorGnss : public SensorBase ...@@ -110,14 +110,10 @@ class SensorGnss : public SensorBase
~SensorGnss() override; ~SensorGnss() override;
// Gets // Gets
StateBlockConstPtr getEnuMapTranslation() const; StateBlockPtr getEnuMapTranslation() const;
StateBlockPtr getEnuMapTranslation(); StateBlockPtr getEnuMapRoll() const;
StateBlockConstPtr getEnuMapRoll() const; StateBlockPtr getEnuMapPitch() const;
StateBlockPtr getEnuMapRoll(); StateBlockPtr getEnuMapYaw() const;
StateBlockConstPtr getEnuMapPitch() const;
StateBlockPtr getEnuMapPitch();
StateBlockConstPtr getEnuMapYaw() const;
StateBlockPtr getEnuMapYaw();
const Eigen::Matrix3d& getREnuEcef() const; const Eigen::Matrix3d& getREnuEcef() const;
const Eigen::Vector3d& gettEnuEcef() const; const Eigen::Vector3d& gettEnuEcef() const;
Eigen::Matrix3d getREnuMap() const; Eigen::Matrix3d getREnuMap() const;
...@@ -182,42 +178,22 @@ inline bool SensorGnss::isEnuModeAuto() const ...@@ -182,42 +178,22 @@ inline bool SensorGnss::isEnuModeAuto() const
return params_->ENU_mode == "auto"; return params_->ENU_mode == "auto";
} }
inline StateBlockConstPtr SensorGnss::getEnuMapTranslation() const inline StateBlockPtr SensorGnss::getEnuMapTranslation() const
{ {
return getStateBlock('t'); return getStateBlock('t');
} }
inline StateBlockPtr SensorGnss::getEnuMapTranslation() inline StateBlockPtr SensorGnss::getEnuMapRoll() const
{
return getStateBlock('t');
}
inline StateBlockConstPtr SensorGnss::getEnuMapRoll() const
{
return getStateBlock('r');
}
inline StateBlockPtr SensorGnss::getEnuMapRoll()
{ {
return getStateBlock('r'); return getStateBlock('r');
} }
inline StateBlockConstPtr SensorGnss::getEnuMapPitch() const inline StateBlockPtr SensorGnss::getEnuMapPitch() const
{ {
return getStateBlock('p'); return getStateBlock('p');
} }
inline StateBlockPtr SensorGnss::getEnuMapPitch() inline StateBlockPtr SensorGnss::getEnuMapYaw() const
{
return getStateBlock('p');
}
inline StateBlockConstPtr SensorGnss::getEnuMapYaw() const
{
return getStateBlock('y');
}
inline StateBlockPtr SensorGnss::getEnuMapYaw()
{ {
return getStateBlock('y'); return getStateBlock('y');
} }
......
...@@ -144,11 +144,11 @@ void ProcessorGnssTdcp::processKeyFrame(FrameBasePtr _keyframe) ...@@ -144,11 +144,11 @@ void ProcessorGnssTdcp::processKeyFrame(FrameBasePtr _keyframe)
} }
// Iterate over all KF of the trajectory with GNSS captures // Iterate over all KF of the trajectory with GNSS captures
for (auto KF_rit = getProblem()->getTrajectory()->getFrameMap().rbegin(); for (auto KF_it = getProblem()->getTrajectory()->rbegin();
KF_rit != getProblem()->getTrajectory()->getFrameMap().rend(); KF_it != getProblem()->getTrajectory()->rend();
KF_rit++) KF_it++)
{ {
auto KF_ref = KF_rit->second; auto KF_ref = *KF_it;
if (_keyframe->getTimeStamp() < KF_ref->getTimeStamp()) if (_keyframe->getTimeStamp() < KF_ref->getTimeStamp())
continue; continue;
......
...@@ -410,11 +410,11 @@ void ProcessorTrackerGnss::establishFactors() ...@@ -410,11 +410,11 @@ void ProcessorTrackerGnss::establishFactors()
WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id()); WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id());
FactorBasePtr last_fac_ptr = nullptr; FactorBasePtr last_fac_ptr = nullptr;
for (auto KF_rit = getProblem()->getTrajectory()->getFrameMap().rbegin(); for (auto KF_rit = getProblem()->getTrajectory()->rbegin();
KF_rit != getProblem()->getTrajectory()->getFrameMap().rend(); KF_rit != getProblem()->getTrajectory()->rend();
KF_rit++) KF_rit++)
{ {
FrameBasePtr KF = KF_rit->second; FrameBasePtr KF = (*KF_rit);
WOLF_DEBUG("TDCP BATCH ref frame ", KF->id()); WOLF_DEBUG("TDCP BATCH ref frame ", KF->id());
...@@ -424,7 +424,7 @@ void ProcessorTrackerGnss::establishFactors() ...@@ -424,7 +424,7 @@ void ProcessorTrackerGnss::establishFactors()
continue; continue;
// static cast // static cast
auto ref_cap_gnss = std::static_pointer_cast<CaptureGnss>(KF->getCaptureOf(getSensor(),"CaptureGnss")); auto ref_cap_gnss = std::static_pointer_cast<CaptureGnss>((*KF_rit)->getCaptureOf(getSensor(),"CaptureGnss"));
auto last_cap_gnss = std::static_pointer_cast<CaptureGnss>(last_ptr_); auto last_cap_gnss = std::static_pointer_cast<CaptureGnss>(last_ptr_);
// dt // dt
......
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