WIP start coding processor odom icp

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}

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:

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_

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

src/processor/processor_odom_icp_3d.cpp

+#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