ROS:二维坐标映射到三维坐标上(彩色与深度图像匹配)（基于深度相机D415）

• 最近在用D435做物体识别抓取的时候发现二维坐标到三维坐标没有一个直接能用的从二维像素点坐标转换到三维坐标的ROS节点，自己之前写过的kinect V2的坐标映射的通用性太差，所以这次写了一个节点，利用深度相机ROS节点发布的深度与彩色对齐的图像话题和图像信息内外参话题，将二维坐标映射到三维坐标系。
• 我挂上来的这个节点是将鼠标指向的二维坐标转换到三维坐标系下，并发布话题可视化到了rviz中，因为我自己的物体识别发布的像素坐标话题是一个自己写的消息文件，这次就不放出来了，需要用的把我这里的鼠标反馈的坐标改成你自己的坐标就行了。
• 原理：深度相机会发布一个裁剪对齐好的深度图像或者彩色图像话题，以D415为例，它的ROS节点发布了/camera/aligned_depth_to_color/image_raw（即深度对齐到彩色的话题），这样只需要找到彩色图像的坐标影色到它的坐标读取一下深度，再通过内参矩阵计算就行了，而内参矩阵也通过了/camera/aligned_depth_to_color/camera_info话题发布出来，直接读取即可。
• 效果图：（ps.为了满足gif尺寸限制只能牺牲下帧率了,左边的鼠标映射到右边的粉色的球）

---

代码：

coordinate_map.cpp

/**********************
coordinate_map.cpp
author: wxw
email: [email protected]
time: 2019-7-29
**********************/

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

using namespace cv;
using namespace std;
class ImageConverter {
private:
	ros::NodeHandle			nh_;
	image_transport::ImageTransport it_;
	image_transport::Subscriber	image_sub_color;//接收彩色图像
	image_transport::Subscriber	image_sub_depth;//接收深度图像

	ros::Subscriber camera_info_sub_;//接收深度图像对应的相机参数话题
	ros::Publisher	arm_point_pub_;//发布一个三维坐标点，可用于可视化

	sensor_msgs::CameraInfo		camera_info;
	geometry_msgs::PointStamped	output_point;

	/* Mat depthImage,colorImage; */
	Mat	colorImage;
	Mat	depthImage	= Mat::zeros( 480, 640, CV_16UC1 );//注意这里要修改为你接收的深度图像尺寸
	Point	mousepos	= Point( 0, 0 );        /* mousepoint to be map */

public:
	//获取鼠标的坐标，通过param指针传出到类成员Point mousepos
	static void on_mouse( int event, int x, int y, int flags, void *param )
	{
		switch ( event )
		{
		case CV_EVENT_MOUSEMOVE:                /* move mouse */
		{
			Point &tmppoint = *(cv::Point *) param;
			tmppoint = Point( x, y );
		} break;
		}
	}


	ImageConverter() : it_( nh_ )
	{
    //topic sub:
		image_sub_depth = it_.subscribe( "/camera/aligned_depth_to_color/image_raw",
						 1, &ImageConverter::imageDepthCb, this );
		image_sub_color = it_.subscribe( "/camera/color/image_raw", 1,
						 &ImageConverter::imageColorCb, this );
		camera_info_sub_ =
			nh_.subscribe( "/camera/aligned_depth_to_color/camera_info", 1,
				       &ImageConverter::cameraInfoCb, this );
    
    //topic pub:
		arm_point_pub_ =
			nh_.advertise<geometry_msgs::PointStamped>( "/mouse_point", 10 );

		cv::namedWindow( "colorImage" );
		setMouseCallback( "colorImage", &ImageConverter::on_mouse,
				  (void *) &mousepos );
	}


	~ImageConverter()
	{
		cv::destroyWindow( "colorImage" );
	}


	void cameraInfoCb( const sensor_msgs::CameraInfo &msg )
	{
		camera_info = msg;
	}


	void imageDepthCb( const sensor_msgs::ImageConstPtr &msg )
	{
		cv_bridge::CvImagePtr cv_ptr;

		try {
			cv_ptr =
				cv_bridge::toCvCopy( msg, sensor_msgs::image_encodings::TYPE_16UC1 );
			depthImage = cv_ptr->image;
		} catch ( cv_bridge::Exception &e ) {
			ROS_ERROR( "cv_bridge exception: %s", e.what() );
			return;
		}
	}


