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

Odd odometry computation to ProcOdomIcp

parent f75c1241
No related branches found
No related tags found
2 merge requests!30Release after RAL,!29After 2nd RAL submission
#include "laser/processor/processor_odom_icp.h"
#include "core/common/wolf.h"
#include "laser/math/laser_tools.h"
#include <core/common/wolf.h>
using namespace wolf;
using namespace laserscanutils;
ProcessorOdomIcp::ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params):
ProcessorTracker("ProcessorOdomIcp", "PO", 2, _params)
ProcessorTracker("ProcessorOdomIcp", "PO", 2, _params),
IsMotion("PO")
{
proc_params_ = _params;
......@@ -53,7 +56,6 @@ ProcessorOdomIcp::ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params):
trf_origin_last_.res_covar = Eigen::Matrix3d::Identity();
trf_origin_incoming_.res_covar = Eigen::Matrix3d::Identity();
trf_last_incoming_.res_covar = Eigen::Matrix3d::Identity();
setStateStructure("PO");
}
ProcessorOdomIcp::~ProcessorOdomIcp()
......@@ -84,46 +86,12 @@ unsigned int ProcessorOdomIcp::processKnown()
CaptureLaser2dPtr origin_ptr = std::static_pointer_cast<CaptureLaser2d>(origin_ptr_);
CaptureLaser2dPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2d>(incoming_ptr_);
// WOLF_INFO("max_correspondence_dist ", this->icp_params_.max_correspondence_dist);
// WOLF_INFO("max_iterations ", this->icp_params_.max_iterations);
// WOLF_INFO("outliers_adaptive_mult ", this->icp_params_.outliers_adaptive_mult);
// WOLF_INFO("outliers_adaptive_order ", this->icp_params_.outliers_adaptive_order);
// WOLF_INFO("outliers_maxPerc ", this->icp_params_.outliers_maxPerc);
// WOLF_INFO("use_corr_tricks ", this->icp_params_.use_corr_tricks);
// WOLF_INFO("use_point_to_line_distance ", this->icp_params_.use_point_to_line_distance);
// WOLF_INFO("angle_max_ ", this->laser_scan_params_.angle_max_);
// WOLF_INFO("angle_min_ ", this->laser_scan_params_.angle_min_);
// WOLF_INFO("angle_std_dev_ ", this->laser_scan_params_.angle_std_dev_);
// WOLF_INFO("angle_step_ ", this->laser_scan_params_.angle_step_);
// WOLF_INFO("range_max_ ", this->laser_scan_params_.range_max_);
// WOLF_INFO("range_min_ ", this->laser_scan_params_.range_min_);
// WOLF_INFO("range_std_dev_ ", this->laser_scan_params_.range_std_dev_);
// WOLF_INFO("scan_time_ ", this->laser_scan_params_.scan_time_);
// WOLF_INFO("ODOM Icp: Aligning scans ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
// WOLF_INFO("Laser scan params\n");
// this->laser_scan_params_.print();
// WOLF_INFO("Icp params\n");
// WOLF_INFO("\n max_correspondence_dist: ", this->icp_params_.max_correspondence_dist,
// "\n use_point_to_line_distance: ", this->icp_params_.use_point_to_line_distance, "\n max_iterations: ", this->icp_params_.max_iterations,
// "\n outliers_adaptive_mult: ", this->icp_params_.outliers_adaptive_mult,
// "\n outliers_adaptive_order: ", this->icp_params_.outliers_adaptive_order, "\n outliers_maxPerc: ", this->icp_params_.outliers_maxPerc,
// "\n use corr tricks: ", this->icp_params_.use_corr_tricks);
trf_origin_incoming_ = icp_tools_ptr_->align(incoming_ptr->getScan(),
origin_ptr->getScan(),
this->laser_scan_params_,
this->icp_params_,
this->trf_origin_last_.res_transf); // Check order
// double score_normalized
// ((double)trf_origin_incoming_.nvalid / (double)min(incoming_ptr->getScan().ranges_raw_.size(), origin_ptr->getScan().ranges_raw_.size()));
// double mean_error = trf_origin_incoming_.error / trf_origin_incoming_.nvalid;
// WOLF_INFO("DBG ------------------------------");
// WOLF_INFO("DBG valid? ", trf_origin_incoming_.valid, " m_er ", mean_error, " ", score_normalized * 100, "% own_id: ", incoming_ptr->id(),
// " other_id: ", origin_ptr->id());
// WOLF_INFO("DBG own_POSE: 0 0 0 other_POSE: ", this->trf_origin_last_.res_transf.transpose(),
// " Icp_guess: ", this->trf_origin_last_.res_transf.transpose(), " Icp_trf_origin_incoming_: ", trf_origin_incoming_.res_transf.transpose());
// WOLF_INFO("DBG odometry");
trf_origin_incoming_.valid = trf_origin_incoming_.valid && trf_origin_incoming_.error / trf_origin_incoming_.nvalid < 5e-2;
}
return 0;
......@@ -134,7 +102,6 @@ unsigned int ProcessorOdomIcp::processNew(const int& _max_features)
Eigen::Vector3d t_identity;
t_identity << 0, 0, 0;
// 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_);
......@@ -144,28 +111,8 @@ unsigned int ProcessorOdomIcp::processNew(const int& _max_features)
this->icp_params_,
t_identity);
// WOLF_INFO("ODOM Icp: Aligning scans ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
// WOLF_INFO("Laser scan params\n");
// this->laser_scan_params_.print();
// WOLF_INFO("Icp params\n");
// WOLF_INFO("\n max_correspondence_dist: ", this->icp_params_.max_correspondence_dist, "\n use_point_to_line_distance: ", this->icp_params_.use_point_to_line_distance, "\n max_iterations: ", this->icp_params_.max_iterations,
// "\n outliers_adaptive_mult: ", this->icp_params_.outliers_adaptive_mult, "\n outliers_adaptive_order: ", this->icp_params_.outliers_adaptive_order, "\n outliers_maxPerc: ",
// this->icp_params_.outliers_maxPerc, "\n use corr tricks: ", this->icp_params_.use_corr_tricks);
trf_last_incoming_ = icp_tools_ptr_->align(incoming_ptr->getScan(), last_ptr->getScan(), this->laser_scan_params_, this->icp_params_, t_identity);
//TEST! TO BE REMOVED
// trf_last_incoming_.res_transf = t_identity;
// WOLF_TRACE("trf_last_incoming_: ", trf_last_incoming_.res_transf.transpose());
// Eigen::Matrix2s R_last;
// double alphal = trf_origin_last_.res_transf(2);
// double alphali = trf_origin_incoming_.res_transf(2) - alphal;
// R_last << cos(alphal), -sin(alphal), sin(alphal), cos(alphal);
// Eigen::Vector3s trf_last_incoming_check;
// trf_last_incoming_check.head(2) = R_last.transpose() * (trf_origin_incoming_.res_transf.head(2) - trf_origin_last_.res_transf.head(2));
// trf_last_incoming_check(2) = alphali;
// WOLF_TRACE("trf_last_incoming_check: ", trf_last_incoming_check.transpose());
trf_last_incoming_.valid = trf_last_incoming_.valid && trf_last_incoming_.error / trf_last_incoming_.nvalid < 5e-2;
return 0;
......@@ -236,60 +183,69 @@ void ProcessorOdomIcp::advanceDerived()
// overwrite last frame's state
Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getState().at('P')) * Rotation2Dd(origin_ptr_->getFrame()->getState().at('O')(0));
Isometry2d ro_T_so = Translation2d(origin_ptr_->getSensorP()->getState()) * Rotation2Dd(origin_ptr_->getSensorO()->getState()(0));
Isometry2d& ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
auto w_T_ro = laser::trf2isometry(origin_ptr_->getFrame()->getP()->getState(), origin_ptr_->getFrame()->getO()->getState());
auto ro_T_so = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorO()->getState());
const auto& ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
// incoming
Isometry2d so_T_si = Translation2d(trf_origin_incoming_.res_transf.head(2)) * Rotation2Dd(trf_origin_incoming_.res_transf(2));
auto so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
Isometry2d w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
Vector3d x_inc; x_inc << w_T_ri.translation() , Rotation2Dd(w_T_ri.rotation()).angle();
// Put the state of incoming in last
last_ptr_->getFrame()->setState(x_inc, "PO", {2,1});
// computing odometry
auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
Isometry2d sl_T_si = so_T_sl.inverse() * so_T_si;
odometry_['P'] = sl_T_si.translation();
odometry_['O'] = Vector1d(Rotation2Dd(sl_T_si.rotation()).angle());
// advance transforms
trf_origin_last_ = trf_origin_incoming_;
}
void ProcessorOdomIcp::resetDerived()
{
// WOLF_INFO("NDEBUG origin ", origin_ptr_->id(), " last ", last_ptr_->id(), " error ", trf_origin_last_.error, " merror ", trf_origin_last_.error/(double) trf_origin_last_.nvalid, " nvalid ", trf_origin_last_.nvalid, " sqrt diag. ", trf_origin_last_.res_covar.diagonal().cwiseSqrt().transpose(), " trnsf ", trf_origin_last_.res_transf.transpose());
if (origin_ptr_ != nullptr and last_ptr_ != nullptr)
{
// std::cout << "NDEBUG origin " << std::setw(4) << origin_ptr_->id()
// << " last " << std::setw(4) << last_ptr_->id()
// << " is valid? " << std::setw(4) << trf_origin_last_.valid
// << " error " << std::setw(8) << trf_origin_last_.error
// << " merror " << std::setw(10) << trf_origin_last_.error / (double)trf_origin_last_.nvalid
// << " nvalid " << std::setw(3) << trf_origin_last_.nvalid
// << " sqrt diag. " << std::setw(10) << trf_origin_last_.res_covar.diagonal().cwiseSqrt().transpose()
// << " trnsf " << std::setw(10) << trf_origin_last_.res_transf.transpose() << std::endl;
}
// Using processing_step_ to ensure that origin, last and incoming are different
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
Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getP()->getState()) * Rotation2Dd(origin_ptr_->getFrame()->getO()->getState()(0));
Isometry2d ro_T_so = Translation2d(origin_ptr_->getSensorP()->getState()) * Rotation2Dd(origin_ptr_->getSensorO()->getState()(0));
Isometry2d &ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
// incoming
Isometry2d so_T_si = Translation2d(trf_origin_incoming_.res_transf.head(2)) * Rotation2Dd(trf_origin_incoming_.res_transf(2));
Isometry2d w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
Vector3d x_current;
x_current << w_T_ri.translation(), Rotation2Dd(w_T_ri.rotation()).angle();
// last
last_ptr_->getFrame()->setState(x_current, "PO", {2,1});
// incoming_ptr_->getFrame()->setState(x_current);
trf_origin_last_ = trf_last_incoming_;
// WOLF_INFO("NDEBUG origin ", origin_ptr_->id(), " last ", last_ptr_->id(), " error ", trf_origin_last_.error, " merror ", trf_origin_last_.error/(double) trf_origin_last_.nvalid, " nvalid ", trf_origin_last_.nvalid, " sqrt diag. ", trf_origin_last_.res_covar.diagonal().cwiseSqrt().transpose(), " trnsf ", trf_origin_last_.res_transf.transpose());
if (origin_ptr_ != nullptr and last_ptr_ != nullptr)
{
//
}
// Using processing_step_ to ensure that origin, last and incoming are different
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
auto w_T_ro = laser::trf2isometry(origin_ptr_->getFrame()->getP()->getState(), origin_ptr_->getFrame()->getO()->getState());
auto ro_T_so = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorO()->getState());
const auto &ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
// incoming
auto so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
Isometry2d w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
Vector3d x_current;
x_current << w_T_ri.translation(), Rotation2Dd(w_T_ri.rotation()).angle();
// last
last_ptr_->getFrame()->setState(x_current, "PO", {2,1});
// incoming_ptr_->getFrame()->setState(x_current);
// computing odometry
auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
Isometry2d sl_T_si = so_T_sl.inverse() * so_T_si;
odometry_['P'] = sl_T_si.translation();
odometry_['O'] = Vector1d(Rotation2Dd(sl_T_si.rotation()).angle());
// advance transforms
trf_origin_last_ = trf_last_incoming_;
}
}
......@@ -321,21 +277,17 @@ FactorBasePtr ProcessorOdomIcp::emplaceFactor(FeatureBasePtr _feature)
VectorComposite ProcessorOdomIcp::getState( const StateStructure& _structure ) const
{
Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getP()->getState()) * Rotation2Dd(origin_ptr_->getFrame()->getO()->getState()(0));
Isometry2d ro_T_so = Translation2d(origin_ptr_->getSensorP()->getState()) * Rotation2Dd(origin_ptr_->getSensorO()->getState()(0));
Isometry2d &ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
auto w_T_ro = laser::trf2isometry(origin_ptr_->getFrame()->getP()->getState(), origin_ptr_->getFrame()->getO()->getState());
auto ro_T_so = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorO()->getState());
const auto& rl_T_sl = ro_T_so; // A reference just to have nice names without computing overhead
// incoming
Isometry2d so_T_si = Translation2d(trf_origin_last_.res_transf.head(2)) * Rotation2Dd(trf_origin_last_.res_transf(2));
Isometry2d w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
Isometry2d w_T_rl = w_T_ro * ro_T_so * so_T_sl * rl_T_sl.inverse();
Vector3d x_current;
VectorComposite state("PO", {w_T_ri.translation(), Vector1d(Rotation2Dd(w_T_ri.rotation()).angle())});
// Eigen::Vector3d state = Vector3d(0, 0, 0);
// state.head(2) << w_T_ri.translation();
// state(2) = Rotation2Dd(w_T_ri.rotation()).angle();
VectorComposite state("PO", {w_T_rl.translation(), Vector1d(Rotation2Dd(w_T_rl.rotation()).angle())});
return state;
}
......
......@@ -161,7 +161,6 @@ TEST_F(ProcessorOdomIcp_Test, solve)
for (auto F : problem->getTrajectory()->getFrameList())
{
if (F->isKey())
// F->setState(0.5 * Vector3d::Random());
F->perturb(0.5);
}
......
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