雷达相机融合（四）--点云着色_不同材质的物体在点云相机下成像效果

源码一

//*************************************************************************************************
//         融合  image_fire  /cloud_cut  发布：image_color_cloud
//    source ~/catkin_ws/devel/setup.bash &&  rosrun my_lidar_cam_fusion lidar_cam_fusion
//
//****************************************************************************************************

#include <ros/ros.h>
#include <boost/thread.hpp>
#include <iomanip>
#include <thread>
#include <mutex>
#include <unistd.h>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>

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


using namespace sensor_msgs;
using namespace std;
using namespace cv;


class lidar_cam_fusion
{

public:

  lidar_cam_fusion()
  {
      ROS_INFO_STREAM("Image and dense point cloud fusion, Publisher: image_color_cloud");
      M= (Mat_<double>(3, 3) << 1120.793247366556, 0, 404.6240125739656, 0, 1126.958397947255, 248.9162767468943, 0, 0, 1);
      cout << "M=" << M << endl;
      RT = (Mat_<double>(3, 4) << -0.0209971, - 0.999327, 0.0300679 , 0.01784, - 0.0238557 ,- 0.0295651, - 0.999278, - 0.0963614, 0.999495 ,- 0.0216992, - 0.0232189, 0.00614244 );
      cout << "RT=" << RT << endl;
      row = 480; //行
      col = 640;

      colorCloud_pub = n.advertise<sensor_msgs::PointCloud2>("image_color_cloud", 1);//着色点云发布
                                                    //  points_raw   image_raw cloud_cut  /image_fire
      sub1 = n.subscribe<sensor_msgs::PointCloud2>("cloud_cut", 10, &lidar_cam_fusion::myCallback1,this); //,ros::TransportHints().tcpNoDelay());
      sub2 = n.subscribe<sensor_msgs::Image>("image_fire", 1000, &lidar_cam_fusion::myCallback2,this );

    }

  void myCallback1(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg);
  void myCallback2(const  boost::shared_ptr<const sensor_msgs::Image>& imagemsg);

  bool cloud_Queue(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg);

  void syn_image_lidar();

  bool get_image();

  void cloud_Color(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);

  void Publishe_msg(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_color);

private:
   ros::NodeHandle n;

   ros::Subscriber sub1;
   ros::Subscriber sub2;
   ros::Publisher colorCloud_pub;

   std::mutex cloudLock;
   std::mutex imageLock;

   std::deque<sensor_msgs::Image> imageQueue;
   std::deque<sensor_msgs::PointCloud2> cloudQueue;

   sensor_msgs::PointCloud2 currentCloudMsg;
   sensor_msgs::Image currentImageMsg;

   ros::Time time;
   double currentCloudtime;
   double currentImagetime;

   cv::Mat M;
   cv::Mat RT;
   cv::Mat image ;
   int row; //行
   int col;

};


void lidar_cam_fusion::myCallback1(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg)
{
    if (!cloud_Queue(cloudmsg))
        return;
    if (!get_image())
        return;

    syn_image_lidar();
}
void lidar_cam_fusion::myCallback2(const  boost::shared_ptr<const sensor_msgs::Image>& imagemsg)
{
    std::lock_guard<std::mutex> lock2(imageLock);
    imageQueue.push_back(*imagemsg);
}

bool lidar_cam_fusion::cloud_Queue(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg)
{
    std::lock_guard<std::mutex> lock1(cloudLock);
    cloudQueue.push_back(*cloudmsg);
    if (cloudQueue.size() <= 2)
        return false;

    currentCloudMsg = std::move(cloudQueue.front());
    cloudQueue.pop_front();
    time = currentCloudMsg.header.stamp;
    currentCloudtime= currentCloudMsg.header.stamp.toSec();
    return true;
}

