PCL点云-RGBD图像ICP迭代最近点之点云配准与匹配

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;

const double camera_factor=1000;
const double camera_cx=325.5;
const double camera_cy=253.5;
const double camera_fx=518.0;
const double camera_fy=519.0;


int main（int argc char **argv）
{
  ros::init（argc argv “img2join“）;
  ros::NodeHandle nh;

  ros::Publisher pub=nh.advertise（“img_join“1）;

  cv::Mat rgbdepth;
  rgb=cv::imread（“/home/qy/dev/qtcatkin_cam/src/plc/data/rgb/1.png“）;
  depth=cv::imread（“/home/qy/dev/qtcatkin_cam/src/plc/data/depth/1.png“-1）;
  cv::imshow（“aa“rgb）;
  cv::waitKey（30）;
  sensor_msgs::PointCloud2 out;
  pcl::PointCloud cloud1;
  pcl::PointCloud cloud2;
  pcl::PointCloud cloud3;

  // 遍历深度图
  for （int m = 0; m < depth.rows; m++）
      for （int n=0; n < depth.cols; n++）
      {
          // 获取深度图中（mn）处的值
          ushort d = depth.ptr（m）[n];
          // d 可能没有值，若如此，跳过此点