• 最近在用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: wangxianwei1994@foxmail.com 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 mouseposstatic 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="wangxianwei1994@foxmail.com">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>

總結

以上是生活随笔為你收集整理的ROS:二维坐标映射到三维坐标上(彩色与深度图像匹配)（基于深度相机D415）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。