Calculating incident angle of laserscan and laserpoints 2D

edit

Hi guys,

I'm trying to calculate the angle of incident between a laserscan and laserscanner points. I'm having trouble calculating the normal of the surface from neighbouring points before actually calculating angle of incidence. Here's a figure of what I'm trying to accomplish (the laserscanner is at the corner of the robot). I'm trying to find alpha1 and alpha2

To calculate the normal vector for each point I've tried using PCL, but the values are just (0,0,-1) for all points. I suspect this is because the pcl function expects a 3D pointcloud and I'm actually parsing a 2D pointcloud. I've used the code directly from the pcl documentation:

#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>

{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  ... read, pass in or create a point cloud ...

  // Create the normal estimation class, and pass the input dataset to it
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud (cloud);

  // Create an empty kdtree representation, and pass it to the normal estimation object.
  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  ne.setSearchMethod (tree);

  // Output datasets
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

  // Use all neighbors in a sphere of radius 3cm
  ne.setRadiusSearch (0.03);

  // Compute the features
  ne.compute (*cloud_normals);

  // cloud_normals->points.size () should have the same size as the input cloud->points.size ()*
}

If there's an easier way to calcuate the normal for each point please let me know. Thank you!