Kinect相关topic

（1）RGB图像：/camera/rgb/image_color

    ROS数据格式：sensor_msgs/Image
    OPENCV数据格式：Mat
    图像尺寸：640*480
    像素数据类型：8UC3

（2）深度图像：/camera/depth/image

    ROS数据格式：sensor_msgs/Image
    OPENCV数据格式：Mat
    图像尺寸：640*480
    像素数据类型：32FC1 或 16UC1

（3）点云数据（无整合RGB）: /camera/depth/points

    ROS数据格式：sensor_msgs/Image
    PCL点云库数据格式：pcl::PointCloud<pcl::PointXYZ>
    图像尺寸：有序点云，640*480
    像素数据类型：double

RGB以及深度图像的像素数据类型非常关键，一旦出错便不能得到正确的坐标

像素坐标系到世界坐标系的转化

[u,v,d]—->[x,y,z]

// 相机内参
const double camera_factor=1000;
const double camera_cx=325.5;
const double camera_cy=253.5;
const double camera_fx=518.0;
const double camera_fy=519.0;

//深度图中(m,n)处的值
ushort d = depth.ptr<ushort>(m)[n];

//点P的世界坐标
p.z = double(d)/camera_factor;
p.x = (n-camera_cx)*p.z/camera_fx;
p.y = (m-camera_cy)*p.z/camera_fy;

camera_factor是深度图里给的数据与实际距离的比例。

由于深度图给的都是short (mm单位)，camera_factor通常为1000。

[x,y,z]—->[u,v,d]

u = (x*fx)/z + cx ;
v = (y*fy)/z + cy ;
d = z*camera_factor