bool lidar_cam_fusion::get_image()
{
    std::lock_guard<std::mutex> lock2(imageLock);
    while (!imageQueue.empty())
    {
        if (imageQueue.front().header.stamp.toSec() < currentCloudtime - 0.03)
            imageQueue.pop_front();
        else
            break;
    }

    for (int i = 0; i < (int)imageQueue.size(); ++i)
    {
        sensor_msgs::Image thisImageMsg = imageQueue[i];
        double currentImageTime = thisImageMsg.header.stamp.toSec();
        if (currentImageTime <= currentCloudtime)
        {
            currentImageMsg = std::move(imageQueue.front());
            currentImagetime=currentImageMsg.header.stamp.toSec();
            return true;
        }
        if (currentImageTime > currentCloudtime + 0.03)
            break;
    }
    return false;

}

void lidar_cam_fusion::syn_image_lidar()
{
    //cout<<"currentCloudtime="<<setprecision(16)<<currentCloudtime<<endl;
    //cout<<"currentImagetime="<<setprecision(16)<<currentImagetime<<endl;
    //从ros消息中提取opencv图像与pcl点云
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(currentImageMsg, sensor_msgs::image_encodings::BGR8);
    image= cv_ptr -> image;
    //cv::imshow("FIRE", image);
    //cv::waitKey(1);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//点云
    pcl::fromROSMsg (currentCloudMsg, *cloud);
    cloud_Color(cloud);

}

void lidar_cam_fusion::cloud_Color(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>);
    //遍历点云
    for (int i = 0; i < cloud->points.size(); i++)
    {
        Mat point = (Mat_<double>(4, 1) << cloud->points[i].x, cloud->points[i].y, cloud->points[i].z, 1);
        //cout << "point=" << aa[i][0]<<"  "<< aa[i][1]<<"  "<< aa[i][2] << endl;
        Mat uv(3, 1, CV_64F);
        uv = M * RT*point;
        if (uv.at<double>(2) == 0)
        {
            cout << "uv.at<double>(2)=0" << endl;
            break;
        }
        double u = uv.at<double>(0) / uv.at<double>(2);
        double v = uv.at<double>(1) / uv.at<double>(2);
        int px = int(u + 0.5);
        int py = int(v + 0.5);
        //cout << "(u,v)=" << px << "  "<< py << endl;
        //注意：image.at<Vec3b>(row,rol)  但是像素坐标为(x,y)，即(u,v),即(rol,row)
        pcl::PointXYZRGB colorPoint;
        if (0 <= px && px < col && 0 <= py && py < row)
        {
            colorPoint.x=cloud->points[i].x;
            colorPoint.y=cloud->points[i].y;
            colorPoint.z=cloud->points[i].z;

            colorPoint.b=image.at<Vec3b>(py, px)[0];//{r,g,b};
            colorPoint.g=image.at<Vec3b>(py, px)[1];
            colorPoint.r=image.at<Vec3b>(py, px)[2];

            cloud_color->push_back(colorPoint);
        }
    }
    Publishe_msg(cloud_color);

}


void lidar_cam_fusion::Publishe_msg(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_color)
{
    sensor_msgs::PointCloud2 output;//发布点云话题消息
    pcl::toROSMsg(*cloud_color, output);
    output.header.stamp = time;
    output.header.frame_id = "map";
    colorCloud_pub.publish(output);

}



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

  ros::MultiThreadedSpinner spinner(2);
  spinner.spin();
  //ros::spin();
  return 0;
}


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237

源码二

和源码1功能一样，写法略有不同,可读性更强

//*************************************************************************************************
//         融合  image_fire  /cloud_cut
//    source ~/catkin_ws/devel/setup.bash &&  rosrun my_lidar_cam_fusion my_lidar_cam_fusion
//
//****************************************************************************************************

#include <ros/ros.h>
#include <boost/thread.hpp>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>

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

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


using namespace sensor_msgs;
using namespace std;
using namespace cv;

