C++图像加Lidar点云转写rosbag

近期需要处理一批Lidar+image的数据，拿到的是其他格式，但要转存成rosbag使用，参考部分网上做法，完成并记录。

1.Lidar处理

　　主要是将Lidar点云信息按点转为pcl::PointXYZI，再将其用pcl::toROSMsg(mypoints, ros_Lidar_msg)转换成rosmsg，然后使用bag.write()。

2.Image处理

　　使用cv_bridge::CvImage ros_image作为中介，然后ros_image.toImageMsg()转换成rosmsg，再使用bag.write()。

下述代码为对已匹配好的img+lidar按帧写成bag，部分代码冗余，供参考。

int main(int argc, char **argv)
{
    if (argc < 2) {
        fprintf(stderr, "usage: read_log <logfile> <savepath>\n");
        return 1;
    }

    // Open the log file.
    vector<string> vecImgTimeName;

    string strTimestampfile = argv[1];
    string strLidarPath = strTimestampfile + "/lidar_data_reader/";
    string strImagePath = strTimestampfile + "/img_data_reader/";
    getDirFiles(strImagePath,vecImgTimeName,".jpg");
    sort(vecImgTimeName.begin(), vecImgTimeName.end());

    ros::Time::init();
    std::string output_dir = argv[2];
    cout << "begin to subscribe" << endl;

    pcl::PointCloud<pcl::PointXYZI> points;
    const size_t kMaxNumberOfPoints = 1e6;  // From Readme for raw files.
    points.clear();
    points.reserve(kMaxNumberOfPoints);
    rosbag::Bag bag;
    bag.open(output_dir + "/res.bag", rosbag::bagmode::Write);
    int imageCount = 0;
    int seq = 0;

    for(vector<string>::iterator it=vecImgTimeName.begin();it!=vecImgTimeName.end();it++)
    {
        string tmp = *it;
        string strLidarFile = strLidarPath + tmp + ".txt";
        string strImgFile =  strImagePath + tmp + ".jpg";
        cout<<"Lidar : "<<strLidarFile<<endl;
        cout<<"Image : "<<strImgFile<<endl;
        cv::Mat img;
        vector<LidarPoint> lidar_pts;
        try {
            img = cv::imread(strImgFile);
            if (img.empty())
                throw(img);
            readLidarPts(strLidarFile, lidar_pts);
            if (lidar_pts.empty())
                throw(lidar_pts);
        }

        catch (const cv::Mat &) {
            cerr << "[Fatal Error] Failed to load image. Check for Path." << endl;
        }

        catch (const std::vector<LidarPoint> &) {
            cerr << "[Fatal Error] Failed to load LiDAR points. Check for Path."<< endl;
        }

        pcl::PointCloud<pcl::PointXYZI> mypoints;
        sensor_msgs::PointCloud2 ros_Lidar_msg;
        cv_bridge::CvImage ros_image;
        sensor_msgs::ImagePtr ros_image_msg;
        for(vector<LidarPoint>::iterator iter = lidar_pts.begin();iter!=lidar_pts.end();iter++)
        {
            LidarPoint temp = *iter;
            PointXYZI point;
            float x = temp.x;
            float y = temp.y;
            float z = temp.z;
            int32_t intensity = temp.r;
            if(abs(x)<100000)
            {
                point.x = x;
                point.y = y;
                point.z = z;
                point.intensity = intensity;
            }
            else
            {
                x=0,y=0,z=0;
                point.x = x;
                point.y = y;
                point.z = z;
                point.intensity = intensity;
            }
            mypoints.push_back(point);

        }
        ros::Time timestamp_ros = ros::Time::now();
        pcl_conversions::toPCL(ros::Time::now(), mypoints.header.stamp);
        mypoints.header.frame_id = "velodyne";
        pcl::toROSMsg(mypoints, ros_Lidar_msg);
        ros_Lidar_msg.header.stamp = timestamp_ros;
        ros_Lidar_msg.header.frame_id = "velodyne_points";

        ros_image.image = img;
        ros_image.encoding = "mono8";
        //cout<<"debug_______"<<endl;
        ros::Time timestamp_ros2 = ros::Time::now();
        ros_image_msg = ros_image.toImageMsg();
        ros_image_msg->header.seq = seq;
        ros_image_msg->header.stamp = timestamp_ros2;
        ros_image_msg->header.frame_id = "image";
        seq++;

        bag.write("points_raw", ros_Lidar_msg.header.stamp, ros_Lidar_msg);
        bag.write("image_converter/cam1", ros_Lidar_msg.header.stamp, ros_image_msg);
        cout<<"write one frame: "<<seq<<endl;


    }
    printf("done\n");
    return 0;
}