removed odom icp

CMakeLists.txt

@@ -215,7 +215,6 @@ SET(SRCS_DTASSC
 SET(SRCS_SOLVER
   )
 SET(SRCS_YAML
-  src/yaml/processor_odom_ICP_yaml.cpp
   src/yaml/sensor_laser_2D_yaml.cpp
   )
 #OPTIONALS
@@ -230,6 +229,9 @@ if(csm_FOUND)
       src/processor/processor_odom_icp.cpp
       src/feature/feature_icp_align.cpp
     )
+  SET(SRCS_YAML
+      src/yaml/processor_odom_ICP_yaml.cpp
+    )
 endif(csm_FOUND)
 #optional HDRS and SRCS
 # ==================EXAMPLE===============

include/laser/processor/processor_odom_icp.h

-#ifndef SRC_PROCESSOR_ODOM_ICP_H_
-#define SRC_PROCESSOR_ODOM_ICP_H_
-
-
-#include "laser/capture/capture_laser_2D.h"
-#include "laser/feature/feature_icp_align.h"
-
-#include <core/processor/processor_tracker.h>
-
-#include <laser_scan_utils/laser_scan_utils.h>
-#include <laser_scan_utils/icp.h>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(ProcessorOdomICP);
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdomICP);
-
-struct ProcessorParamsOdomICP : public ProcessorParamsTracker
-{
-    // ICP Params
-    int use_point_to_line_distance;
-    int max_correspondence_dist;
-    int max_iterations;
-    int use_corr_tricks;
-    double outliers_maxPerc;
-    double outliers_adaptive_order;
-    double outliers_adaptive_mult;
-
-    // Other processor params
-    double vfk_min_dist;
-    double vfk_min_angle;
-    double vfk_min_time;
-    double vfk_min_error;
-    int vfk_max_points;
-
-    ProcessorParamsOdomICP() = default;
-    ProcessorParamsOdomICP(std::string _unique_name, const ParamsServer &_server):
-        ProcessorParamsTracker(_unique_name, _server)
-    {
-        use_point_to_line_distance = _server.getParam<int>      (_unique_name   + "/use_point_to_line_distance");
-        max_correspondence_dist    = _server.getParam<int>      (_unique_name   + "/max_correspondence_dist");
-        max_iterations             = _server.getParam<int>      (_unique_name   + "/max_iterations");
-        use_corr_tricks            = _server.getParam<int>      (_unique_name   + "/use_corr_tricks");
-        outliers_maxPerc           = _server.getParam<double>   (_unique_name   + "/outliers_maxPerc");
-        outliers_adaptive_order    = _server.getParam<double>   (_unique_name   + "/outliers_adaptive_order");
-        outliers_adaptive_mult     = _server.getParam<double>   (_unique_name   + "/outliers_adaptive_mult");
-        vfk_min_dist               = _server.getParam<double>   (_unique_name   + "/vfk_min_dist");
-        vfk_min_angle              = _server.getParam<double>   (_unique_name   + "/vfk_min_angle");
-        vfk_min_time               = _server.getParam<double>   (_unique_name   + "/vfk_min_time");
-        vfk_min_error              = _server.getParam<double>   (_unique_name   + "/vfk_min_error");
-        vfk_max_points             = _server.getParam<int>      (_unique_name   + "/vfk_max_points");
-    }
-    std::string print() {
-        return "\n" + ProcessorParamsTracker::print() + "\n"
-                + "use_point_to_line_distance: "+ std::to_string(use_point_to_line_distance)+ "\n"
-                + "max_correspondence_dist: "   + std::to_string(max_correspondence_dist)   + "\n"
-                + "max_iterations: "            + std::to_string(max_iterations)            + "\n"
-                + "use_corr_tricks: "           + std::to_string(use_corr_tricks)           + "\n"
-                + "outliers_maxPerc: "          + std::to_string(outliers_maxPerc)          + "\n"
-                + "outliers_adaptive_order: "   + std::to_string(outliers_adaptive_order)   + "\n"
-                + "outliers_adaptive_mult: "    + std::to_string(outliers_adaptive_mult)    + "\n"
-                + "vfk_min_dist: "              + std::to_string(vfk_min_dist)              + "\n"
-                + "vfk_min_angle: "             + std::to_string(vfk_min_angle)             + "\n"
-                + "vfk_min_time: "              + std::to_string(vfk_min_time)              + "\n"
-                + "vfk_min_error: "             + std::to_string(vfk_min_error)             + "\n"
-                + "vfk_max_points: "            + std::to_string(vfk_max_points)            + "\n";
-    }
-};
-
-class ProcessorOdomICP : public ProcessorTracker
-{
-    protected:
-        // Useful sensor stuff
-        SensorLaser2DPtr sensor_laser_; // casted pointer to parent
-        laserscanutils::LaserScanParams laser_scan_params_;
-
-        // ICP algorithm
-        std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
-
-        // temporary and carry-on transformations
-        laserscanutils::icpOutput trf_origin_last_;
-        laserscanutils::icpOutput trf_origin_incoming_;
-        laserscanutils::icpOutput trf_last_incoming_;
-
-    public:
-        laserscanutils::icpParams icp_params_;
-        ProcessorParamsOdomICPPtr proc_params_;
-
-    public:
-        ProcessorOdomICP(ProcessorParamsOdomICPPtr _params);
-        WOLF_PROCESSOR_CREATE(ProcessorOdomICP, ProcessorParamsOdomICP);
-        virtual ~ProcessorOdomICP();
-        virtual void configure(SensorBasePtr _sensor) override;
-
-    protected:
-        virtual void preProcess() override;
-
-        virtual unsigned int processKnown() override;
-        virtual unsigned int processNew(const int& _max_features) override;
-
-        virtual bool voteForKeyFrame() override;
-
-        virtual void advanceDerived() override;
-        virtual void resetDerived() override;
-
-
-        virtual void establishFactors() override;
-        FeatureBasePtr emplaceFeature(CaptureBasePtr _capture_laser);
-        FactorBasePtr emplaceFactor(FeatureBasePtr _feature);
-
-        inline bool voteForKeyFrameDistance();
-        inline bool voteForKeyFrameAngle();
-        inline bool voteForKeyFrameTime();
-        inline bool voteForKeyFrameMatchQuality();
-
-
-
-
-};
-
-
-
-
-}
-
-#endif // SRC_PROCESSOR_ODOM_ICP_H_

test/CMakeLists.txt

@@ -16,8 +16,10 @@ target_link_libraries(gtest_example ${PLUGIN_NAME})       #
 wolf_add_gtest(gtest_landmark_polyline gtest_landmark_polyline.cpp)
 target_link_libraries(gtest_landmark_polyline ${PLUGIN_NAME} ${wolf_LIBRARY})
 
-wolf_add_gtest(gtest_processor_odom_icp gtest_processor_odom_icp.cpp)
-target_link_libraries(gtest_processor_odom_icp ${PLUGIN_NAME} ${wolf_LIBRARY})
+if(csm_FOUND)
+	wolf_add_gtest(gtest_processor_odom_icp gtest_processor_odom_icp.cpp)
+	target_link_libraries(gtest_processor_odom_icp ${PLUGIN_NAME} ${wolf_LIBRARY})
+endif(csm_FOUND)
 
 wolf_add_gtest(gtest_polyline_2D gtest_polyline_2D.cpp)
 target_link_libraries(gtest_polyline_2D ${PLUGIN_NAME} ${wolf_LIBRARY})