cv::Mat M;
cv::Mat RT;
int i_=0;
ros::Publisher cloud_pub;

void myCallback(const  boost::shared_ptr<const sensor_msgs::Image>& imagemsg ,const  boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg)
{
    //输入相机内参及雷达与相机间的投影矩阵
    if(i_==0)
    {
        M= (Mat_<double>(3, 3) << 1120.793247366556, 0, 404.6240125739656, 0, 1126.958397947255, 248.9162767468943, 0, 0, 1);
        cout << "M=" << M << endl;
        RT = (Mat_<double>(3, 4) << -0.0209971, - 0.999327, 0.0300679 , 0.01784, - 0.0238557 ,- 0.0295651, - 0.999278, - 0.0963614, 0.999495 ,- 0.0216992, - 0.0232189, 0.00614244 );
        cout << "RT=" << RT << endl;
        i_++;
    }
    //从ros消息中提取opencv图像与pcl点云
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(imagemsg, sensor_msgs::image_encodings::BGR8);
    cv::Mat image  = cv_ptr -> image;
    int row = image.rows; //行
    int col = image.cols;

    pcl::PointCloud<pcl::PointXYZ> cloud;//点云
    pcl::fromROSMsg (*cloudmsg, cloud);


    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>);
    //遍历点云
    for (int i = 0; i < cloud.points.size(); i++)
    {
        Mat point = (Mat_<double>(4, 1) << cloud.points[i].x, cloud.points[i].y, cloud.points[i].z, 1);
        //cout << "point=" << aa[i][0]<<"  "<< aa[i][1]<<"  "<< aa[i][2] << endl;
        Mat uv(3, 1, CV_64F);
        uv = M * RT*point;
        if (uv.at<double>(2) == 0)
        {
            cout << "uv.at<double>(2)=0" << endl;
            break;
        }
        double u = uv.at<double>(0) / uv.at<double>(2);
        double v = uv.at<double>(1) / uv.at<double>(2);
        int px = int(u + 0.5);
        int py = int(v + 0.5);
        //cout << "(u,v)=" << px << "  "<< py << endl;
        //注意：image.at<Vec3b>(row,rol)  但是像素坐标为(x,y)，即(u,v),即(rol,row)
        pcl::PointXYZRGB colorPoint;
        if (0 <= px && px < col && 0 <= py && py < row)
        {
            colorPoint.x=cloud.points[i].x;
            colorPoint.y=cloud.points[i].y;
            colorPoint.z=cloud.points[i].z;

            colorPoint.b=image.at<Vec3b>(py, px)[0];//{r,g,b};
            colorPoint.g=image.at<Vec3b>(py, px)[1];
            colorPoint.r=image.at<Vec3b>(py, px)[2];
            cloud_color->push_back(colorPoint);
        }
    }
    // 发布点云消息
    //cout<< j <<endl;
    sensor_msgs::PointCloud2 output;//发布点云话题消息
    pcl::toROSMsg(*cloud_color, output);
    output.header.stamp = cloudmsg->header.stamp;
    output.header.frame_id = "map";
    cloud_pub.publish(output);


}


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

  ros::NodeHandle nh;
  cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/image_color_cloud", 1);//彩色点云发布

  message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "/image_fire", 1000); //  /image_raw  /image_fire
  message_filters::Subscriber<sensor_msgs::PointCloud2> lidar_sub(nh,"/cloud_cut",1000);//订阅激光雷达  /points_raw /cloud_lidar /cloud_cut

  typedef  message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,sensor_msgs::PointCloud2> MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(2000),image_sub,lidar_sub);    // 同步
  sync.registerCallback(boost::bind(&myCallback, _1, _2));                   // 回调

  ros::MultiThreadedSpinner spinner(2);
  spinner.spin();
  //ros::spin();
  return 0;
}



1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129

效果

由于是红外图像与点云融合，所以视觉效果不太好。

原图：

融合后