简单运用
很久没有留意这篇博客了, 上面的内容, 是之前一个工作中需要用到Kinect2, 所以试着弄了一下, 将其整理下来形成这篇博客的.
今天突然有一个同学在站内给我私信, 问我这篇博客的内容. 分享出来的东西帮助到了别人, 确实很高兴! 关于这位同学问到的问题, 其实在前面的工作中, 我也实现过. 下面试着回忆整理一下.
保存图片
其实目的就一个, 将Kinect2的RGB图保存下来. 在前面的叙述中, 输入rosrun kinect2_viewer kinect2_viewer sd cloud来查看显示效果. 这句命令实质就是运行kinect2_viewer包中的kinect2_viewer节点, 给定两个参数sd 和 cloud. 进入这个包的路径, 是可以看到这个节点源码. 源码中主函数摘录如下:
int main(int argc, char **argv) { ... ... // 省略了部分代码 topicColor = "/" + ns + topicColor; topicDepth = "/" + ns + topicDepth; OUT_INFO("topic color: " FG_CYAN << topicColor << NO_COLOR); OUT_INFO("topic depth: " FG_CYAN << topicDepth << NO_COLOR); // Receiver是一个类, 也定义在该文件中.useExact(true), useCompressed(false) Receiver receiver(topicColor, topicDepth, useExact, useCompressed); OUT_INFO("starting receiver..."); // 运行时给出参数"cloud", 则mode = Receiver::CLOUD // 运行时给出参数"image", 则mode = Receiver::IMAGE // 运行时给出参数"both", 则mode = Receiver::BOTH receiver.run(mode); ros::shutdown(); return 0; }
Receiver类的实现也不算太复杂. 其中用于显示的主循环在imageViewer()或cloudViewer()中. 依据给的参数的不同, 将会调用不同的函数. 对应关系如下所示:
switch(mode) { case CLOUD: cloudViewer(); break; case IMAGE: imageViewer(); break; case BOTH: imageViewerThread = std::thread(&Receiver::imageViewer, this); cloudViewer(); break; }
BOTH选项, 将会出现两个窗口来显示图像. 上面两个函数都已经实现了图片保存的功能.代码摘录如下, 两个函数实现类似, 故只是摘录了imageViewer()的内容:
int key = cv::waitKey(1); switch(key & 0xFF) { case 27: case 'q': running = false; break; case ' ': case 's': if(mode == IMAGE) { createCloud(depth, color, cloud); saveCloudAndImages(cloud, color, depth, depthDisp); } else { save = true; } break; }
其中关键函数saveCloudAndImages()的实现如下:
void saveCloudAndImages( const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud, const cv::Mat &color, const cv::Mat &depth, const cv::Mat &depthColored) { oss.str(""); oss << "./" << std::setfill('0') << std::setw(4) << frame; // 所有文件都保存在当前路径下 const std::string baseName = oss.str(); const std::string cloudName = baseName + "_cloud.pcd"; const std::string colorName = baseName + "_color.jpg"; const std::string depthName = baseName + "_depth.png"; const std::string depthColoredName = baseName + "_depth_colored.png"; OUT_INFO("saving cloud: " << cloudName); // writer是该类的pcl::PCDWriter类型的成员变量 writer.writeBinary(cloudName, *cloud); OUT_INFO("saving color: " << colorName); cv::imwrite(colorName, color, params); OUT_INFO("saving depth: " << depthName); cv::imwrite(depthName, depth, params); OUT_INFO("saving depth: " << depthColoredName); cv::imwrite(depthColoredName, depthColored, params); OUT_INFO("saving complete!"); ++frame; }
从上面代码中可以看出来, 如果想要保存图片, 只需要选中显示图片的窗口, 然后输入单击键盘s键或者空格键就OK, 保存的图片就在当前目录.
如果有一些特别的需求, 在上面源码的基础上来进行实现, 将会事半功倍. 下面就是一个小小的例子.
保存图片序列
如果想要保存一个图片序列, 仅仅控制开始结束, 例如, 按键B(Begin)开始保存, 按键E(End)结束保存.
完成这样一个功能, 在源码的基础上进行适当更改就可以满足要求. 首选, 需要每一次对按键B和E的判断, 需要新增到上面摘录的switch(key & 0xFF)块中. 需要连续保存的话, 简单的设定一个标志位即可.
首先, 复制vewer.cpp文件, 命名为save_seq.cpp. 修改save_seq.cpp文件, 在Receiver类中bool save变量下面添加一个新的成员变量, bool save_seq. 在类的构造函数的初始化列表中新增该变量的初始化save_seq(false).
定位到void imageViewer()函数, 修改对应的switch(key & 0xFF)块如下:
int key = cv::waitKey(1); switch(key & 0xFF) { case 27: case 'q': running = false; break; case 'b': save_seq = true; save = true; break; case 'e': save_seq = false; save = false; break; case ' ': case 's': if (save_seq) break; if(mode == IMAGE) { createCloud(depth, color, cloud); saveCloudAndImages(cloud, color, depth, depthDisp); } else { save = true; } break; } if (save_seq) { createCloud(depth, color, cloud); saveCloudAndImages(cloud, color, depth, depthDisp); }
定位到void cloudViewer()函数, 修改对应的if (save)块如下:
if(save || save_seq) { save = false; cv::Mat depthDisp; dispDepth(depth, depthDisp, 12000.0f); saveCloudAndImages(cloud, color, depth, depthDisp); }
定位到void keyboardEvent(const pcl::visualization::KeyboardEvent &event, void *)函数, 修改源码如下:
if(event.keyUp()) { switch(event.getKeyCode()) { case 27: case 'q': running = false; break; case ' ': case 's': save = true; break; case 'b': save_seq = true; break; case 'e': save_seq = false; break; } }
在CMakeList.txt的最后, 添加如下指令:
add_executable(save_seq src/save_seq.cpp) target_link_libraries(save_seq ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ${kinect2_bridge_LIBRARIES} )
返回到catkin主目录, 运行catkin_make, 编译, 链接没有问题的话, 就可以查看效果了. 当然了, 首先是需要启动kinect_bride的. 依次运行下述指令:
roslaunch kinect2_bridge kinect2_bridge.launch rosrun kinect2_viewer save_seq hd cloud
选中弹出的窗口, 按键B 开始, 按键E结束保存. Terminal输出结果如下:
点击点云图获取坐标
主要想法是, 鼠标在点云图中移动时, 实时显示当前鼠标所指的点的三维坐标, 点击鼠标时, 获取该坐标发送出去.
这样的话, 首先就需要对鼠标有一个回调函数, 当鼠标状态改变时, 做出对应的变化. 鼠标变化可以简化为三种情况:
----鼠标移动
----鼠标左键点击
----鼠标右键点击
因为涉及到回调函数, 而且也只是一个小功能, 代码实现很简单. 直接使用了三个全局变量代表这三个状态(代码需要支持C++11, 不想那么麻烦的话, 可以将类型更改为volatile int), 以及一个全局的普通函数:
std::atomic_int mouseX; std::atomic_int mouseY; std::atomic_int mouseBtnType; void onMouse(int event, int x, int y, int flags, void* ustc) { // std::cout << "onMouse: " << x << " " << y << std::endl; mouseX = x; mouseY = y; mouseBtnType = event; }
在初始化中添加两个ros::Publisher, 分别对应鼠标左键和右键压下应该发布数据到达的话题. 如下所示:
ros::Publisher leftBtnPointPub = nh.advertise<geometry_msgs::PointStamped>("/kinect2/click_point/left", 1); ros::Publisher rightBtnPointPub = nh.advertise<geometry_msgs::PointStamped>("/kinect2/click_point/right", 1);
在初始化中添加两个ros::Publisher, 分别对应鼠标左键和右键压下应该发布数据到达的话题. 如下所示:
ros::Publisher leftBtnPointPub = nh.advertise<geometry_msgs::PointStamped>("/kinect2/click_point/left", 1); ros::Publisher rightBtnPointPub = nh.advertise<geometry_msgs::PointStamped>("/kinect2/click_point/right", 1);
其中消息格式是geometry_msgs/PointStamped, ROS自带的消息, 在源码头部需要包含头文件, #include <geometry_msgs/PointStamped.h>, 具体定义如下:
std_msgs/Header header uint32 seq time stamp string frame_id geometry_msgs/Point point float64 x float64 y float64 z
然后再重写cloudViewer()函数如下:
void cloudViewer() { cv::Mat color, depth; std::chrono::time_point<std::chrono::high_resolution_clock> start, now; double fps = 0; size_t frameCount = 0; std::ostringstream oss; std::ostringstream ossXYZ; // 新增一个string流 const cv::Point pos(5, 15); const cv::Scalar colorText = CV_RGB(255, 0, 0); const double sizeText = 0.5; const int lineText = 1; const int font = cv::FONT_HERSHEY_SIMPLEX; // 从全局变量获取当前鼠标坐标 int img_x = mouseX; int img_y = mouseY; geometry_msgs::PointStamped ptMsg; ptMsg.header.frame_id = "kinect_link"; lock.lock(); color = this->color; depth = this->depth; updateCloud = false; lock.unlock(); const std::string window_name = "color viewer"; cv::namedWindow(window_name); // 注册鼠标回调函数, 第三个参数是C++11中的关键字, 若不支持C++11, 替换成NULL cv::setMouseCallback(window_name, onMouse, nullptr); createCloud(depth, color, cloud); for(; running && ros::ok();) { if(updateCloud) { lock.lock(); color = this->color; depth = this->depth; updateCloud = false; lock.unlock(); createCloud(depth, color, cloud); img_x = mouseX; img_y = mouseY; const pcl::PointXYZRGBA& pt = cloud->points[img_y * depth.cols + img_x]; ptMsg.point.x = pt.x; ptMsg.point.y = pt.y; ptMsg.point.z = pt.z; // 根据鼠标左键压下或右键压下, 分别发布三维坐标到不同的话题上去 switch (mouseBtnType) { case cv::EVENT_LBUTTONUP: ptMsg.header.stamp = ros::Time::now(); leftBtnPointPub.publish(ptMsg); ros::spinOnce(); break; case cv::EVENT_RBUTTONUP: ptMsg.header.stamp = ros::Time::now(); rightBtnPointPub.publish(ptMsg); ros::spinOnce(); break; default: break; } mouseBtnType = cv::EVENT_MOUSEMOVE; ++frameCount; now = std::chrono::high_resolution_clock::now(); double elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() / 1000.0; if(elapsed >= 1.0) { fps = frameCount / elapsed; oss.str(""); oss << "fps: " << fps << " ( " << elapsed / frameCount * 1000.0 << " ms)"; start = now; frameCount = 0; } cv::putText(color, oss.str(), pos, font, sizeText, colorText, lineText, CV_AA); ossXYZ.str(""); ossXYZ << "( " << ptMsg.point.x << ", " << ptMsg.point.y << ", " << ptMsg.point.z << " )"; cv::putText(color, ossXYZ.str(), cv::Point(img_x, img_y), font, 1, colorText, 3, CV_AA); // cv::circle(color, cv::Point(mouseX, mouseY), 5, cv::Scalar(0, 0, 255), -1); cv::imshow(window_name, color); // cv::imshow(window_name, depth); cv::waitKey(1); } } cv::destroyAllWindows(); cv::waitKey(100); }
最后, 稍微改写一下主函数就OK了, 整个主函数摘录如下, 其中去掉了多余的参数解析, 让使用更加固定, 简单.
int main(int argc, char **argv) { #if EXTENDED_OUTPUT ROSCONSOLE_AUTOINIT; if(!getenv("ROSCONSOLE_FORMAT")) { ros::console::g_formatter.tokens_.clear(); ros::console::g_formatter.init("[${severity}] ${message}"); } #endif ros::init(argc, argv, "kinect2_viewer", ros::init_options::AnonymousName); if(!ros::ok()) { return 0; } std::string ns = K2_DEFAULT_NS; std::string topicColor = K2_TOPIC_HD K2_TOPIC_IMAGE_COLOR K2_TOPIC_IMAGE_RECT; std::string topicDepth = K2_TOPIC_HD K2_TOPIC_IMAGE_DEPTH K2_TOPIC_IMAGE_RECT; bool useExact = true; bool useCompressed = false; Receiver::Mode mode = Receiver::CLOUD; topicColor = "/" + ns + topicColor; topicDepth = "/" + ns + topicDepth; OUT_INFO("topic color: " FG_CYAN << topicColor << NO_COLOR); OUT_INFO("topic depth: " FG_CYAN << topicDepth << NO_COLOR); Receiver receiver(topicColor, topicDepth, useExact, useCompressed); OUT_INFO("starting receiver..."); OUT_INFO("Please click mouse in color viewer..."); receiver.run(mode); ros::shutdown(); return 0; }
在CMakeList.txt中加入下面两段话, 然后make就OK.
add_executable(click_rgb src/click_rgb.cpp) target_link_libraries(click_rgb ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ${kinect2_bridge_LIBRARIES} ) install(TARGETS click_rgb RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )
运行的话, 输入rosrun kinect2_viewer click_rgb就OK. 效果如下图所示, 图中红色坐标位置, 实际是鼠标所在位置, 截图时, 鼠标截不下来.
基于深度相机D415的彩色与深度图像匹配
环境配置可以参考考另一篇博文
- 最近在用D435做物体识别抓取的时候发现二维坐标到三维坐标没有一个直接能用的从二维像素点坐标转换到三维坐标的ROS节点，自己之前写过的kinect V2的坐标映射的通用性太差，所以这次写了一个节点，利用深度相机ROS节点发布的深度与彩色对齐的图像话题和图像信息内外参话题，将二维坐标映射到三维坐标系。
- 我挂上来的这个节点是将鼠标指向的二维坐标转换到三维坐标系下，并发布话题可视化到了rviz中，因为我自己的物体识别发布的像素坐标话题是一个自己写的消息文件，这次就不放出来了，需要用的把我这里的鼠标反馈的坐标改成你自己的坐标就行了。
- 原理：深度相机会发布一个裁剪对齐好的深度图像或者彩色图像话题，以D415为例，当我们输入：roslaunch realsense2_camera rs_camera.launch后就可以启动程序。并看到它的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 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="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>
