diff --git a/include/laser/processor/processor_closeloop_icp.h b/include/laser/processor/processor_closeloop_icp.h index edc50d4bcd5ebcdaa14e283ec39354775c01e3ec..481ab6a42ab0552176291fc8859b18b26623d37c 100644 --- a/include/laser/processor/processor_closeloop_icp.h +++ b/include/laser/processor/processor_closeloop_icp.h @@ -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); }; diff --git a/include/laser/state_block/local_parametrization_polyline_extreme.h b/include/laser/state_block/local_parametrization_polyline_extreme.h index f57dd3a4a50d70843cf9e2307fd34c656e0ee8a1..8abe58ce3419dd0aa4c053d3ee302ae3efeb3a90 100644 --- a/include/laser/state_block/local_parametrization_polyline_extreme.h +++ b/include/laser/state_block/local_parametrization_polyline_extreme.h @@ -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); diff --git a/src/processor/processor_closeloop_icp.cpp b/src/processor/processor_closeloop_icp.cpp index 8e5417f4cf92817f25d9acd8671e0a835db070da..fe491488daed46ed31b51febb65e0420bfcbd9e6 100644 --- a/src/processor/processor_closeloop_icp.cpp +++ b/src/processor/processor_closeloop_icp.cpp @@ -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; } diff --git a/src/state_block/local_parametrization_polyline_extreme.cpp b/src/state_block/local_parametrization_polyline_extreme.cpp index 741c708a8f9c4f7e08bcd768456aca0d71cf25b4..50999a48da61aeb39b75fa81d0e5b33e94f062c7 100644 --- a/src/state_block/local_parametrization_polyline_extreme.cpp +++ b/src/state_block/local_parametrization_polyline_extreme.cpp @@ -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.");