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
...@@ -189,20 +189,20 @@ class ProcessorCloseLoopICP : public ProcessorBase ...@@ -189,20 +189,20 @@ class ProcessorCloseLoopICP : public ProcessorBase
protected: protected:
virtual void processCapture(CaptureBasePtr) override; 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 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 storeKeyFrame(FrameBasePtr) override;
virtual bool storeCapture(CaptureBasePtr) override; virtual bool storeCapture(CaptureBasePtr) override;
virtual bool voteForKeyFrame() const override; virtual bool voteForKeyFrame() const override;
FrameBasePtrList selectCandidates(FrameBasePtr _keyframe_ptr, const Scalar &_time_tolerance); FrameBasePtrList selectCandidates(FrameBasePtr _keyframe_ptr, const double &_time_tolerance);
std::map<Scalar, CapturesAligned> evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList& _keyframe_candidates); std::map<double, CapturesAligned> evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList& _keyframe_candidates);
CapturesAligned bestCandidate(std::map<Scalar, CapturesAligned>& _capture_candidates); CapturesAligned bestCandidate(std::map<double, CapturesAligned>& _capture_candidates);
FactorBasePtr emplaceFeatureAndFactor(CapturesAligned& _captures_aligned); FactorBasePtr emplaceFeatureAndFactor(CapturesAligned& _captures_aligned);
}; };
......
...@@ -27,7 +27,7 @@ class LocalParametrizationPolylineExtreme : public LocalParametrizationBase ...@@ -27,7 +27,7 @@ class LocalParametrizationPolylineExtreme : public LocalParametrizationBase
virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _point, virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _point,
Eigen::Map<const Eigen::VectorXd>& _delta_theta, Eigen::Map<const Eigen::VectorXd>& _delta_theta,
Eigen::Map<Eigen::VectorXd>& _point_plus_delta_theta) const; 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, virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _point1,
Eigen::Map<const Eigen::VectorXd>& _point2, Eigen::Map<const Eigen::VectorXd>& _point2,
Eigen::Map<Eigen::VectorXd>& _delta_theta); Eigen::Map<Eigen::VectorXd>& _delta_theta);
......
...@@ -84,7 +84,7 @@ void ProcessorCloseLoopICP::processCapture(CaptureBasePtr _capture_ptr) ...@@ -84,7 +84,7 @@ void ProcessorCloseLoopICP::processCapture(CaptureBasePtr _capture_ptr)
// _origin_cpt = cpt_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_) if(key_frames_skipped_ > key_frames_to_wait_)
{ {
...@@ -115,7 +115,7 @@ bool ProcessorCloseLoopICP::triggerInCapture(CaptureBasePtr) const ...@@ -115,7 +115,7 @@ bool ProcessorCloseLoopICP::triggerInCapture(CaptureBasePtr) const
return true; 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; return true;
} }
...@@ -135,7 +135,7 @@ bool ProcessorCloseLoopICP::voteForKeyFrame() const ...@@ -135,7 +135,7 @@ bool ProcessorCloseLoopICP::voteForKeyFrame() const
return false; return false;
} }
FrameBasePtrList ProcessorCloseLoopICP::selectCandidates(FrameBasePtr _keyframe_ptr, const Scalar &_time_tolerance) FrameBasePtrList ProcessorCloseLoopICP::selectCandidates(FrameBasePtr _keyframe_ptr, const double &_time_tolerance)
{ {
FrameBasePtrList candidates; FrameBasePtrList candidates;
int key_frames_counter = 0; int key_frames_counter = 0;
...@@ -154,9 +154,9 @@ FrameBasePtrList ProcessorCloseLoopICP::selectCandidates(FrameBasePtr _keyframe_ ...@@ -154,9 +154,9 @@ FrameBasePtrList ProcessorCloseLoopICP::selectCandidates(FrameBasePtr _keyframe_
WOLF_INFO("%%%%%%%%%%%%%%%%%% CANDIDATES SIZE ", candidates.size()); WOLF_INFO("%%%%%%%%%%%%%%%%%% CANDIDATES SIZE ", candidates.size());
return candidates; 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())); CaptureLaser2DPtr capture_current = std::static_pointer_cast<wolf::CaptureLaser2D>(_keyframe_own->getCaptureOf(this->getSensor()));
for(const auto& capture_own : _keyframe_own->getCaptureList()) for(const auto& capture_own : _keyframe_own->getCaptureList())
{ {
...@@ -171,19 +171,19 @@ std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(Fram ...@@ -171,19 +171,19 @@ std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(Fram
CaptureLaser2DPtr capture_laser_other = std::static_pointer_cast<wolf::CaptureLaser2D>(capture_other); CaptureLaser2DPtr capture_laser_other = std::static_pointer_cast<wolf::CaptureLaser2D>(capture_other);
// Initial guess for alignment // Initial guess for alignment
Eigen::Vector3s transform_guess; Eigen::Vector3d transform_guess;
Eigen::Isometry2ds T_w_rown, T_w_rother, T_r_s, T_sother_sown; Eigen::Isometry2d T_w_rown, T_w_rother, T_r_s, T_sother_sown;
// Transformations // 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 = T_w_rother =
Eigen::Translation2ds(_keyframe_other->getP()->getState()) * Eigen::Rotation2Ds(_keyframe_other->getO()->getState()(0)); Eigen::Translation2d(_keyframe_other->getP()->getState()) * Eigen::Rotation2Dd(_keyframe_other->getO()->getState()(0));
T_r_s = Eigen::Translation2ds(this->getSensor()->getP()->getState()) * Eigen::Rotation2Ds(this->getSensor()->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; T_sother_sown = T_r_s.inverse() * T_w_rother.inverse() * T_w_rown * T_r_s;
// Fill up initial guess // Fill up initial guess
transform_guess.head(2) = T_sother_sown.translation(); 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(); transform_guess(2) = R.angle();
SensorLaser2DPtr sensor_own, sensor_other; SensorLaser2DPtr sensor_own, sensor_other;
...@@ -206,9 +206,9 @@ std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(Fram ...@@ -206,9 +206,9 @@ std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(Fram
laserscanutils::icpOutput trf = laserscanutils::icpOutput trf =
icp_tools_ptr_->align(capture_laser_own->getScan(), capture_laser_other->getScan(), sensor_own->getScanParams(), icp_tools_ptr_->align(capture_laser_own->getScan(), capture_laser_other->getScan(), sensor_own->getScanParams(),
sensor_other->getScanParams(), this->icp_params_, transform_guess); sensor_other->getScanParams(), this->icp_params_, transform_guess);
Scalar points_coeff_used = double points_coeff_used =
((Scalar)trf.nvalid / (Scalar)fmin(capture_laser_own->getScan().ranges_raw_.size(), capture_laser_other->getScan().ranges_raw_.size())); ((double)trf.nvalid / (double)fmin(capture_laser_own->getScan().ranges_raw_.size(), capture_laser_other->getScan().ranges_raw_.size()));
Scalar mean_error = trf.error / trf.nvalid; double mean_error = trf.error / trf.nvalid;
WOLF_INFO("DBG ------------------------------"); 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 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()); 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 ...@@ -223,7 +223,7 @@ std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(Fram
} }
return captures_candidates; 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; return _capture_candidates.begin()->second;
} }
......
...@@ -29,7 +29,7 @@ bool LocalParametrizationPolylineExtreme::plus(Eigen::Map<const Eigen::VectorXd> ...@@ -29,7 +29,7 @@ bool LocalParametrizationPolylineExtreme::plus(Eigen::Map<const Eigen::VectorXd>
} }
bool LocalParametrizationPolylineExtreme::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _point, 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(_point.size() == global_size_ && "Wrong size of input point.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix."); assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
......
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