diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index 4ec5e9adb4de8d885f00ae012e0565e0590e8781..a362d6e7ad3bcc24deffff288cff3b25908d1a97 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -1,11 +1,14 @@
#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;
}
diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp
index 4026584fd5ceadd57d98f5918bf3f6478eaa10e2..d4ba77e8e8a96e2ffb78bd7491272630a26aacfc 100644
--- a/test/gtest_processor_odom_icp.cpp
+++ b/test/gtest_processor_odom_icp.cpp
@@ -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);
}