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

Organize params better

parent e73b64aa
No related branches found
No related tags found
3 merge requests!30Release after RAL,!29After 2nd RAL submission,!3Resolve "new processor: pc matching for demo"
Hide whitespace changes
Inline Side-by-side
include/laser/processor/processor_odom_icp.h
+ 11
− 3
View file @ 1e0d257b
......@@ -70,12 +70,20 @@ struct ProcessorParamsOdomICP : public ProcessorParamsTracker
class ProcessorOdomICP : public ProcessorTracker
{
protected:
laserscanutils::icpOutput origin_last_;
laserscanutils::icpOutput origin_incoming_;
laserscanutils::icpOutput last_incoming_;
// Useful sensor stuff
SensorLaser2DPtr sensor_laser_; // casted pointer to parent
laserscanutils::LaserScanParams laser_scan_params_;
// ICP algorithm
std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
// temporary and carry-on transformations
laserscanutils::icpOutput trf_origin_last_;
laserscanutils::icpOutput trf_origin_incoming_;
laserscanutils::icpOutput trf_last_incoming_;
public:
ProcessorOdomICP(ProcessorParamsOdomICPPtr _params);
virtual ~ProcessorOdomICP();
......
src/processor/processor_odom_icp.cpp
+ 30
− 26
View file @ 1e0d257b
......@@ -21,9 +21,9 @@ ProcessorOdomICP::ProcessorOdomICP(ProcessorParamsOdomICPPtr _params):
icp_tools_ptr_ = std::make_shared<ICP>();
// Frame transforms
origin_last_.res_covar = Eigen::Matrix3s::Identity();
origin_incoming_.res_covar = Eigen::Matrix3s::Identity();
last_incoming_.res_covar = Eigen::Matrix3s::Identity();
trf_origin_last_.res_covar = Eigen::Matrix3s::Identity();
trf_origin_incoming_.res_covar = Eigen::Matrix3s::Identity();
trf_last_incoming_.res_covar = Eigen::Matrix3s::Identity();
}
ProcessorOdomICP::~ProcessorOdomICP()
......@@ -37,12 +37,14 @@ unsigned int ProcessorOdomICP::processKnown()
if (processing_step_ != FIRST_TIME_WITH_PACK && processing_step_ != FIRST_TIME_WITHOUT_PACK) //FIRST_TIME
{
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();
origin_incoming_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(), origin_ptr->getScan(), scan_params, this->icp_params_, origin_last_.res_transf); // Check order
CaptureLaser2DPtr origin_ptr = std::static_pointer_cast<CaptureLaser2D>(origin_ptr_);
CaptureLaser2DPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_);
trf_origin_incoming_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(),
origin_ptr->getScan(),
this->laser_scan_params_,
this->icp_params_,
this->trf_origin_last_.res_transf); // Check order
}
return 0;
......@@ -56,11 +58,12 @@ unsigned int ProcessorOdomICP::processNew(const int& _max_features)
// XXX: Dynamic or static? JS: static is OK.
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();
last_incoming_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(), last_ptr->getScan(), scan_params, this->icp_params_, t_identity);
trf_last_incoming_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(),
last_ptr->getScan(),
this->laser_scan_params_,
this->icp_params_,
t_identity);
return 0;
}
......@@ -74,23 +77,23 @@ bool ProcessorOdomICP::voteForKeyFrame()
inline bool ProcessorOdomICP::voteForKeyFrameDistance()
{
if (origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist)
if (trf_origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist)
{
std::cout << "Distance True" << '\n';
}
return (origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist);
return (trf_origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist);
}
inline bool ProcessorOdomICP::voteForKeyFrameAngle()
{
if (fabs(origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle)
if (fabs(trf_origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle)
{
std::cout << "Angle True" << '\n';
}
return (fabs(origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle);
return (fabs(trf_origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle);
}
inline bool ProcessorOdomICP::voteForKeyFrameTime()
......@@ -107,17 +110,17 @@ inline bool ProcessorOdomICP::voteForKeyFrameTime()
inline bool ProcessorOdomICP::voteForKeyFrameMatchQuality()
{
if (origin_incoming_.error > proc_params_->vfk_min_error || origin_incoming_.nvalid < proc_params_->vfk_max_points)
if (trf_origin_incoming_.error > proc_params_->vfk_min_error || trf_origin_incoming_.nvalid < proc_params_->vfk_max_points)
{
std::cout << "Quality True" << '\n';
}
return (origin_incoming_.error > proc_params_->vfk_min_error || origin_incoming_.nvalid < proc_params_->vfk_max_points) ;
return (trf_origin_incoming_.error > proc_params_->vfk_min_error || trf_origin_incoming_.nvalid < proc_params_->vfk_max_points) ;
}
void ProcessorOdomICP::advanceDerived()
{
origin_last_ = origin_incoming_;
trf_origin_last_ = trf_origin_incoming_;
}
void ProcessorOdomICP::establishFactors()
......@@ -139,7 +142,7 @@ void ProcessorOdomICP::resetDerived()
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>& ro_R_so = r_R_s;
Eigen::Rotation2D<Scalar> so_R_sl = Eigen::Rotation2D<Scalar>(origin_last_.res_transf(2));
Eigen::Rotation2D<Scalar> so_R_sl = Eigen::Rotation2D<Scalar>(trf_origin_last_.res_transf(2));
// 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
......@@ -149,26 +152,27 @@ void ProcessorOdomICP::resetDerived()
// Translation composition
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 * ro_R_so * origin_last_.res_transf.head(2);
Eigen::Vector2s w_p_so_sl = w_R_ro * ro_R_so * trf_origin_last_.res_transf.head(2);
Eigen::Vector2s w_p_sl_rl = w_R_rl * (-last_ptr_->getSensorP()->getState());
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) = w_p_w_rl;
curr_state(2) = origin_ptr_->getFrame()->getState()(2) + origin_last_.res_transf(2);
curr_state(2) = origin_ptr_->getFrame()->getState()(2) + trf_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_;
trf_origin_last_ = trf_last_incoming_;
}
}
void ProcessorOdomICP::configure(SensorBasePtr _sensor)
{
icp_params_.sigma = std::static_pointer_cast<SensorLaser2D>(getSensor())->getScanParams().range_std_dev_;
sensor_laser_ = std::static_pointer_cast<SensorLaser2D>(getSensor());
laser_scan_params_ = sensor_laser_->getScanParams();
}
void ProcessorOdomICP::preProcess()
......@@ -209,7 +213,7 @@ FeatureBasePtr ProcessorOdomICP::emplaceFeature(CaptureBasePtr _capture_laser)
{
CaptureLaser2DPtr capture_laser = std::static_pointer_cast<CaptureLaser2D>(_capture_laser);
return FeatureBase::emplace<FeatureICPAlign>(capture_laser,
origin_last_);
trf_origin_last_);
}
FactorBasePtr ProcessorOdomICP::emplaceFactor(FeatureBasePtr _feature)
......
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