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

/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/