From 7cc091e6b159fe74747cf1e150bad4f72cd78f69 Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Thu, 23 Apr 2015 20:42:40 +0200
Subject: [PATCH] normal_estimator compile

---
 hand_control/CMakeLists.txt           |  4 ++++
 hand_control/src/normal_estimator.cpp | 18 ++++++++++--------
 2 files changed, 14 insertions(+), 8 deletions(-)

diff --git a/hand_control/CMakeLists.txt b/hand_control/CMakeLists.txt
index 8e9baba..86fa2b0 100644
--- a/hand_control/CMakeLists.txt
+++ b/hand_control/CMakeLists.txt
@@ -121,6 +121,10 @@ add_dependencies(random_pcl_publisher ${catkin_EXPORTED_TARGETS})
 add_executable(pcl_displayer src/pcl_displayer.cpp)
 target_link_libraries(pcl_displayer ${catkin_LIBRARIES})
 add_dependencies(pcl_displayer ${catkin_EXPORTED_TARGETS})
+
+add_executable(normal_estimator src/normal_estimator.cpp)
+target_link_libraries(normal_estimator ${catkin_LIBRARIES})
+add_dependencies(normal_estimator ${catkin_EXPORTED_TARGETS})
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
 # add_dependencies(hand_control_node hand_control_generate_messages_cpp)
diff --git a/hand_control/src/normal_estimator.cpp b/hand_control/src/normal_estimator.cpp
index 71ad3b5..045c8ff 100644
--- a/hand_control/src/normal_estimator.cpp
+++ b/hand_control/src/normal_estimator.cpp
@@ -1,7 +1,7 @@
 #include <ros/ros.h>
 #include <pcl_ros/point_cloud.h>
 #include <pcl/point_types.h>
-#include <pcl/features/normal_3d.h>
+#include <pcl/features/normal_3d_omp.h>
 #include <hand_control/Plan.h>
 
 typedef pcl::PointXYZRGB Point;
@@ -13,14 +13,16 @@ class Callback {
     operator()(const PointCloud::ConstPtr& msg)
     {
       ROS_INFO("PointCloud received");
-      Eigen::Vector4f pcl_coord();
+      Eigen::Vector4f pcl_coord;
       float curvature;
-      std::vector<int> indices();
-      // TODO : choisir tous les indices
+      std::vector<int> indices;
+      // indices : tout le PointCloud
+      for (int i = 0; i < msg->points.size(); ++i)
+        indices.push_back(i);
       estimator.computePointNormal(*msg, indices,
                                     pcl_coord, curvature);
       // publication
-      ROS_INFO("Plan published")
+      ROS_INFO("Plan published");
       publisher.publish(to_Plan(pcl_coord, curvature));
     }
 
@@ -29,12 +31,13 @@ class Callback {
   private:
     ros::Publisher publisher;
 
-    pcl::NormalEstimationOMP<Point, Eigen::Vector4f> estimator;
+    //pcl::NormalEstimationOMP<Point, Eigen::Vector4f> estimator;
+    pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
 
     const hand_control::Plan::ConstPtr
     to_Plan(const Eigen::Vector4f& pcl_coord, const float& curvature)
     {
-      hand_control::Plan::Ptr ros_msg(new Plan());
+      hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
       ros_msg->normal.x = pcl_coord(0); // a
       ros_msg->normal.y = pcl_coord(1); // b
       ros_msg->normal.z = pcl_coord(2); // c
@@ -53,7 +56,6 @@ main(int argc, char** argv)
   // initialisation
   ros::Publisher  publisher = node.advertise<hand_control::Plan>("output", 1);
   Callback callback(publisher);
-
   ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
 
   // démarrage