How to construct the point cloud data from the depth data

edit

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

int main(int argc, char** argv)

{ ros::init (argc, argv, "pub_pcl"); ros::NodeHandle nh; ros::Publisher pub = nh.advertise<pointcloud> ("point2", 1);

 PointCloud::Ptr msg (new PointCloud);
 msg->header.frame_id = "some_tf_frame";
 msg->height = 480;
 msg->width = 640;
 //msg->fields.resize(4);
 //msg->fields[0].name = "x";
 //msg->fields[1].name = "y";
 //msg->fields[2].name = "z";
 //msg->fields[3].name = "rgb";
 msg->is_dense = true;
 msg->points.resize(640*480);

for( int v=0,i=0;v<480;v++)
{
  for ( int u = 0; u<640;u++,i++)
   {
    pcl::PointXYZ result;
    result.x = (u-cx)*(RawDepthtoMeters(depth[i]) + minDistance*scaleFactor*(cx/cy);
    result.y = (v-cy)*(RawDepthtoMeters(depth[i]) + minDistance*scaleFactor;
    result.z = RawDepethtoMeters(depth[i]);
    msg->points.push.back(result);
   }
}

 ros::Rate loop_rate(4);  
 while (nh.ok())
{   
 msg->header.stamp = ros::Time::now ();
 pub.publish (msg); 
 ros::spinOnce ();
 loop_rate.sleep ();  
}

}