	void imageColorCb( const sensor_msgs::ImageConstPtr &msg )
	{
		cv_bridge::CvImagePtr cv_ptr;
		try {
			cv_ptr		= cv_bridge::toCvCopy( msg, sensor_msgs::image_encodings::BGR8 );
			colorImage	= cv_ptr->image;
		} catch ( cv_bridge::Exception &e ) {
			ROS_ERROR( "cv_bridge exception: %s", e.what() );
			return;
		}
		
		//先查询对齐的深度图像的深度信息，根据读取的camera info内参矩阵求解对应三维坐标
		float	real_z	= 0.001 * depthImage.at<u_int16_t>( mousepos.y, mousepos.x );
		float	real_x	=
			(mousepos.x - camera_info.K.at( 2 ) ) / camera_info.K.at( 0 ) * real_z;
		float real_y =
			(mousepos.y - camera_info.K.at( 5 ) ) / camera_info.K.at( 4 ) * real_z;

		char tam[100];
		sprintf( tam, "(%0.2f,%0.2f,%0.2f)", real_x, real_y, real_z );
		putText( colorImage, tam, mousepos, FONT_HERSHEY_SIMPLEX, 0.6,
			 cvScalar( 0, 0, 255 ), 1 );//打印到屏幕上
		circle( colorImage, mousepos, 2, Scalar( 255, 0, 0 ) );
		output_point.header.frame_id	= "/camera_depth_optical_frame";
		output_point.header.stamp	= ros::Time::now();
		output_point.point.x		= real_x;
		output_point.point.y		= real_y;
		output_point.point.z		= real_z;
		arm_point_pub_.publish( output_point );
		cv::imshow( "colorImage", colorImage );
		cv::waitKey( 1 );
	}
};

int main( int argc, char **argv )
{
	ros::init( argc, argv, "coordinate_map" );
	ImageConverter imageconverter;
	ros::spin();
	return(0);
}

---

CMakeList.txt

cmake_minimum_required(VERSION 2.8.3)
project(coordinate_map)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS roscpp rostime std_msgs sensor_msgs message_filters cv_bridge image_transport 
compressed_image_transport tf compressed_depth_image_transport  geometry_msgs )

## System dependencies are found with CMake's conventions
find_package(OpenCV REQUIRED)


catkin_package(

)


include_directories(include
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

add_executable(coordinate_map src/coordinate_map.cpp)
target_link_libraries(coordinate_map
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}
)

---

package.xml

<package>
  <name>coordinate_mapname>
  <version>1.0.0version>
  <description>coordinate_map packagedescription>

  <maintainer email="[email protected]">Wangxianweimaintainer>

  <license>BSDlicense>

  <buildtool_depend>catkinbuildtool_depend>

  <build_depend>roscppbuild_depend>
  <build_depend>rostimebuild_depend>
  <build_depend>std_msgsbuild_depend>
  <build_depend>sensor_msgsbuild_depend>
  <build_depend>message_filtersbuild_depend>
  <build_depend>image_transportbuild_depend>
  <build_depend>compressed_image_transportbuild_depend>
  <build_depend>compressed_depth_image_transportbuild_depend>
  <build_depend>cv_bridgebuild_depend>
  <build_depend>tfbuild_depend>
  <build_depend>nav_msgsbuild_depend>

  <run_depend>message_runtimerun_depend>
  <run_depend>roscpprun_depend>
  <run_depend>rostimerun_depend>
  <run_depend>std_msgsrun_depend>
  <run_depend>sensor_msgsrun_depend>
  <run_depend>message_filtersrun_depend>
  <run_depend>image_transportrun_depend>
  <run_depend>compressed_image_transportrun_depend>
  <run_depend>compressed_depth_image_transportrun_depend>
  <run_depend>cv_bridgerun_depend>
  <run_depend>tfrun_depend>
  <run_depend>nav_msgsrun_depend>


  <export>
  export>
package>