multi_boards.cpp
Go to the documentation of this file.
00001 
00009 #include <iostream>
00010 #include <fstream>
00011 #include <sstream>
00012 #include <aruco/aruco.h>
00013 #include <aruco/boarddetector.h>
00014 #include <aruco/cvdrawingutils.h>
00015 
00016 #include <opencv2/core/core.hpp>
00017 #include <ros/ros.h>
00018 #include <image_transport/image_transport.h>
00019 #include <cv_bridge/cv_bridge.h>
00020 #include <sensor_msgs/image_encodings.h>
00021 #include <ar_sys/utils.h>
00022 #include <tf/transform_broadcaster.h>
00023 #include <tf/transform_listener.h>
00024 
00025 using namespace aruco;
00026 
00027 struct board_t
00028 {
00029         int uid;
00030         std::string name;
00031         BoardConfiguration config;
00032         double marker_size;
00033 };
00034 
00035 class ArSysMultiBoards
00036 {
00037         private:
00038                 cv::Mat inImage, resultImg;
00039                 aruco::CameraParameters camParam;
00040                 bool useRectifiedImages;
00041                 bool draw_markers;
00042                 bool draw_markers_cube;
00043                 bool draw_markers_axis;
00044                 MarkerDetector mDetector;
00045                 vector<Marker> markers;
00046                 BoardDetector the_board_detector;
00047                 ros::Subscriber cam_info_sub;
00048                 bool cam_info_received;
00049                 image_transport::Publisher image_pub;
00050                 image_transport::Publisher debug_pub;
00051                 ros::Publisher pose_pub;
00052                 ros::Publisher transform_pub; 
00053                 ros::Publisher position_pub;
00054                 std::string boards_config;
00055                 std::string boards_directory;
00056                 vector<board_t> boards;
00057 
00058                 ros::NodeHandle nh;
00059                 image_transport::ImageTransport it;
00060                 image_transport::Subscriber image_sub;
00061 
00062                 tf::TransformListener _tfListener;
00063 
00064         public:
00065                 ArSysMultiBoards()
00066                         : cam_info_received(false),
00067                         nh("~"),
00068                         it(nh)
00069                 {
00070                         image_sub = it.subscribe("/image", 1, &ArSysMultiBoards::image_callback, this);
00071                         cam_info_sub = nh.subscribe("/camera_info", 1, &ArSysMultiBoards::cam_info_callback, this);
00072 
00073                         image_pub = it.advertise("result", 1);
00074                         debug_pub = it.advertise("debug", 1);
00075                         pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose", 100);
00076                         transform_pub = nh.advertise<geometry_msgs::TransformStamped>("transform", 100);
00077                         position_pub = nh.advertise<geometry_msgs::Vector3Stamped>("position", 100);
00078 
00079                         nh.param<std::string>("boards_config", boards_config, "boardsConfiguration.yml");
00080                         nh.param<std::string>("boards_directory", boards_directory, "./data");
00081                         nh.param<bool>("image_is_rectified", useRectifiedImages, true);
00082                         nh.param<bool>("draw_markers", draw_markers, false);
00083                         nh.param<bool>("draw_markers_cube", draw_markers_cube, false);
00084                         nh.param<bool>("draw_markers_axis", draw_markers_axis, false);
00085 
00086                         readFromFile(boards_config.c_str());
00087                         ROS_INFO("ArSys node started with boards configuration: %s", boards_config.c_str());
00088                 }
00089 
00090                 void readFromFile ( string sfile ) throw ( cv::Exception )
00091                 {
00092                         try
00093                         {
00094                                 cv::FileStorage fs ( sfile,cv::FileStorage::READ );
00095                                 readFromFile ( fs );
00096                         }
00097                         catch (std::exception &ex)
00098                         {
00099                                 throw   cv::Exception ( 81818,"ArSysMultiBoards::readFromFile",ex.what()+string(" file=)")+sfile ,__FILE__,__LINE__ );
00100                         }
00101                 }
00102 
00103 
00104                 void readFromFile ( cv::FileStorage &fs ) throw ( cv::Exception )
00105                 {
00106                         //look for the ar_sys_boards
00107                         if (fs["ar_sys_boards"].name() != "ar_sys_boards")
00108                                 throw cv::Exception ( 81818,"ArSysMultiBoards::readFromFile","invalid file type" ,__FILE__,__LINE__ );
00109 
00110                         cv::FileNode FnBoards=fs["ar_sys_boards"];
00111                         for (cv::FileNodeIterator it = FnBoards.begin(); it != FnBoards.end(); ++it)
00112                         {
00113                                 board_t board;
00114 
00115                                 board.uid = boards.size();
00116                                 board.name = (std::string)(*it)["name"];
00117                                 board.marker_size = (double)(*it)["marker_size"];
00118 
00119                                 std::string path(boards_directory);
00120                                 path.append("/");
00121                                 path.append((std::string)(*it)["path"]);
00122                                 board.config.readFromFile(path);
00123 
00124                                 boards.push_back(board);
00125                         }
00126 
00127                         ROS_ASSERT(boards.size() > 0);
00128                 }
00129 
00130                 void image_callback(const sensor_msgs::ImageConstPtr& msg)
00131                 {
00132                         if(!cam_info_received) return;
00133 
00134                         cv_bridge::CvImagePtr cv_ptr;
00135                         try
00136                         {
00137                                 cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
00138                                 inImage = cv_ptr->image;
00139                                 resultImg = cv_ptr->image.clone();
00140 
00141                                 //detection results will go into "markers"
00142                                 markers.clear();
00143 
00144                                 //Ok, let's detect
00145                                 double min_size = boards[0].marker_size;
00146                                 for (int board_index = 1; board_index < boards.size(); board_index++)
00147                                         if (min_size > boards[board_index].marker_size) min_size = boards[board_index].marker_size;
00148                                 mDetector.detect(inImage, markers, camParam, min_size, false);
00149 
00150                                 for (int board_index = 0; board_index < boards.size(); board_index++)
00151                                 {
00152                                         Board board_detected;
00153 
00154                                         //Detection of the board
00155                                         float probDetect = the_board_detector.detect(markers, boards[board_index].config, board_detected, camParam, boards[board_index].marker_size);
00156                                         if (probDetect > 0.0)
00157                                         {
00158                                                 tf::Transform transform = ar_sys::getTf(board_detected.Rvec, board_detected.Tvec);
00159 
00160                                                 tf::StampedTransform stampedTransform(transform, msg->header.stamp, msg->header.frame_id, boards[board_index].name);
00161 
00162                                                 geometry_msgs::PoseStamped poseMsg;
00163                                                 tf::poseTFToMsg(transform, poseMsg.pose);
00164                                                 poseMsg.header.frame_id = msg->header.frame_id;
00165                                                 poseMsg.header.stamp = msg->header.stamp;
00166                                                 pose_pub.publish(poseMsg);
00167 
00168                                                 geometry_msgs::TransformStamped transformMsg;
00169                                                 tf::transformStampedTFToMsg(stampedTransform, transformMsg);
00170                                                 transform_pub.publish(transformMsg);
00171 
00172                                                 geometry_msgs::Vector3Stamped positionMsg;
00173                                                 positionMsg.header = transformMsg.header;
00174                                                 positionMsg.vector = transformMsg.transform.translation;
00175                                                 position_pub.publish(positionMsg);
00176 
00177                                                 if(camParam.isValid())
00178                                                 {
00179                                                         //draw board axis
00180                                                         CvDrawingUtils::draw3dAxis(resultImg, board_detected, camParam);
00181                                                 }
00182                                         }
00183                                 }
00184 
00185                                 //for each marker, draw info and its boundaries in the image
00186                                 for(size_t i=0; draw_markers && i < markers.size(); ++i)
00187                                 {
00188                                         markers[i].draw(resultImg,cv::Scalar(0,0,255),2);
00189                                 }
00190 
00191                                 if(camParam.isValid())
00192                                 {
00193                                         //draw a 3d cube in each marker if there is 3d info
00194                                         for(size_t i=0; i<markers.size(); ++i)
00195                                         {
00196                                                 if (draw_markers_cube) CvDrawingUtils::draw3dCube(resultImg, markers[i], camParam);
00197                                                 if (draw_markers_axis) CvDrawingUtils::draw3dAxis(resultImg, markers[i], camParam);
00198                                         }
00199                                 }
00200 
00201                                 if(image_pub.getNumSubscribers() > 0)
00202                                 {
00203                                         //show input with augmented information
00204                                         cv_bridge::CvImage out_msg;
00205                                         out_msg.header.frame_id = msg->header.frame_id;
00206                                         out_msg.header.stamp = msg->header.stamp;
00207                                         out_msg.encoding = sensor_msgs::image_encodings::RGB8;
00208                                         out_msg.image = resultImg;
00209                                         image_pub.publish(out_msg.toImageMsg());
00210                                 }
00211 
00212                                 if(debug_pub.getNumSubscribers() > 0)
00213                                 {
00214                                         //show also the internal image resulting from the threshold operation
00215                                         cv_bridge::CvImage debug_msg;
00216                                         debug_msg.header.frame_id = msg->header.frame_id;
00217                                         debug_msg.header.stamp = msg->header.stamp;
00218                                         debug_msg.encoding = sensor_msgs::image_encodings::MONO8;
00219                                         debug_msg.image = mDetector.getThresholdedImage();
00220                                         debug_pub.publish(debug_msg.toImageMsg());
00221                                 }
00222                         }
00223                         catch (cv_bridge::Exception& e)
00224                         {
00225                                 ROS_ERROR("cv_bridge exception: %s", e.what());
00226                                 return;
00227                         }
00228                 }
00229 
00230                 // wait for one camerainfo, then shut down that subscriber
00231                 void cam_info_callback(const sensor_msgs::CameraInfo &msg)
00232                 {
00233                         camParam = ar_sys::getCamParams(msg, useRectifiedImages);
00234                         cam_info_received = true;
00235                         cam_info_sub.shutdown();
00236                 }
00237 };
00238 
00239 
00240 int main(int argc,char **argv)
00241 {
00242         ros::init(argc, argv, "ar_multi_boards");
00243 
00244         ArSysMultiBoards node;
00245 
00246         ros::spin();
00247 }


ar_sys
Author(s): Hamdi Sahloul , Rafael Muñoz Salinas , Bence Magyar
autogenerated on Sun Oct 5 2014 22:12:49