視覺SLAM（3）_RGBD相機的點雲數據生成

本節展示瞭如何將rgbd的深度及圖像信息轉換爲點雲數據的過程，轉換的公式需要一定的相機模型的基礎知識，可以參考高翔的《視覺SLAM十四講》，這裏以奧比的一款相機爲例，理論都比較簡單，理解起來並不困難，這裏直接貼上代碼：

#include<ros/ros.h>
#include<opencv2/highgui/highgui.hpp>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/image_encodings.h>
#include<sensor_msgs/PointCloud2.h>
#include<image_transport/image_transport.h>
//pcl
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include<pcl_conversions/pcl_conversions.h>

#include<message_filters/subscriber.h>
#include<message_filters/synchronizer.h>
#include<message_filters/sync_policies/approximate_time.h>
using namespace std;

sensor_msgs::CameraInfo cameraInfo;
Eigen::Matrix3f m_K;
std::string depth_topic_;
std::string rgb_topic_;
std::string camera_info_topic_;
std::string rgbd_frame_id_;
ros::Publisher pc_publisher;
const float camera_factor = 0.001;
void convertToDepthPointCloud(const cv::Mat &depth_pic,const cv::Mat &_nonZeroCoordinates)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    sensor_msgs::PointCloud2 pub_pointcloud;
    for (int m = 0; m < depth_pic.rows; m++)
    {
        for (int n = 0; n < depth_pic.cols; n++)
        {
            float d = depth_pic.ptr<float>(m)[n]; // 獲取深度圖中(m,n)處的值
            if(d==0.0)
            {
                continue;
            }
            //注入點雲
            pcl::PointXYZRGB point;
            point.z=double(d)*camera_factor;
            point.x=((n-cameraInfo.K.at(2))*point.z/cameraInfo.K.at(0));
            point.y=((m-cameraInfo.K.at(5))*point.z/cameraInfo.K.at(4));
            point.b=depth_pic.data[m*depth_pic.step+n*depth_pic.channels()+0];
            point.g=depth_pic.data[m*depth_pic.step+n*depth_pic.channels()+1];
            point.r=depth_pic.data[m*depth_pic.step+n*depth_pic.channels()+2];
            cloud->points.push_back(point);
        }
    }
    cloud->height=1;
    cloud->width=cloud->points.size();
    cloud->is_dense=false;
    pcl::toROSMsg(*cloud,pub_pointcloud);
    pub_pointcloud.header.frame_id=rgbd_frame_id_;
    pub_pointcloud.header.stamp=ros::Time::now();
    pc_publisher.publish(pub_pointcloud);
}

void PointCloudCallback(const sensor_msgs::ImageConstPtr& _depthImage, const sensor_msgs::ImageConstPtr& _rgbImage)
{
    cv_bridge::CvImageConstPtr rgbImage,depthRaw,depthImage;
    try
    {
        rgbImage=cv_bridge::toCvCopy(_rgbImage,sensor_msgs::image_encodings::BGR8);
    }
    catch(cv_bridge::Exception& ex)
    {
        ROS_ERROR("cv_bridge RGB exception: %s", ex.what());
        return;
    }
    try
    {
        depthImage=cv_bridge::toCvShare(_depthImage,sensor_msgs::image_encodings::TYPE_32FC1);
        depthRaw=cv_bridge::toCvShare(_depthImage,sensor_msgs::image_encodings::TYPE_8UC1);
    }
    catch(cv_bridge::Exception &ex)
    {
        ROS_ERROR("cv_bridge DEPTH exception: %s", ex.what());
        return;
    }
    cv::Mat NonZeroCoordinates;
    cv::findNonZero(depthRaw->image,NonZeroCoordinates);
    convertToDepthPointCloud(depthImage->image,NonZeroCoordinates);
}
int main(int argc,char** argv)
{
    ros::init(argc,argv,"image_listener");
    ros::NodeHandle nh("~");
    nh.param("depth_topic", depth_topic_, std::string("/depth/image_raw"));
    nh.param("rgb_topic", rgb_topic_, std::string("/rgb/image_raw"));
    nh.param("camera_info_topic", camera_info_topic_, std::string("/depth/camera_info"));
    nh.param("rgbd_frame_id",rgbd_frame_id_,std::string("camera_01_depth_optical_frame"));
    try
    {
        ROS_INFO("WAITING FOR CAMERA INFO");
        cameraInfo = *(ros::topic::waitForMessage<sensor_msgs::CameraInfo>(camera_info_topic_, nh));
        m_K <<  cameraInfo.K.at(0), cameraInfo.K.at(1), cameraInfo.K.at(2),
                cameraInfo.K.at(3), cameraInfo.K.at(4), cameraInfo.K.at(5),
                cameraInfo.K.at(6), cameraInfo.K.at(7), cameraInfo.K.at(8);
        std::cout << m_K << std::endl;        
        ROS_INFO("GOT CAMERA INFO");
    }
    catch(std::exception ex)
    {
        ROS_WARN("NO CAMERA INFO");
    }
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, depth_topic_, 1);
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, rgb_topic_, 1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,sensor_msgs::Image> syncPolicy;
    message_filters::Synchronizer<syncPolicy> sync(syncPolicy(10),depth_sub,rgb_sub);
    sync.registerCallback(boost::bind(&PointCloudCallback,_1,_2));
    pc_publisher=nh.advertise<sensor_msgs::PointCloud2>("rgbd_pointcloud",1);
    ros::spin();
}

對應的Launch文件：

<launch>
	<arg name="prefix" value="/camera" />
	<node name="image_listener" pkg="rgbd2pc" type="image_listener" output="screen">	
		<param name="depth_topic" value="$(arg prefix)/depth/image_raw" />
		<param name="camera_info_topic" value="$(arg prefix)/depth/camera_info" />
		<param name="rgb_topic" value="$(arg prefix)/rgb/image_raw" />	
		<param name="rgbd_frame_id" value="camera_depth_optical_frame"/>
	</node>
</launch>

結果：

學會了這裏的方法，以後拿到realsense、奧比也、樂視等RGBD相機，稍微理解一下，直接轉就可以了。