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

Fix some scalar/double things

parent c4d31fd9
No related branches found
No related tags found
2 merge requests!30Release after RAL,!29After 2nd RAL submission
Hide whitespace changes
Inline Side-by-side
include/laser/processor/processor_closeloop_icp.h
+ 5
− 5
View file @ f7107bd2
......@@ -189,20 +189,20 @@ class ProcessorCloseLoopICP : public ProcessorBase
protected:
virtual void processCapture(CaptureBasePtr) override;
virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) override;
virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
virtual bool triggerInCapture(CaptureBasePtr) const override;
virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) const override;
virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override;
virtual bool storeKeyFrame(FrameBasePtr) override;
virtual bool storeCapture(CaptureBasePtr) override;
virtual bool voteForKeyFrame() const override;
FrameBasePtrList selectCandidates(FrameBasePtr _keyframe_ptr, const Scalar &_time_tolerance);
std::map<Scalar, CapturesAligned> evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList& _keyframe_candidates);
CapturesAligned bestCandidate(std::map<Scalar, CapturesAligned>& _capture_candidates);
FrameBasePtrList selectCandidates(FrameBasePtr _keyframe_ptr, const double &_time_tolerance);
std::map<double, CapturesAligned> evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList& _keyframe_candidates);
CapturesAligned bestCandidate(std::map<double, CapturesAligned>& _capture_candidates);
FactorBasePtr emplaceFeatureAndFactor(CapturesAligned& _captures_aligned);
};
......
include/laser/state_block/local_parametrization_polyline_extreme.h
+ 1
− 1
View file @ f7107bd2
......@@ -27,7 +27,7 @@ class LocalParametrizationPolylineExtreme : public LocalParametrizationBase
virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _point,
Eigen::Map<const Eigen::VectorXd>& _delta_theta,
Eigen::Map<Eigen::VectorXd>& _point_plus_delta_theta) const;
virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _point, Eigen::Map<Eigen::MatrixXd>& _jacobian) const;
virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _point, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const;
virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _point1,
Eigen::Map<const Eigen::VectorXd>& _point2,
Eigen::Map<Eigen::VectorXd>& _delta_theta);
......
src/processor/processor_closeloop_icp.cpp
+ 15
− 15
View file @ f7107bd2
......@@ -84,7 +84,7 @@ void ProcessorCloseLoopICP::processCapture(CaptureBasePtr _capture_ptr)
// _origin_cpt = cpt_ptr;
}
void ProcessorCloseLoopICP::processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance)
void ProcessorCloseLoopICP::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
{
if(key_frames_skipped_ > key_frames_to_wait_)
{
......@@ -115,7 +115,7 @@ bool ProcessorCloseLoopICP::triggerInCapture(CaptureBasePtr) const
return true;
}
bool ProcessorCloseLoopICP::triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) const
bool ProcessorCloseLoopICP::triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const
{
return true;
}
......@@ -135,7 +135,7 @@ bool ProcessorCloseLoopICP::voteForKeyFrame() const
return false;
}
FrameBasePtrList ProcessorCloseLoopICP::selectCandidates(FrameBasePtr _keyframe_ptr, const Scalar &_time_tolerance)
FrameBasePtrList ProcessorCloseLoopICP::selectCandidates(FrameBasePtr _keyframe_ptr, const double &_time_tolerance)
{
FrameBasePtrList candidates;
int key_frames_counter = 0;
......@@ -154,9 +154,9 @@ FrameBasePtrList ProcessorCloseLoopICP::selectCandidates(FrameBasePtr _keyframe_
WOLF_INFO("%%%%%%%%%%%%%%%%%% CANDIDATES SIZE ", candidates.size());
return candidates;
}
std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList &_keyframe_candidates)
std::map<double, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList &_keyframe_candidates)
{
std::map<Scalar, CapturesAligned> captures_candidates;
std::map<double, CapturesAligned> captures_candidates;
CaptureLaser2DPtr capture_current = std::static_pointer_cast<wolf::CaptureLaser2D>(_keyframe_own->getCaptureOf(this->getSensor()));
for(const auto& capture_own : _keyframe_own->getCaptureList())
{
......@@ -171,19 +171,19 @@ std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(Fram
CaptureLaser2DPtr capture_laser_other = std::static_pointer_cast<wolf::CaptureLaser2D>(capture_other);
// Initial guess for alignment
Eigen::Vector3s transform_guess;
Eigen::Isometry2ds T_w_rown, T_w_rother, T_r_s, T_sother_sown;
Eigen::Vector3d transform_guess;
Eigen::Isometry2d T_w_rown, T_w_rother, T_r_s, T_sother_sown;
// Transformations
T_w_rown = Eigen::Translation2ds(_keyframe_own->getP()->getState()) * Eigen::Rotation2Ds(_keyframe_own->getO()->getState()(0));
T_w_rown = Eigen::Translation2d(_keyframe_own->getP()->getState()) * Eigen::Rotation2Dd(_keyframe_own->getO()->getState()(0));
T_w_rother =
Eigen::Translation2ds(_keyframe_other->getP()->getState()) * Eigen::Rotation2Ds(_keyframe_other->getO()->getState()(0));
T_r_s = Eigen::Translation2ds(this->getSensor()->getP()->getState()) * Eigen::Rotation2Ds(this->getSensor()->getO()->getState()(0));
Eigen::Translation2d(_keyframe_other->getP()->getState()) * Eigen::Rotation2Dd(_keyframe_other->getO()->getState()(0));
T_r_s = Eigen::Translation2d(this->getSensor()->getP()->getState()) * Eigen::Rotation2Dd(this->getSensor()->getO()->getState()(0));
T_sother_sown = T_r_s.inverse() * T_w_rother.inverse() * T_w_rown * T_r_s;
// Fill up initial guess
transform_guess.head(2) = T_sother_sown.translation();
Eigen::Rotation2Ds R(T_sother_sown.rotation());
Eigen::Rotation2Dd R(T_sother_sown.rotation());
transform_guess(2) = R.angle();
SensorLaser2DPtr sensor_own, sensor_other;
......@@ -206,9 +206,9 @@ std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(Fram
laserscanutils::icpOutput trf =
icp_tools_ptr_->align(capture_laser_own->getScan(), capture_laser_other->getScan(), sensor_own->getScanParams(),
sensor_other->getScanParams(), this->icp_params_, transform_guess);
Scalar points_coeff_used =
((Scalar)trf.nvalid / (Scalar)fmin(capture_laser_own->getScan().ranges_raw_.size(), capture_laser_other->getScan().ranges_raw_.size()));
Scalar mean_error = trf.error / trf.nvalid;
double points_coeff_used =
((double)trf.nvalid / (double)fmin(capture_laser_own->getScan().ranges_raw_.size(), capture_laser_other->getScan().ranges_raw_.size()));
double mean_error = trf.error / trf.nvalid;
WOLF_INFO("DBG ------------------------------");
WOLF_INFO("DBG valid? ", trf.valid, " m_er ", mean_error, " ", points_coeff_used*100, "% own_id: ", _keyframe_own->id(), " other_id: ", _keyframe_other->id());
WOLF_INFO("DBG own_POSE: ", _keyframe_own->getState().transpose(), " other_POSE: ", _keyframe_other->getState().transpose(), " ICP_guess: ", transform_guess.transpose(), " ICP_trf: ", trf.res_transf.transpose());
......@@ -223,7 +223,7 @@ std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(Fram
}
return captures_candidates;
}
CapturesAligned ProcessorCloseLoopICP::bestCandidate(std::map<Scalar, CapturesAligned> &_capture_candidates)
CapturesAligned ProcessorCloseLoopICP::bestCandidate(std::map<double, CapturesAligned> &_capture_candidates)
{
return _capture_candidates.begin()->second;
}
......
src/state_block/local_parametrization_polyline_extreme.cpp
+ 1
− 1
View file @ f7107bd2
......@@ -29,7 +29,7 @@ bool LocalParametrizationPolylineExtreme::plus(Eigen::Map<const Eigen::VectorXd>
}
bool LocalParametrizationPolylineExtreme::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _point,
Eigen::Map<Eigen::MatrixXd>& _jacobian) const
Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
{
assert(_point.size() == global_size_ && "Wrong size of input point.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment