test added & factory registered

CMakeLists.txt

@@ -233,8 +233,11 @@ if(PCL_FOUND)
   SET(SRCS_CAPTURE ${SRCS_CAPTURE}
     src/capture/capture_laser_3d.cpp
     )
+    SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+    src/processor/processor_odom_icp_3d.cpp
+  )
   SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
-      src/processor/processor_odom_icp_3d.cpp
+      src/sensor/sensor_laser_3d.cpp
     )
 endif(PCL_FOUND)
 

include/laser/sensor/sensor_laser_3d.h

@@ -33,7 +33,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorLaser3d);
 struct ParamsSensorLaser3d : public ParamsSensorBase
 {
     ParamsSensorLaser3d() = default;
-    ParamsSensorLaser3d(std::string _unique_name, const wolf::ParamsServer& _server)
+    ParamsSensorLaser3d(std::string _unique_name, const ParamsServer& _server)
         : ParamsSensorBase(_unique_name, _server)
     {
     }
@@ -46,7 +46,7 @@ class SensorLaser3d : public SensorBase
 {
   public:
     SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr);
-    SensorLaser3d(const Eigen::VectorXd& _extrinsics, const std::shared_ptr<ParamsSensorLaser3d> _params);
+    SensorLaser3d(const Eigen::VectorXd& _extrinsics, ParamsSensorLaser3dPtr _params);
     ~SensorLaser3d();
     WOLF_SENSOR_CREATE(SensorLaser3d, ParamsSensorLaser3d, 7);
     ParamsSensorLaser3dPtr params_laser3d_;

src/processor/processor_odom_icp_3d.cpp

@@ -86,11 +86,14 @@ void ProcessorOdomIcp3d::preProcess()
  */
 unsigned int ProcessorOdomIcp3d::processKnown()
 {
-    pairAlign(origin_laser_->getPointCloud(),
-              incoming_laser_->getPointCloud(),
-              T_origin_last_,
-              T_origin_incoming_,
-              registration_solver_);
+    if (origin_ptr_ && (incoming_ptr_ != origin_ptr_))
+    {
+        pairAlign(origin_laser_->getPointCloud(),
+                  incoming_laser_->getPointCloud(),
+                  T_origin_last_,
+                  T_origin_incoming_,
+                  registration_solver_);
+    }
     return 0;
 };
 
@@ -186,4 +189,10 @@ void ProcessorOdomIcp3d::establishFactors()
         feature, feature, origin_laser_->getFrame(), shared_from_this(), false, TOP_MOTION);
 };
 
-}  // namespace wolf
+}  // namespace wolf
\ No newline at end of file
+#include "core/processor/factory_processor.h"
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorOdomIcp3d);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorOdomIcp3d);
+}  // namespace wolf

src/sensor/sensor_laser_3d.cpp

@@ -21,6 +21,9 @@
 //--------LICENSE_END--------
 #include "laser/sensor/sensor_laser_3d.h"
 
+#include <core/state_block/state_block_derived.h>
+#include <core/state_block/state_quaternion.h>
+
 namespace wolf
 {
 
@@ -29,17 +32,25 @@ SensorLaser3d::SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr)
 {
 }
 
-SensorLaser3d::SensorLaser3d(const Eigen::VectorXd& _extrinsics, const std::shared_ptr<ParamsSensorLaser3d> _params);
-SensorBase("SensorLaser3d",
-           std::make_shared<StatePoint3d>(_extrinsics.head(3), true),
-           std::make_shared<StateQuaternion>(_extrinsics.tail(4), true),
-           nullptr,
-           0),
-    params_laser3d_(_params)
+SensorLaser3d::SensorLaser3d(const Eigen::VectorXd& _extrinsics, ParamsSensorLaser3dPtr _params)
+    : SensorBase("SensorLaser3d",
+                 std::make_shared<StatePoint3d>(_extrinsics.head(3), true),
+                 std::make_shared<StateQuaternion>(_extrinsics.tail(4), true),
+                 nullptr,
+                 0),
+      params_laser3d_(_params)
 {
     //
 }
 
 SensorLaser3d::~SensorLaser3d() {}
 
-}  // namespace wolf
+}  // namespace wolf
\ No newline at end of file
+
+// Register in the FactorySensor and the ParameterFactory
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorLaser3d)
+WOLF_REGISTER_SENSOR_AUTO(SensorLaser3d)
+} // namespace wolf
+

test/yaml/processor_odom_icp_3d.yaml → test/yaml/problem_odom_icp_3d.yaml

@@ -7,6 +7,9 @@ config:
       $state:
         P: [0,0,0]
         O: [0,0,0,1]
+      $sigma:
+        P: [.1,.1,.1]
+        O: [.1,.1,.1]
       time_tolerance: 0.05
     tree_manager: 
       type: "None"
@@ -31,4 +34,15 @@ config:
     icp_max_iterations:  20
     icp_transformation_rotation_epsilon:  0.99
     icp_transformation_translation_epsilon: 1e-6
-    icp_max_correspondence_distance:  0.5
+    icp_max_correspondence_distance:  0.5
\ No newline at end of file
+    keyframe_vote:
+      voting_active: false
+      min_features_for_keyframe : 0
+    apply_loss_function: false
+    max_new_features: 0
+    state_getter: true
+    state_priority: 1
+    pcl_downsample: true
+    vote_elapsed_time: 0.99
+
+