From 214fa9cf70a8a2ae90ebefb25706de53e6ba58a5 Mon Sep 17 00:00:00 2001
From: vsainz <vsainz@iri.upc.edu>
Date: Thu, 4 Aug 2022 17:49:33 +0200
Subject: [PATCH] Preparing for tomorrow testing
---
include/laser/capture/capture_laser_3d.h | 8 +--
.../laser/processor/processor_odom_icp_3d.h | 69 ++++++++----------
include/laser/utils/laser3d_tools.h | 6 +-
src/capture/capture_laser_3d.cpp | 4 +-
src/processor/processor_odom_icp_3d.cpp | 72 ++++++++++---------
5 files changed, 75 insertions(+), 84 deletions(-)
diff --git a/include/laser/capture/capture_laser_3d.h b/include/laser/capture/capture_laser_3d.h
index 35540419e..7917cb0d7 100644
--- a/include/laser/capture/capture_laser_3d.h
+++ b/include/laser/capture/capture_laser_3d.h
@@ -34,20 +34,18 @@ namespace wolf
namespace laser
{
-typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
-
WOLF_PTR_TYPEDEFS(CaptureLaser3d);
class CaptureLaser3d : public CaptureBase
{
public:
- CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, PointCloud::Ptr _point_cloud);
+ CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud);
~CaptureLaser3d();
- PointCloud::Ptr getPointCloud() const;
+ pcl::PointCloud<pcl::PointXYZ>::Ptr getPointCloud() const;
private:
- PointCloud::Ptr point_cloud_;
+ pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_;
};
} // namespace laser
diff --git a/include/laser/processor/processor_odom_icp_3d.h b/include/laser/processor/processor_odom_icp_3d.h
index e66b941d4..8ac0c656b 100644
--- a/include/laser/processor/processor_odom_icp_3d.h
+++ b/include/laser/processor/processor_odom_icp_3d.h
@@ -47,57 +47,53 @@ namespace wolf
namespace laser
{
-WOLF_PTR_TYPEDEFS(ProcessorOdomIcp3d);
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp3d);
struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMotionProvider
{
- bool pcl_downsample;
+ bool pcl_downsample;
- int vote_elapsed_time;
+ double vote_elapsed_time;
- int icp_max_iterations;
- double icp_transformation_translation_epsilon;
- double icp_transformation_rotation_epsilon;
+ int icp_max_iterations;
+ double icp_transformation_translation_epsilon; // squared value of translation epsilon
+ double icp_transformation_rotation_epsilon; // cosinus of rotation angle epsilon
double icp_max_correspondence_distance;
-
- // add more pà rams as required
-
+ // add more params as required
ParamsProcessorOdomIcp3d() = default;
- ParamsProcessorOdomIcp3d(std::string _unique_name, const ParamsServer &_server):
- ParamsProcessorTracker(_unique_name, _server),
- ParamsMotionProvider(_unique_name, _server)
+ ParamsProcessorOdomIcp3d(std::string _unique_name, const ParamsServer& _server)
+ : ParamsProcessorTracker(_unique_name, _server), ParamsMotionProvider(_unique_name, _server)
{
+ pcl_downsample = _server.getParam<bool>(prefix + _unique_name + "/pcl_downsample");
- pcl_downsample = _server.getParam<bool>(prefix + _unique_name + "/pcl_downsample");
-
- vote_elapsed_time = _server.getParam<int>(prefix + _unique_name + "/vote_elapsed_time");
-
- icp_max_iterations = _server.getParam<int>(prefix + _unique_name + "/icp_max_iterations");
- icp_transformation_translation_epsilon = _server.getParam<double>(prefix + _unique_name + "/icp_transformation_translation_epsilon");
- icp_transformation_rotation_epsilon = _server.getParam<double>(prefix + _unique_name + "/icp_transformation_rotation_epsilon");
- icp_max_correspondence_distance = _server.getParam<double>(prefix + _unique_name + "/icp_max_correspondence_distance");
+ vote_elapsed_time = _server.getParam<double>(prefix + _unique_name + "/vote_elapsed_time");
+ icp_max_iterations = _server.getParam<int>(prefix + _unique_name + "/icp_max_iterations");
+ icp_transformation_translation_epsilon =
+ _server.getParam<double>(prefix + _unique_name + "/icp_transformation_translation_epsilon");
+ icp_transformation_rotation_epsilon =
+ _server.getParam<double>(prefix + _unique_name + "/icp_transformation_rotation_epsilon");
+ icp_max_correspondence_distance =
+ _server.getParam<double>(prefix + _unique_name + "/icp_max_correspondence_distance");
}
std::string print() const override
- {
- return "\n" + ParamsProcessorTracker::print() + "\n"
- + ParamsMotionProvider::print() + "\n";
- // + "initial_guess: " + initial_guess + "\n"
- // + "keyframe_vote/min_dist: " + std::to_string(vfk_min_dist) + "\n"
- // + "keyframe_vote/min_angle: " + std::to_string(vfk_min_angle) + "\n"
- // + "keyframe_vote/min_time: " + std::to_string(vfk_min_time) + "\n"
- // + "keyframe_vote/min_error: " + std::to_string(vfk_min_error) + "\n"
- // + "keyframe_vote/max_points: " + std::to_string(vfk_max_points) + "\n";
+ {
+ // TODO
+ return "\n" + ParamsProcessorTracker::print() + "\n" + ParamsMotionProvider::print() + "\n";
+ // + "initial_guess: " + initial_guess + "\n"
+ // + "keyframe_vote/min_dist: " + std::to_string(vfk_min_dist) + "\n"
+ // + "keyframe_vote/min_angle: " + std::to_string(vfk_min_angle) + "\n"
+ // + "keyframe_vote/min_time: " + std::to_string(vfk_min_time) + "\n"
+ // + "keyframe_vote/min_error: " + std::to_string(vfk_min_error) + "\n"
+ // + "keyframe_vote/max_points: " + std::to_string(vfk_max_points) + "\n";
}
-
-
-
};
+WOLF_PTR_TYPEDEFS(ProcessorOdomIcp3d);
+
class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
{
protected:
@@ -107,19 +103,18 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
CaptureLaser3dPtr origin_laser_, last_laser_, incoming_laser_;
Eigen::Isometry3d T_origin_last_, T_last_incoming_, T_origin_incoming_, T_robot_sensor_, T_sensor_robot_;
+
pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> registration_solver_;
Eigen::Matrix6d transform_cov_;
-
-
public:
ParamsProcessorOdomIcp3dPtr params_odom_icp_;
public:
ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params);
- WOLF_PROCESSOR_CREATE(ProcessorOdomIcp3d, ParamsProcessorOdomIcp3d);
~ProcessorOdomIcp3d() override;
+ WOLF_PROCESSOR_CREATE(ProcessorOdomIcp3d, ParamsProcessorOdomIcp3d);
void configure(SensorBasePtr _sensor) override;
// API required by MotionProvider
@@ -171,9 +166,7 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
*/
void resetDerived() override;
- void establishFactors() override;
- FeatureBasePtr emplaceFeature(CaptureBasePtr _capture_laser);
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature);
+ void establishFactors() override;
};
} // namespace laser
} // namespace wolf
diff --git a/include/laser/utils/laser3d_tools.h b/include/laser/utils/laser3d_tools.h
index b3a61908b..f1c19fa21 100644
--- a/include/laser/utils/laser3d_tools.h
+++ b/include/laser/utils/laser3d_tools.h
@@ -49,10 +49,8 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr
bool _downsample = false)
{
// Internal PointCloud pointers (boost::shared_ptr)
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ref =
- pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_other =
- pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ref = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_other = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
// Downsample for consistency and speed
// \note enable this for large datasets
diff --git a/src/capture/capture_laser_3d.cpp b/src/capture/capture_laser_3d.cpp
index c228afe9f..63fe33ceb 100644
--- a/src/capture/capture_laser_3d.cpp
+++ b/src/capture/capture_laser_3d.cpp
@@ -26,14 +26,14 @@ namespace wolf
namespace laser
{
-CaptureLaser3d::CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, PointCloud::Ptr _point_cloud)
+CaptureLaser3d::CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud)
: CaptureBase("CaptureLaser3d", _timestamp, _sensor), point_cloud_(_point_cloud)
{
}
CaptureLaser3d::~CaptureLaser3d() {}
-PointCloud::Ptr CaptureLaser3d::getPointCloud() const
+pcl::PointCloud<pcl::PointXYZ>::Ptr CaptureLaser3d::getPointCloud() const
{
return point_cloud_;
}
diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp
index 2b1ed003d..92e2dfdaf 100644
--- a/src/processor/processor_odom_icp_3d.cpp
+++ b/src/processor/processor_odom_icp_3d.cpp
@@ -21,7 +21,7 @@
//--------LICENSE_END--------
#include "laser/processor/processor_odom_icp_3d.h"
#include "laser/utils/laser3d_tools.h"
-#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
+#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
namespace wolf
{
@@ -43,7 +43,7 @@ ProcessorOdomIcp3d::ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params)
params_odom_icp_ = _params;
transform_cov_.setIdentity();
- }
+}
ProcessorOdomIcp3d::~ProcessorOdomIcp3d() {}
@@ -51,8 +51,8 @@ void ProcessorOdomIcp3d::configure(SensorBasePtr _sensor)
{
sensor_laser_ = std::static_pointer_cast<SensorLaser3d>(_sensor);
- T_robot_sensor_ = Eigen::Translation3d(getSensor()->getStateBlock('P')->getState()) *
- Eigen::Quaterniond(getSensor()->getStateBlock('O')->getState().data());
+ T_robot_sensor_ = Eigen::Translation3d(_sensor->getStateBlock('P')->getState()) *
+ Eigen::Quaterniond(_sensor->getStateBlock('O')->getState().data());
T_sensor_robot_ = T_robot_sensor_.inverse();
}
@@ -87,11 +87,11 @@ void ProcessorOdomIcp3d::preProcess()
*/
unsigned int ProcessorOdomIcp3d::processKnown()
{
- pairAlign(origin_laser_->getPointCloud(),
- incoming_laser_->getPointCloud(),
- T_origin_last_,
- T_origin_incoming_,
- registration_solver_);
+ pairAlign(origin_laser_->getPointCloud(),
+ incoming_laser_->getPointCloud(),
+ T_origin_last_,
+ T_origin_incoming_,
+ registration_solver_);
return 0;
};
@@ -100,11 +100,11 @@ unsigned int ProcessorOdomIcp3d::processKnown()
*/
unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features)
{
- pairAlign(last_laser_->getPointCloud(),
- incoming_laser_->getPointCloud(),
- Eigen::Isometry3d::Identity(),
- T_last_incoming_,
- registration_solver_);
+ pairAlign(last_laser_->getPointCloud(),
+ incoming_laser_->getPointCloud(),
+ Eigen::Isometry3d::Identity(),
+ T_last_incoming_,
+ registration_solver_);
return 0;
};
@@ -117,7 +117,7 @@ unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features)
*/
bool ProcessorOdomIcp3d::voteForKeyFrame() const
{
- if (incoming_laser_->getTimeStamp() - origin_laser_->getTimeStamp() > params_odom_icp_->vote_elapsed_time)
+ if (incoming_laser_->getTimeStamp() - origin_laser_->getTimeStamp() > params_odom_icp_->vote_elapsed_time)
{
return true;
}
@@ -129,18 +129,20 @@ bool ProcessorOdomIcp3d::voteForKeyFrame() const
* Call this when the tracking and keyframe policy work is done and
* we need to get ready to accept a new incoming Capture.
*/
-void ProcessorOdomIcp3d::advanceDerived(){
- T_origin_last_ = T_origin_incoming_;
- last_laser_ = incoming_laser_;
+void ProcessorOdomIcp3d::advanceDerived()
+{
+ T_origin_last_ = T_origin_incoming_;
+ last_laser_ = incoming_laser_;
incoming_laser_ = nullptr;
};
/** \brief Reset the tracker using the \b last Capture as the new \b origin.
*/
-void ProcessorOdomIcp3d::resetDerived(){
- T_origin_last_ = T_last_incoming_;
- origin_laser_ = last_laser_;
- last_laser_ = incoming_laser_;
+void ProcessorOdomIcp3d::resetDerived()
+{
+ T_origin_last_ = T_last_incoming_;
+ origin_laser_ = last_laser_;
+ last_laser_ = incoming_laser_;
incoming_laser_ = nullptr;
};
@@ -152,10 +154,11 @@ TimeStamp ProcessorOdomIcp3d::getTimeStamp() const
VectorComposite ProcessorOdomIcp3d::getState(const StateStructure& _structure) const
{
- VectorComposite state_origin = origin_laser_->getFrame()->getState("PO");
- Eigen::Isometry3d T_world_origin_robot = Eigen::Translation3d(state_origin.at('P')) * Eigen::Quaterniond(state_origin.at('O').data());
+ VectorComposite state_origin = origin_laser_->getFrame()->getState("PO");
+ Eigen::Isometry3d T_world_origin_robot =
+ Eigen::Translation3d(state_origin.at('P')) * Eigen::Quaterniond(state_origin.at('O').data());
Eigen::Isometry3d T_world_last_robot = T_world_origin_robot * T_robot_sensor_ * T_origin_last_ * T_sensor_robot_;
- VectorComposite state_last;
+ VectorComposite state_last;
state_last.at('P') = T_world_last_robot.translation();
state_last.at('O') = Eigen::Quaterniond(T_world_last_robot.rotation()).coeffs();
return state_last;
@@ -167,22 +170,21 @@ VectorComposite ProcessorOdomIcp3d::getState(const TimeStamp& _ts, const StateSt
return getState(_structure);
}
-/**\brief Creates and adds factors from last_ to origin_
+/** \brief Creates and adds factors from last_ to origin_
*
*/
void ProcessorOdomIcp3d::establishFactors()
{
-// emplace a feature of type FeatureMotion undet the capture last_ptr_ or last_laser_
- Eigen::Vector7d measurement_of_motion ;
+ // emplace a feature of type FeatureMotion undet the capture last_ptr_ or last_laser_
+ Eigen::Vector7d measurement_of_motion;
measurement_of_motion.head(3) = T_origin_last_.translation();
- measurement_of_motion.tail(4) = Eigen::Quaterniond( T_origin_last_.rotation() ).coeffs();
- auto feature = FeatureBase::emplace<FeatureBase>(last_laser_, "FeatureBase", measurement_of_motion, transform_cov_ );
-
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(feature, feature, origin_laser_->getFrame(), shared_from_this(), false , TOP_MOTION);
-
-// emplace a factor of type FactorRelativePose3dWithExtrinsics under the feature above
-// TODO
+ measurement_of_motion.tail(4) = Eigen::Quaterniond(T_origin_last_.rotation()).coeffs();
+ auto feature =
+ FeatureBase::emplace<FeatureBase>(last_laser_, "FeatureBase", measurement_of_motion, transform_cov_);
+ // emplace a factor of type FactorRelativePose3dWithExtrinsics under the feature above
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(
+ feature, feature, origin_laser_->getFrame(), shared_from_this(), false, TOP_MOTION);
};
} // namespace laser
--
GitLab