diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2aff3964dc1f960767780c9e9571995b0cdac205..3de1ad0855c24d42a947a640eb44b619e0d4b6ed 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -226,21 +226,21 @@ if(PCL_FOUND)
# message("Found PCL. Compiling some extra classes.")
# message(STATUS "PCL DIRS:" ${PCL_DIR})
# message(STATUS "PCL include DIRS:" ${PCL_INCLUDE_DIRS})
- SET(HDRS_CAPTURE
+ SET(HDRS_CAPTURE ${HDRS_CAPTURE}
include/${PROJECT_NAME}/capture/capture_laser_3d.h
)
- SET(HDRS_SENSOR
- include/${PROJECT_NAME}/sensor/sensor_laser_3d.h
- )
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/${PROJECT_NAME}/processor/processor_odom_icp_3d.h
+ include/${PROJECT_NAME}/processor/processor_odom_icp_3d.h
)
- SET(SRCS_CAPTURE
- src/capture/capture_laser_3d.cpp
+ SET(HDRS_SENSOR ${HDRS_SENSOR}
+ include/${PROJECT_NAME}/sensor/sensor_laser_3d.h
)
- SET(SRCS_CAPTURE
+ SET(SRCS_CAPTURE ${SRCS_CAPTURE}
src/capture/capture_laser_3d.cpp
)
+ SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+ src/processor/processor_odom_icp_3d.cpp
+ )
# SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
diff --git a/include/laser/capture/capture_laser_3d.h b/include/laser/capture/capture_laser_3d.h
index a82c35e0d620a92b29cedd300908eb625df24f4c..26c7d9330764fe1195527dae2d78b032b9b88d08 100644
--- a/include/laser/capture/capture_laser_3d.h
+++ b/include/laser/capture/capture_laser_3d.h
@@ -15,6 +15,8 @@ namespace laser
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+WOLF_PTR_TYPEDEFS(CaptureLaser3d);
+
class CaptureLaser3d : public CaptureBase
{
public:
diff --git a/include/laser/processor/processor_odom_icp_3d.h b/include/laser/processor/processor_odom_icp_3d.h
index d0eed4511121a98d4dfaf3588c43377242c373f8..06da63839a3d7deb90a4d3b5ce50808f3c50b360 100644
--- a/include/laser/processor/processor_odom_icp_3d.h
+++ b/include/laser/processor/processor_odom_icp_3d.h
@@ -7,6 +7,8 @@
**************************/
// #include "laser/processor/params_icp.h" //vsainz: parameters not specified yet
#include "laser/sensor/sensor_laser_3d.h"
+#include "laser/capture/capture_laser_3d.h"
+#include "laser/utils/laser3d_tools.h"
#include "core/common/wolf.h"
#include "core/processor/processor_tracker.h"
@@ -17,21 +19,64 @@
**************************/
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/registration/icp.h>
-#include <pcl/registration/icp_nl.h>
-#include <pcl/registration/transforms.h>
+// #include <pcl/io/pcd_io.h>
+// #include <pcl/filters/voxel_grid.h>
+// #include <pcl/registration/icp.h>
+// #include <pcl/registration/icp_nl.h>
+// #include <pcl/registration/transforms.h>
namespace wolf
{
+namespace laser
+{
+
WOLF_PTR_TYPEDEFS(ProcessorOdomIcp3d);
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp3d);
struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMotionProvider
{
-}; // vsainz: empty, for now
+ bool pcl_downsample;
+
+ int icp_max_iterations;
+ double icp_transformation_translation_epsilon;
+ double icp_transformation_rotation_epsilon;
+ double icp_max_correspondence_distance;
+
+ // add more pà rams as required
+
+
+
+ ParamsProcessorOdomIcp3d() = default;
+ 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");
+
+ 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";
+ }
+
+
+
+};
class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
{
@@ -39,16 +84,12 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
// Useful sensor stuff
SensorLaser3dPtr sensor_laser_; // casted pointer to parent
- // ICP algorithm
- // wip
+ CaptureLaser3dPtr origin_laser_, last_laser_, incoming_laser_;
- // temporary and carry-on transformations
+ Eigen::Isometry3d T_origin_last_, T_last_incoming_, T_origin_incoming_, T_robot_sensor_, T_sensor_robot_;
- Eigen::VectorXd odom_origin_;
- Eigen::VectorXd odom_last_;
- Eigen::VectorXd odom_incoming_;
+ pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> registration_solver_;
- Eigen::Isometry3d rl_T_sl_, ro_T_so_;
public:
ParamsProcessorOdomIcp3dPtr params_odom_icp_;
@@ -59,14 +100,14 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
~ProcessorOdomIcp3d() override;
void configure(SensorBasePtr _sensor) override;
+ // API required by MotionProvider
VectorComposite getState(const StateStructure& _structure = "") const override;
TimeStamp getTimeStamp() const override;
VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override;
void setProblem(ProblemPtr _problem_ptr) override;
- Eigen::Vector3d getOriginLastTransform(double& dt) const;
-
protected:
+ // API required by ProcessorTracker
void preProcess() override;
/** \brief Compute transform from origin to incoming.
@@ -112,23 +153,8 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
void establishFactors() override;
FeatureBasePtr emplaceFeature(CaptureBasePtr _capture_laser);
FactorBasePtr emplaceFactor(FeatureBasePtr _feature);
-
- bool voteForKeyFrameDistance() const;
- bool voteForKeyFrameAngle() const;
- bool voteForKeyFrameTime() const;
- bool voteForKeyFrameMatchQuality() const;
-
- template <typename D1, typename D2>
- void convert2dTo3d(const Eigen::Isometry2d& T2, Eigen::MatrixBase<D1>& p3, Eigen::QuaternionBase<D2>& q3) const;
-
- template <typename D1>
- void convert2dTo3d(const Eigen::Isometry2d& T2, Eigen::MatrixBase<D1>& x3) const;
- Eigen::Isometry2d computeIsometry2d(const Eigen::VectorXd& x1, const Eigen::VectorXd& x2) const;
-
- void updateExtrinsicsIsometries();
};
-
+} // namespace laser
} // namespace wolf
-
#endif // SRC_PROCESSOR_ODOM_ICP_3D_H_
diff --git a/include/laser/sensor/sensor_laser_3d.h b/include/laser/sensor/sensor_laser_3d.h
index 496f5466b9a1283194f9bfefe0088061e370b8a3..1a16c56076731001e0e410b8887a52c3cec3d795 100644
--- a/include/laser/sensor/sensor_laser_3d.h
+++ b/include/laser/sensor/sensor_laser_3d.h
@@ -4,7 +4,6 @@
// WOLF includes
#include <core/sensor/sensor_base.h>
-
namespace wolf
{
namespace laser
@@ -13,22 +12,24 @@ namespace laser
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorLaser3d);
struct ParamsSensorLaser3d : public ParamsSensorBase
-{}; // Not yet defined
-
-WOLF_PTR_TYPEDEFS(SensorLaser3d);
-
-
-class SensorLaser3d : public SensorBase
{
- public:
- SensorLaser3d(StateBlockPtr p_ptr, StateBlockPtr _o_ptr);
- ~SensorLaser3d();
-};
-
-
-
-WOLF_SENSOR_CREATE(SensorLaser3d, ParamsSensorLaser3d, 0);
-
+ ParamsSensorLaser3d() = default;
+ ParamsSensorLaser3d(std::string _unique_name, const wolf::ParamsServer& _server)
+ : ParamsSensorBase(_unique_name, _server)
+ {
+ }
+ ~ParamsSensorLaser3d() override = default;
+
+ WOLF_PTR_TYPEDEFS(SensorLaser3d);
+
+ class SensorLaser3d : public SensorBase
+ {
+ public:
+ SensorLaser3d(StateBlockPtr p_ptr, StateBlockPtr _o_ptr);
+ ~SensorLaser3d();
+ };
+
+ WOLF_SENSOR_CREATE(SensorLaser3d, ParamsSensorLaser3d, 0);
} // namespace laser
} // namespace wolf
diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d2372ed77d0eb34bdebce23915bdb566df61f4e2
--- /dev/null
+++ b/src/processor/processor_odom_icp_3d.cpp
@@ -0,0 +1,38 @@
+#include "laser/processor/processor_odom_icp_3d.h"
+
+namespace wolf
+{
+namespace laser
+{
+ProcessorOdomIcp3d::ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params)
+ : ProcessorTracker("ProcessorOdomIcp3d", "PO", 3, _params), MotionProvider("PO", _params)
+{
+ T_origin_incoming_.setIdentity();
+ T_origin_last_.setIdentity();
+ T_last_incoming_.setIdentity();
+ T_robot_sensor_.setIdentity();
+ T_sensor_robot_.setIdentity();
+
+ registration_solver_.setMaximumIterations(_params->icp_max_iterations);
+ registration_solver_.setTransformationRotationEpsilon(_params->icp_transformation_rotation_epsilon);
+ registration_solver_.setTransformationEpsilon(_params->icp_transformation_translation_epsilon);
+ registration_solver_.setMaxCorrespondenceDistance(_params->icp_max_correspondence_distance);
+}
+
+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_sensor_robot_ = T_robot_sensor_.inverse();
+}
+
+void ProcessorOdomIcp3d::preProcess()
+{
+ incoming_laser_ = std::static_pointer_cast<CaptureLaser3d>(incoming_ptr_);
+}
+
+} // namespace laser
+} // namespace wolf
\ No newline at end of file