Skip to content
Snippets Groups Projects
Commit 214fa9cf authored by Víctor Sainz Ubide's avatar Víctor Sainz Ubide
Browse files

Preparing for tomorrow testing

parent 19362bd8
No related branches found
No related tags found
1 merge request!41Draft: Resolve "New branch laser 3d"
Hide whitespace changes
Inline Side-by-side
include/laser/capture/capture_laser_3d.h
+ 3
− 5
View file @ 214fa9cf
...@@ -34,20 +34,18 @@ namespace wolf ...@@ -34,20 +34,18 @@ namespace wolf
namespace laser namespace laser
{ {
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
WOLF_PTR_TYPEDEFS(CaptureLaser3d); WOLF_PTR_TYPEDEFS(CaptureLaser3d);
class CaptureLaser3d : public CaptureBase class CaptureLaser3d : public CaptureBase
{ {
public: 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(); ~CaptureLaser3d();
PointCloud::Ptr getPointCloud() const; pcl::PointCloud<pcl::PointXYZ>::Ptr getPointCloud() const;
private: private:
PointCloud::Ptr point_cloud_; pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_;
}; };
} // namespace laser } // namespace laser
......
include/laser/processor/processor_odom_icp_3d.h
+ 31
− 38
View file @ 214fa9cf
...@@ -47,57 +47,53 @@ namespace wolf ...@@ -47,57 +47,53 @@ namespace wolf
namespace laser namespace laser
{ {
WOLF_PTR_TYPEDEFS(ProcessorOdomIcp3d);
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp3d); WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp3d);
struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMotionProvider struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMotionProvider
{ {
bool pcl_downsample; bool pcl_downsample;
int vote_elapsed_time; double vote_elapsed_time;
int icp_max_iterations; int icp_max_iterations;
double icp_transformation_translation_epsilon; double icp_transformation_translation_epsilon; // squared value of translation epsilon
double icp_transformation_rotation_epsilon; double icp_transformation_rotation_epsilon; // cosinus of rotation angle epsilon
double icp_max_correspondence_distance; double icp_max_correspondence_distance;
// add more pàrams as required
// add more params as required
ParamsProcessorOdomIcp3d() = default; ParamsProcessorOdomIcp3d() = default;
ParamsProcessorOdomIcp3d(std::string _unique_name, const ParamsServer &_server): ParamsProcessorOdomIcp3d(std::string _unique_name, const ParamsServer& _server)
ParamsProcessorTracker(_unique_name, _server), : ParamsProcessorTracker(_unique_name, _server), ParamsMotionProvider(_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<double>(prefix + _unique_name + "/vote_elapsed_time");
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");
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 std::string print() const override
{ {
return "\n" + ParamsProcessorTracker::print() + "\n" // TODO
+ ParamsMotionProvider::print() + "\n"; return "\n" + ParamsProcessorTracker::print() + "\n" + ParamsMotionProvider::print() + "\n";
// + "initial_guess: " + initial_guess + "\n" // + "initial_guess: " + initial_guess + "\n"
// + "keyframe_vote/min_dist: " + std::to_string(vfk_min_dist) + "\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_angle: " + std::to_string(vfk_min_angle) + "\n"
// + "keyframe_vote/min_time: " + std::to_string(vfk_min_time) + "\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/min_error: " + std::to_string(vfk_min_error) + "\n"
// + "keyframe_vote/max_points: " + std::to_string(vfk_max_points) + "\n"; // + "keyframe_vote/max_points: " + std::to_string(vfk_max_points) + "\n";
} }
}; };
WOLF_PTR_TYPEDEFS(ProcessorOdomIcp3d);
class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
{ {
protected: protected:
...@@ -107,19 +103,18 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider ...@@ -107,19 +103,18 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
CaptureLaser3dPtr origin_laser_, last_laser_, incoming_laser_; CaptureLaser3dPtr origin_laser_, last_laser_, incoming_laser_;
Eigen::Isometry3d T_origin_last_, T_last_incoming_, T_origin_incoming_, T_robot_sensor_, T_sensor_robot_; 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_; pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> registration_solver_;
Eigen::Matrix6d transform_cov_; Eigen::Matrix6d transform_cov_;
public: public:
ParamsProcessorOdomIcp3dPtr params_odom_icp_; ParamsProcessorOdomIcp3dPtr params_odom_icp_;
public: public:
ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params); ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params);
WOLF_PROCESSOR_CREATE(ProcessorOdomIcp3d, ParamsProcessorOdomIcp3d);
~ProcessorOdomIcp3d() override; ~ProcessorOdomIcp3d() override;
WOLF_PROCESSOR_CREATE(ProcessorOdomIcp3d, ParamsProcessorOdomIcp3d);
void configure(SensorBasePtr _sensor) override; void configure(SensorBasePtr _sensor) override;
// API required by MotionProvider // API required by MotionProvider
...@@ -171,9 +166,7 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider ...@@ -171,9 +166,7 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
*/ */
void resetDerived() override; void resetDerived() override;
void establishFactors() override; void establishFactors() override;
FeatureBasePtr emplaceFeature(CaptureBasePtr _capture_laser);
FactorBasePtr emplaceFactor(FeatureBasePtr _feature);
}; };
} // namespace laser } // namespace laser
} // namespace wolf } // namespace wolf
......
include/laser/utils/laser3d_tools.h
+ 2
− 4
View file @ 214fa9cf
...@@ -49,10 +49,8 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr ...@@ -49,10 +49,8 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr
bool _downsample = false) bool _downsample = false)
{ {
// Internal PointCloud pointers (boost::shared_ptr) // Internal PointCloud pointers (boost::shared_ptr)
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ref = pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ref = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
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_other =
pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
// Downsample for consistency and speed // Downsample for consistency and speed
// \note enable this for large datasets // \note enable this for large datasets
......
src/capture/capture_laser_3d.cpp
+ 2
− 2
View file @ 214fa9cf
...@@ -26,14 +26,14 @@ namespace wolf ...@@ -26,14 +26,14 @@ namespace wolf
namespace laser 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) : CaptureBase("CaptureLaser3d", _timestamp, _sensor), point_cloud_(_point_cloud)
{ {
} }
CaptureLaser3d::~CaptureLaser3d() {} CaptureLaser3d::~CaptureLaser3d() {}
PointCloud::Ptr CaptureLaser3d::getPointCloud() const pcl::PointCloud<pcl::PointXYZ>::Ptr CaptureLaser3d::getPointCloud() const
{ {
return point_cloud_; return point_cloud_;
} }
......
src/processor/processor_odom_icp_3d.cpp
+ 37
− 35
View file @ 214fa9cf
...@@ -21,7 +21,7 @@ ...@@ -21,7 +21,7 @@
//--------LICENSE_END-------- //--------LICENSE_END--------
#include "laser/processor/processor_odom_icp_3d.h" #include "laser/processor/processor_odom_icp_3d.h"
#include "laser/utils/laser3d_tools.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 namespace wolf
{ {
...@@ -43,7 +43,7 @@ ProcessorOdomIcp3d::ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params) ...@@ -43,7 +43,7 @@ ProcessorOdomIcp3d::ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params)
params_odom_icp_ = _params; params_odom_icp_ = _params;
transform_cov_.setIdentity(); transform_cov_.setIdentity();
} }
ProcessorOdomIcp3d::~ProcessorOdomIcp3d() {} ProcessorOdomIcp3d::~ProcessorOdomIcp3d() {}
...@@ -51,8 +51,8 @@ void ProcessorOdomIcp3d::configure(SensorBasePtr _sensor) ...@@ -51,8 +51,8 @@ void ProcessorOdomIcp3d::configure(SensorBasePtr _sensor)
{ {
sensor_laser_ = std::static_pointer_cast<SensorLaser3d>(_sensor); sensor_laser_ = std::static_pointer_cast<SensorLaser3d>(_sensor);
T_robot_sensor_ = Eigen::Translation3d(getSensor()->getStateBlock('P')->getState()) * T_robot_sensor_ = Eigen::Translation3d(_sensor->getStateBlock('P')->getState()) *
Eigen::Quaterniond(getSensor()->getStateBlock('O')->getState().data()); Eigen::Quaterniond(_sensor->getStateBlock('O')->getState().data());
T_sensor_robot_ = T_robot_sensor_.inverse(); T_sensor_robot_ = T_robot_sensor_.inverse();
} }
...@@ -87,11 +87,11 @@ void ProcessorOdomIcp3d::preProcess() ...@@ -87,11 +87,11 @@ void ProcessorOdomIcp3d::preProcess()
*/ */
unsigned int ProcessorOdomIcp3d::processKnown() unsigned int ProcessorOdomIcp3d::processKnown()
{ {
pairAlign(origin_laser_->getPointCloud(), pairAlign(origin_laser_->getPointCloud(),
incoming_laser_->getPointCloud(), incoming_laser_->getPointCloud(),
T_origin_last_, T_origin_last_,
T_origin_incoming_, T_origin_incoming_,
registration_solver_); registration_solver_);
return 0; return 0;
}; };
...@@ -100,11 +100,11 @@ unsigned int ProcessorOdomIcp3d::processKnown() ...@@ -100,11 +100,11 @@ unsigned int ProcessorOdomIcp3d::processKnown()
*/ */
unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features) unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features)
{ {
pairAlign(last_laser_->getPointCloud(), pairAlign(last_laser_->getPointCloud(),
incoming_laser_->getPointCloud(), incoming_laser_->getPointCloud(),
Eigen::Isometry3d::Identity(), Eigen::Isometry3d::Identity(),
T_last_incoming_, T_last_incoming_,
registration_solver_); registration_solver_);
return 0; return 0;
}; };
...@@ -117,7 +117,7 @@ unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features) ...@@ -117,7 +117,7 @@ unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features)
*/ */
bool ProcessorOdomIcp3d::voteForKeyFrame() const 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; return true;
} }
...@@ -129,18 +129,20 @@ bool ProcessorOdomIcp3d::voteForKeyFrame() const ...@@ -129,18 +129,20 @@ bool ProcessorOdomIcp3d::voteForKeyFrame() const
* Call this when the tracking and keyframe policy work is done and * Call this when the tracking and keyframe policy work is done and
* we need to get ready to accept a new incoming Capture. * we need to get ready to accept a new incoming Capture.
*/ */
void ProcessorOdomIcp3d::advanceDerived(){ void ProcessorOdomIcp3d::advanceDerived()
T_origin_last_ = T_origin_incoming_; {
last_laser_ = incoming_laser_; T_origin_last_ = T_origin_incoming_;
last_laser_ = incoming_laser_;
incoming_laser_ = nullptr; incoming_laser_ = nullptr;
}; };
/** \brief Reset the tracker using the \b last Capture as the new \b origin. /** \brief Reset the tracker using the \b last Capture as the new \b origin.
*/ */
void ProcessorOdomIcp3d::resetDerived(){ void ProcessorOdomIcp3d::resetDerived()
T_origin_last_ = T_last_incoming_; {
origin_laser_ = last_laser_; T_origin_last_ = T_last_incoming_;
last_laser_ = incoming_laser_; origin_laser_ = last_laser_;
last_laser_ = incoming_laser_;
incoming_laser_ = nullptr; incoming_laser_ = nullptr;
}; };
...@@ -152,10 +154,11 @@ TimeStamp ProcessorOdomIcp3d::getTimeStamp() const ...@@ -152,10 +154,11 @@ TimeStamp ProcessorOdomIcp3d::getTimeStamp() const
VectorComposite ProcessorOdomIcp3d::getState(const StateStructure& _structure) const VectorComposite ProcessorOdomIcp3d::getState(const StateStructure& _structure) const
{ {
VectorComposite state_origin = origin_laser_->getFrame()->getState("PO"); 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_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_; 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('P') = T_world_last_robot.translation();
state_last.at('O') = Eigen::Quaterniond(T_world_last_robot.rotation()).coeffs(); state_last.at('O') = Eigen::Quaterniond(T_world_last_robot.rotation()).coeffs();
return state_last; return state_last;
...@@ -167,22 +170,21 @@ VectorComposite ProcessorOdomIcp3d::getState(const TimeStamp& _ts, const StateSt ...@@ -167,22 +170,21 @@ VectorComposite ProcessorOdomIcp3d::getState(const TimeStamp& _ts, const StateSt
return getState(_structure); return getState(_structure);
} }
/**\brief Creates and adds factors from last_ to origin_ /** \brief Creates and adds factors from last_ to origin_
* *
*/ */
void ProcessorOdomIcp3d::establishFactors() void ProcessorOdomIcp3d::establishFactors()
{ {
// emplace a feature of type FeatureMotion undet the capture last_ptr_ or last_laser_ // emplace a feature of type FeatureMotion undet the capture last_ptr_ or last_laser_
Eigen::Vector7d measurement_of_motion ; Eigen::Vector7d measurement_of_motion;
measurement_of_motion.head(3) = T_origin_last_.translation(); measurement_of_motion.head(3) = T_origin_last_.translation();
measurement_of_motion.tail(4) = Eigen::Quaterniond( T_origin_last_.rotation() ).coeffs(); 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 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
// 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 } // namespace laser
......
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