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

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

Resolve "Adapt to const nonconst new API"

Closes #21

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