深度相机同时保存深度图和彩色图及合成点云

空格键保存深度图和彩色图，办公室环境三维重建

/home/yake/catkin_ws/src/pcl_in_ros/src/16_02rgb_depth_saver.cpp

rosrun pcl_in_ros rgb_depth_saver

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <boost/foreach.hpp>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>

#include <image_transport/image_transport.h>
#include <image_geometry/pinhole_camera_model.h>

#include <cv_bridge/cv_bridge.h>

// OpenCV2
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

// PCL
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>


#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

using std::cout;
using std::endl;
using std::stringstream;
using std::string;

using namespace cv;
using namespace std;
using namespace sensor_msgs;
using namespace message_filters;
using namespace pcl;

unsigned int filesNum = 1;
bool saveCloud = false;

void
keyboardEventOccured(const visualization::KeyboardEvent& event, void* nothing)
{
    if(event.getKeySym() == "space"&& event.keyDown())
        saveCloud = true;

}

boost::shared_ptr<visualization::CloudViewer>
createViewer()
{
    boost::shared_ptr<visualization::CloudViewer> v(new visualization::CloudViewer("OpenNI viewer"));
    v->registerKeyboardCallback(keyboardEventOccured);

    return(v);
}

void callback(const ImageConstPtr& image_color_msg, const ImageConstPtr& image_depth_msg)
{
    cv::Mat image_color = cv_bridge::toCvCopy(image_color_msg)->image; // BGR8
    //    cv::Mat image_depth = cv_bridge::toCvCopy(image_depth_msg)->image;
    //    cv_bridge::CvImagePtr image_depth = cv_bridge::toCvCopy(image_depth_msg , sensor_msgs::image_encodings::TYPE_32FC1);
    //    cv::normalize(image_depth->image, image_depth->image, 1, 0, cv::NORM_MINMAX);

//    cv_bridge::CvImagePtr image_depth = cv_bridge::toCvCopy(image_depth_msg , sensor_msgs::image_encodings::TYPE_16UC1);
    cv_bridge::CvImagePtr image_depth = cv_bridge::toCvCopy(image_depth_msg);

    cv::imshow("color", image_color);
    cv::imshow("depth", image_depth->image);

    if(saveCloud)
    {

        vector<int>compression_param;
        compression_param.push_back(CV_IMWRITE_JPEG_QUALITY);
        compression_param.push_back(100);//  Highest quality

        vector<int>d_compression_param;
        d_compression_param.push_back(CV_IMWRITE_PNG_COMPRESSION);
        d_compression_param.push_back(0);// png Highest quality

        stringstream stream;
        stream  <<"/home/yake/pcl_gaoxiang/rgbd-slam-tutorial-gx-master/yake_potatochip_kinect/rgb_jpgfile/"<< "rgb"<< filesNum<< ".jpg";
        string filename = stream.str();

        stringstream stream2;
        stream2 <<"/home/yake/pcl_gaoxiang/rgbd-slam-tutorial-gx-master/yake_potatochip_kinect/depth_pngfile/"<< "depth"<< filesNum<< ".png";
        string filename2 = stream2.str();


        imwrite (filename.c_str (), image_color,compression_param);
        //        imwrite (filename2.c_str (), image_depth);

        imwrite(filename2.c_str (), image_depth->image, d_compression_param);

        cout << filename<<" Saved."<<endl;
        cout << filename2<<" Saved."<<endl;

        filesNum++;
        saveCloud = false;

    }

    cv::waitKey(3);
}



int main(int argc, char** argv)
{
    ros::init(argc, argv, "vision_node");

    ros::NodeHandle nh;

    cout<< "Press space to save rgb_raw and depth_raw to a file."<<endl;

    boost::shared_ptr<visualization::CloudViewer> viewer;
    viewer = createViewer();

//    message_filters::Subscriber<Image> image_color_sub(nh,"/camera/rgb/image_raw", 1); // bayer_grbg8
    message_filters::Subscriber<Image> image_color_sub(nh,"/camera/rgb/image_color", 1);// bgr8

//     Use the rqt_reconfigure close registration. Otherwise the depth image has no data.(freenect driver)
    message_filters::Subscriber<Image> image_depth_sub(nh,"/camera/depth/image_raw", 1);// 16UC1
//        message_filters::Subscriber<Image> image_depth_sub(nh,"/camera/depth/image", 1);// 32FC1

//     Open the depth registratioin.
//        message_filters::Subscriber<Image> image_depth_sub(nh,"/camera/depth_registered/image_raw", 1); // 16UC1
//    message_filters::Subscriber<Image> image_depth_sub(nh,"/camera/depth_registered/image", 1); // 32FC1

    typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;
    Synchronizer<MySyncPolicy> sync(MySyncPolicy(5), image_color_sub, image_depth_sub);

    sync.registerCallback(boost::bind(&callback, _1, _2));

    ros::Rate rate(30.0);

    while (ros::ok() && ! viewer->wasStopped())
    {
        ros::spinOnce();
        rate.sleep();
    }


    return 0;
}

===============利用彩色图和深度图合成点云================= 

https://blog.csdn.net/u012700322/article/details/51821249

// 遍历深度图
    for (int m = 0; m < depth.rows; m++)
{
        for (int n=0; n < depth.cols; n++)
        {
            // 获取深度图中(m,n)处的值
            ushort d = depth.ptr<ushort>(m)[n];
            // d 可能没有值，若如此，跳过此点
            if (d == 0)
                continue;
            // d 存在值，则向点云增加一个点
            PointT 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;
            
            // 从rgb图像中获取它的颜色
            // rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
            p.b = rgb.ptr<uchar>(m)[n*3];
            p.g = rgb.ptr<uchar>(m)[n*3+1];
            p.r = rgb.ptr<uchar>(m)[n*3+2];
 
            // 把p加入到点云中
            cloud->points.push_back( p );
        }
}

高翔一起做RGBD-SLAM 

https://www.cnblogs.com/gaoxiang12/tag/%E4%B8%80%E8%B5%B7%E5%81%9ARGB-D%20SLAM/