diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h index e06ddbc550852c400748504d228c35a908c60d03..56da0c09150ff9927cc0d706ebdcb6d49b53ad0b 100644 --- a/include/laser/processor/processor_odom_icp.h +++ b/include/laser/processor/processor_odom_icp.h @@ -69,11 +69,9 @@ struct ProcessorParamsOdomICP : public ProcessorParamsTracker class ProcessorOdomICP : public ProcessorTracker { protected: - Eigen::Vector3s t_origin_last_; - Eigen::Vector3s t_origin_incoming_; - Eigen::Vector3s t_last_incoming_; - - laserscanutils::icpOutput icp_match_res_; + laserscanutils::icpOutput origin_last_; + laserscanutils::icpOutput origin_incoming_; + laserscanutils::icpOutput last_incoming_; std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_; @@ -99,7 +97,7 @@ class ProcessorOdomICP : public ProcessorTracker virtual void preProcess() override; - FeatureBasePtr emplaceFeature(CaptureLaser2DPtr _capture_laser); + FeatureBasePtr emplaceFeature(CaptureBasePtr _capture_laser); FactorBasePtr emplaceFactor(FeatureBasePtr _feature); diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp index 7974ced1087c195fb06ff207f437b655e00853f9..e96debd03fcf2bcbf6f2453f7c25e9fe50989421 100644 --- a/src/processor/processor_odom_icp.cpp +++ b/src/processor/processor_odom_icp.cpp @@ -6,12 +6,13 @@ using namespace laserscanutils; ProcessorOdomICP::ProcessorOdomICP(ProcessorParamsOdomICPPtr _params): ProcessorTracker("ODOM ICP", _params) { - t_origin_incoming_ << 0, 0, 0; - t_origin_last_ << 0, 0, 0; - t_last_incoming_ << 0, 0, 0; - proc_params_ = _params; + origin_last_.res_covar = Eigen::Matrix3s::Identity(); + origin_incoming_.res_covar = Eigen::Matrix3s::Identity(); + last_incoming_.res_covar = Eigen::Matrix3s::Identity(); + + icp_params_.use_point_to_line_distance = proc_params_->use_point_to_line_distance; icp_params_.max_correspondence_dist = proc_params_->max_correspondence_dist; icp_params_.max_iterations = proc_params_->max_iterations; @@ -31,17 +32,14 @@ unsigned int ProcessorOdomICP::processKnown() { // Match the incoming with the origin, passing the transform from origin to last as initialization - if (processing_step_ == RUNNING_WITH_PACK || processing_step_ == RUNNING_WITHOUT_PACK) + if (processing_step_ != FIRST_TIME_WITH_PACK && processing_step_ != FIRST_TIME_WITHOUT_PACK) //FIRST_TIME { - // XXX: Dynamic or static? CaptureLaser2DPtr origin_ptr = std::static_pointer_cast<CaptureLaser2D>(origin_ptr_); CaptureLaser2DPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_); SensorLaser2DPtr laser_sen = std::static_pointer_cast<SensorLaser2D>(this->getSensor()); laserscanutils::LaserScanParams scan_params = laser_sen->getScanParams(); - icp_match_res_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(), origin_ptr->getScan(), scan_params, this->icp_params_, t_origin_last_); - - t_origin_incoming_ = icp_match_res_.res_transf; + origin_incoming_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(), origin_ptr->getScan(), scan_params, this->icp_params_, origin_last_.res_transf); // Check order } return 0; @@ -56,12 +54,10 @@ unsigned int ProcessorOdomICP::processNew(const int& _max_features) CaptureLaser2DPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_); CaptureLaser2DPtr last_ptr = std::static_pointer_cast<CaptureLaser2D>(last_ptr_); SensorLaser2DPtr laser_sen = std::static_pointer_cast<SensorLaser2D>(this->getSensor()); - laserscanutils::LaserScanParams scan_params = laser_sen->getScanParams(); - icp_match_res_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(), last_ptr->getScan(), scan_params, this->icp_params_, t_identity); - - t_origin_last_ = icp_match_res_.res_transf; + laserscanutils::LaserScanParams scan_params = laser_sen->getScanParams(); + last_incoming_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(), last_ptr->getScan(), scan_params, this->icp_params_, t_identity); return 0; } @@ -75,23 +71,23 @@ bool ProcessorOdomICP::voteForKeyFrame() inline bool ProcessorOdomICP::voteForKeyFrameDistance() { - if (icp_match_res_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist) + if (origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist) { std::cout << "Distance True" << '\n'; } - return (icp_match_res_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist); + return (origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist); } inline bool ProcessorOdomICP::voteForKeyFrameAngle() { - if (fabs(icp_match_res_.res_transf(2)) > proc_params_->vfk_min_angle) + if (fabs(origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle) { std::cout << "Angle True" << '\n'; } - return (fabs(icp_match_res_.res_transf(2)) > proc_params_->vfk_min_angle); + return (fabs(origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle); } inline bool ProcessorOdomICP::voteForKeyFrameTime() @@ -108,52 +104,61 @@ inline bool ProcessorOdomICP::voteForKeyFrameTime() inline bool ProcessorOdomICP::voteForKeyFrameMatchQuality() { - if (icp_match_res_.error > proc_params_->vfk_min_error || icp_match_res_.nvalid < proc_params_->vfk_max_points) + if (origin_incoming_.error > proc_params_->vfk_min_error || origin_incoming_.nvalid < proc_params_->vfk_max_points) { std::cout << "Quality True" << '\n'; } - return (icp_match_res_.error > proc_params_->vfk_min_error || icp_match_res_.nvalid < proc_params_->vfk_max_points) ; + return (origin_incoming_.error > proc_params_->vfk_min_error || origin_incoming_.nvalid < proc_params_->vfk_max_points) ; } void ProcessorOdomICP::advanceDerived() { - t_origin_last_ = t_origin_incoming_; + origin_last_ = origin_incoming_; } void ProcessorOdomICP::establishFactors() { - CaptureLaser2DPtr last_ptr = std::static_pointer_cast<CaptureLaser2D>(last_ptr_); - auto ftr_ptr = emplaceFeature(last_ptr); + auto ftr_ptr = emplaceFeature(last_ptr_); emplaceFactor(ftr_ptr); } void ProcessorOdomICP::resetDerived() { // Using processing_step_ to ensure that origin, last and incoming are different - if (processing_step_ == RUNNING_WITH_PACK || processing_step_ == RUNNING_WITHOUT_PACK) + if (processing_step_ != FIRST_TIME_WITH_PACK && processing_step_ != FIRST_TIME_WITHOUT_PACK) { + // Notation explanation: + // x1_R_x2: Rotation from frame x1 to frame x2 + // x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1 + // Rotation composition - Eigen::Rotation2D<Scalar> r_world_origin = Eigen::Rotation2D<Scalar>(origin_ptr_->getFrame()->getState()(2)); - Eigen::Rotation2D<Scalar> r_robot_sensor = Eigen::Rotation2D<Scalar>(origin_ptr_->getSensorO()->getState()(0)); - Eigen::Rotation2D<Scalar> r_sorigin_slast = Eigen::Rotation2D<Scalar>(t_origin_last_(2)); + Eigen::Rotation2D<Scalar> w_R_ro = Eigen::Rotation2D<Scalar>(origin_ptr_->getFrame()->getState()(2)); + Eigen::Rotation2D<Scalar> r_R_s = Eigen::Rotation2D<Scalar>(origin_ptr_->getSensorO()->getState()(0)); + Eigen::Rotation2D<Scalar> so_R_sl = Eigen::Rotation2D<Scalar>(origin_last_.res_transf(2)); - Eigen::Rotation2D<Scalar> r_world_last = r_world_origin*r_robot_sensor*r_sorigin_slast*r_robot_sensor.inverse(); + // Eigen::Rotation2D<Scalar> w_R_rl = w_R_ro*r_R_s*so_R_sl*r_R_s.inverse(); + // ro_R_so = rl_R_sl = r_R_s + // Planar rotations are commutative + Eigen::Rotation2D<Scalar> w_R_rl = w_R_ro*so_R_sl; // Translation composition - Eigen::Vector2s p_world_origin_w = origin_ptr_->getFrame()->getState().head(2); - Eigen::Vector2s p_robotorigin_sensor_w = r_world_origin*origin_ptr_->getSensorP()->getState(); - Eigen::Vector2s p_sorigin_slast_w = r_world_origin*r_robot_sensor*t_origin_last_.head(2); - Eigen::Vector2s p_sensor_robotlast_w = r_world_last*-origin_ptr_->getSensorP()->getState(); + Eigen::Vector2s w_p_w_ro = origin_ptr_->getFrame()->getState().head(2); + Eigen::Vector2s w_p_ro_so = w_R_ro*origin_ptr_->getSensorP()->getState(); + Eigen::Vector2s w_p_so_sl = w_R_ro*r_R_s*origin_last_.res_transf.head(2); + Eigen::Vector2s w_p_sl_rl = w_R_rl*(-last_ptr_->getSensorP()->getState()); - Eigen::Vector2s p_world_last_w = p_world_origin_w + p_robotorigin_sensor_w - + p_sorigin_slast_w + p_sensor_robotlast_w; + Eigen::Vector2s w_p_w_rl = w_p_w_ro + w_p_ro_so + w_p_so_sl + w_p_sl_rl; Eigen::Vector3s curr_state; - curr_state.head(2) = p_world_last_w; - curr_state(2) = origin_ptr_->getFrame()->getState()(2) + t_origin_last_(2); + curr_state.head(2) = w_p_w_rl; + curr_state(2) = origin_ptr_->getFrame()->getState()(2) + origin_last_.res_transf(2); last_ptr_->getFrame()->setState(curr_state); + std::cout << "[KEY FRAME DONE: ]" << '\n'; + std::cout << "Current state" << last_ptr_->getFrame()->getState() << '\n'; + + origin_last_ = last_incoming_; } } @@ -196,10 +201,11 @@ ProcessorBasePtr ProcessorOdomICP::createAutoConf(const std::string& _unique_nam return prc_ptr; } -FeatureBasePtr ProcessorOdomICP::emplaceFeature(CaptureLaser2DPtr _capture_laser) +FeatureBasePtr ProcessorOdomICP::emplaceFeature(CaptureBasePtr _capture_laser) { - return FeatureBase::emplace<FeatureICPAlign>(_capture_laser, - icp_match_res_); + CaptureLaser2DPtr capture_laser = std::static_pointer_cast<CaptureLaser2D>(_capture_laser); + return FeatureBase::emplace<FeatureICPAlign>(capture_laser, + origin_last_); } FactorBasePtr ProcessorOdomICP::emplaceFactor(FeatureBasePtr _feature)