How do you take LaserScan data and convert to a point cloud and also do ICP in c++?

edit

So I am still learning ROS, c++ and Ubuntu (14.04), and I am stuck and haven't been able to implement any similar solution thus far. I am hoping someone else has been in a similar situation and can help me through it. My goal is to take a Hokuyo UST-20LX laser scanner using the urg_node ROS package and do this: Start it up, take the mean of the first 40 scans and set that as my "origin" point for the world. Move the sensor, take that next scan, use an interative closest point algorithm (ICP) to calculate the translation and rotation between the two scans, move to the next point and take another scan and repeat the process indefinitaly. At each step I want to publish the transformation matrix (or new estimated position/velocity) resulting from the ICP step. So far I am still stuck on either working on the range data in the LaserScan message as an entire array to do any manipulation manually, or to use the PCL to work with point clouds.

The only thing I have working is the ROS c++ subscriber code below, I can do a for loop to output each individual element of the range array and convert that into cartesian coordinates but this is so time intensive that it makes no sense to do it this way. As a side note I am unable to verify that a pointCloud2 is published via rviz so I have not been able to see if my PointCloud2 is actually being generated. Can someone shine a light on a more appropriate way or maybe explain what I am doing wrong.

Code so far::

#include "ros/ros.h" #include "std_msgs/String.h" #include "sensor_msgs/LaserScan.h" #include "laser_geometry/laser_geometry.h" #include "tf/message_filter.h" #include "message_filters/subscriber.h" #include "sensor_msgs/PointCloud.h"

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)

{

    for(int i=0;i<scan->ranges.size();i++)

    {   float range = scan->ranges[i];

    ROS_INFO("RANGE[%d]=%f",i,range);

//tried converting ranges to point cloud

sensor_msgs::PointCloud2 cloud;

laser_geometry::LaserProjection projector;

    projector.projectLaser(*scan, cloud);

//ROS_INFO("point cloud data =%f",cloud); //tried converting each range to x,y,z coords

float th = scan->angle_min+((float)i/4)*(3.141592653589/180);

float x = range*sin(th);

float y = range*cos(th);

float z = 0;

ROS_INFO("PC Coord = %f, %f, %f, %f",x,y,z,th); 

}

}

int main(int argc, char **argv)

{

ros::init(argc, argv, "subs");

ros::NodeHandle n;

ros::Subscriber subs = n.subscribe("scan", 10, scanCallback);

ros::spin();

return 0;

}