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 <iostream>
#include <string>
#include <opencv2/opencv.hpp>
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/PointStamped.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>

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

<?xml version="1.0"?>
<package>
  <name>coordinate_map</name>
  <version>1.0.0</version>
  <description>coordinate_map package</description>

  <maintainer email="[email protected]">Wangxianwei</maintainer>

  <license>BSD</license>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>
  <build_depend>rostime</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>message_filters</build_depend>
  <build_depend>image_transport</build_depend>
  <build_depend>compressed_image_transport</build_depend>
  <build_depend>compressed_depth_image_transport</build_depend>
  <build_depend>cv_bridge</build_depend>
  <build_depend>tf</build_depend>
  <build_depend>nav_msgs</build_depend>

  <run_depend>message_runtime</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>rostime</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>sensor_msgs</run_depend>
  <run_depend>message_filters</run_depend>
  <run_depend>image_transport</run_depend>
  <run_depend>compressed_image_transport</run_depend>
  <run_depend>compressed_depth_image_transport</run_depend>
  <run_depend>cv_bridge</run_depend>
  <run_depend>tf</run_depend>
  <run_depend>nav_msgs</run_depend>


  <export>
  </export>
</package>