simple_double.cpp
Go to the documentation of this file.
00001 /*****************************
00002 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
00003 
00004 Redistribution and use in source and binary forms, with or without modification, are
00005 permitted provided that the following conditions are met:
00006 
00007    1. Redistributions of source code must retain the above copyright notice, this list of
00008       conditions and the following disclaimer.
00009 
00010    2. Redistributions in binary form must reproduce the above copyright notice, this list
00011       of conditions and the following disclaimer in the documentation and/or other materials
00012       provided with the distribution.
00013 
00014 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
00015 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00016 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
00017 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00018 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00019 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
00020 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00021 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00022 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00023 
00024 The views and conclusions contained in the software and documentation are those of the
00025 authors and should not be interpreted as representing official policies, either expressed
00026 or implied, of Rafael Muñoz Salinas.
00027 ********************************/
00028 
00037 #include <iostream>
00038 #include <aruco/aruco.h>
00039 #include <aruco/cvdrawingutils.h>
00040 
00041 #include <opencv2/core/core.hpp>
00042 #include <ros/ros.h>
00043 #include <image_transport/image_transport.h>
00044 #include <cv_bridge/cv_bridge.h>
00045 #include <sensor_msgs/image_encodings.h>
00046 #include <aruco_ros/aruco_ros_utils.h>
00047 #include <tf/transform_broadcaster.h>
00048 #include <tf/transform_datatypes.h>
00049 
00050 #include <dynamic_reconfigure/server.h>
00051 #include <aruco_ros/ArucoThresholdConfig.h>
00052 
00053 using namespace cv;
00054 using namespace aruco;
00055 
00056 cv::Mat inImage;
00057 aruco::CameraParameters camParam;
00058 bool useRectifiedImages, normalizeImageIllumination;
00059 int dctComponentsToRemove;
00060 MarkerDetector mDetector;
00061 vector<Marker> markers;
00062 ros::Subscriber cam_info_sub;
00063 bool cam_info_received;
00064 image_transport::Publisher image_pub;
00065 image_transport::Publisher debug_pub;
00066 ros::Publisher pose_pub1;
00067 ros::Publisher pose_pub2;
00068 std::string child_name1;
00069 std::string parent_name;
00070 std::string child_name2;
00071 
00072 double marker_size;
00073 int marker_id1;
00074 int marker_id2;
00075 
00076 void image_callback(const sensor_msgs::ImageConstPtr& msg)
00077 {
00078   double ticksBefore = cv::getTickCount();
00079   static tf::TransformBroadcaster br;
00080   if(cam_info_received)
00081   {
00082     ros::Time curr_stamp(ros::Time::now());
00083     cv_bridge::CvImagePtr cv_ptr;
00084     try
00085     {
00086       cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
00087       inImage = cv_ptr->image;
00088 
00089       if(normalizeImageIllumination)
00090       {
00091         ROS_WARN("normalizeImageIllumination is unimplemented!");
00092         //cv::Mat inImageNorm;
00093         //pal_vision_util::dctNormalization(inImage, inImageNorm, dctComponentsToRemove);
00094         //inImage = inImageNorm;
00095       }
00096 
00097       //detection results will go into "markers"
00098       markers.clear();
00099       //Ok, let's detect
00100       mDetector.detect(inImage, markers, camParam, marker_size);
00101       //for each marker, draw info and its boundaries in the image
00102       for(unsigned int i=0; i<markers.size(); ++i)
00103       {
00104         // only publishing the selected marker
00105         if ( markers[i].id == marker_id1 )
00106         {
00107           tf::Transform transform = aruco_ros::arucoMarker2Tf(markers[i]);
00108           br.sendTransform(tf::StampedTransform(transform, curr_stamp,
00109                                                 parent_name, child_name1));
00110           geometry_msgs::Pose poseMsg;
00111           tf::poseTFToMsg(transform, poseMsg);
00112           pose_pub1.publish(poseMsg);
00113         }
00114         else if ( markers[i].id == marker_id2 )
00115         {
00116           tf::Transform transform = aruco_ros::arucoMarker2Tf(markers[i]);
00117           br.sendTransform(tf::StampedTransform(transform, curr_stamp,
00118                                                 parent_name, child_name2));
00119           geometry_msgs::Pose poseMsg;
00120           tf::poseTFToMsg(transform, poseMsg);
00121           pose_pub2.publish(poseMsg);
00122         }
00123 
00124         // but drawing all the detected markers
00125         markers[i].draw(inImage,Scalar(0,0,255),2);
00126       }
00127 
00128       //paint a circle in the center of the image
00129       cv::circle(inImage, cv::Point(inImage.cols/2, inImage.rows/2), 4, cv::Scalar(0,255,0), 1);
00130 
00131       if ( markers.size() == 2 )
00132       {
00133         float x[2], y[2], u[2], v[2];
00134         for (unsigned int i = 0; i < 2; ++i)
00135         {
00136           ROS_DEBUG_STREAM("Marker(" << i << ") at camera coordinates = ("
00137                            << markers[i].Tvec.at<float>(0,0) << ", "
00138                            << markers[i].Tvec.at<float>(1,0) << ", "
00139                            << markers[i].Tvec.at<float>(2,0));
00140           //normalized coordinates of the marker
00141           x[i] = markers[i].Tvec.at<float>(0,0)/markers[i].Tvec.at<float>(2,0);
00142           y[i] = markers[i].Tvec.at<float>(1,0)/markers[i].Tvec.at<float>(2,0);
00143           //undistorted pixel
00144           u[i] = x[i]*camParam.CameraMatrix.at<float>(0,0) +
00145               camParam.CameraMatrix.at<float>(0,2);
00146           v[i] = y[i]*camParam.CameraMatrix.at<float>(1,1) +
00147               camParam.CameraMatrix.at<float>(1,2);
00148         }
00149 
00150         ROS_DEBUG_STREAM("Mid point between the two markers in the image = ("
00151                          << (x[0]+x[1])/2 << ", " << (y[0]+y[1])/2 << ")");
00152 
00153         //              //paint a circle in the mid point of the normalized coordinates of both markers
00154         //              cv::circle(inImage,
00155         //                         cv::Point((u[0]+u[1])/2, (v[0]+v[1])/2),
00156         //                         3, cv::Scalar(0,0,255), CV_FILLED);
00157 
00158 
00159         //compute the midpoint in 3D:
00160         float midPoint3D[3]; //3D point
00161         for (unsigned int i = 0; i < 3; ++i )
00162           midPoint3D[i] = ( markers[0].Tvec.at<float>(i,0) +
00163                             markers[1].Tvec.at<float>(i,0) ) / 2;
00164         //now project the 3D mid point to normalized coordinates
00165         float midPointNormalized[2];
00166         midPointNormalized[0] = midPoint3D[0]/midPoint3D[2]; //x
00167         midPointNormalized[1] = midPoint3D[1]/midPoint3D[2]; //y
00168         u[0] = midPointNormalized[0]*camParam.CameraMatrix.at<float>(0,0) +
00169             camParam.CameraMatrix.at<float>(0,2);
00170         v[0] = midPointNormalized[1]*camParam.CameraMatrix.at<float>(1,1) +
00171             camParam.CameraMatrix.at<float>(1,2);
00172 
00173         ROS_DEBUG_STREAM("3D Mid point between the two markers in undistorted pixel coordinates = ("
00174                          << u[0] << ", " << v[0] << ")");
00175 
00176         //paint a circle in the mid point of the normalized coordinates of both markers
00177         cv::circle(inImage,
00178                    cv::Point(u[0], v[0]),
00179                    3, cv::Scalar(0,0,255), CV_FILLED);
00180 
00181       }
00182 
00183       //draw a 3d cube in each marker if there is 3d info
00184       if(camParam.isValid() && marker_size!=-1)
00185       {
00186         for(unsigned int i=0; i<markers.size(); ++i)
00187         {
00188           CvDrawingUtils::draw3dCube(inImage, markers[i], camParam);
00189         }
00190       }
00191 
00192       if(image_pub.getNumSubscribers() > 0)
00193       {
00194         //show input with augmented information
00195         cv_bridge::CvImage out_msg;
00196         out_msg.header.stamp = curr_stamp;
00197         out_msg.encoding = sensor_msgs::image_encodings::RGB8;
00198         out_msg.image = inImage;
00199         image_pub.publish(out_msg.toImageMsg());
00200       }
00201 
00202       if(debug_pub.getNumSubscribers() > 0)
00203       {
00204         //show also the internal image resulting from the threshold operation
00205         cv_bridge::CvImage debug_msg;
00206         debug_msg.header.stamp = curr_stamp;
00207         debug_msg.encoding = sensor_msgs::image_encodings::MONO8;
00208         debug_msg.image = mDetector.getThresholdedImage();
00209         debug_pub.publish(debug_msg.toImageMsg());
00210       }
00211 
00212       ROS_DEBUG("runtime: %f ms",
00213                 1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
00214     }
00215     catch (cv_bridge::Exception& e)
00216     {
00217       ROS_ERROR("cv_bridge exception: %s", e.what());
00218       return;
00219     }
00220   }
00221 }
00222 
00223 // wait for one camerainfo, then shut down that subscriber
00224 void cam_info_callback(const sensor_msgs::CameraInfo &msg)
00225 {
00226   camParam = aruco_ros::rosCameraInfo2ArucoCamParams(msg, useRectifiedImages);
00227   cam_info_received = true;
00228   cam_info_sub.shutdown();
00229 }
00230 
00231 void reconf_callback(aruco_ros::ArucoThresholdConfig &config, uint32_t level)
00232 {
00233   mDetector.setThresholdParams(config.param1,config.param2);
00234   normalizeImageIllumination = config.normalizeImage;
00235   dctComponentsToRemove      = config.dctComponentsToRemove;
00236 }
00237 
00238 int main(int argc,char **argv)
00239 {
00240   ros::init(argc, argv, "aruco_simple");
00241   ros::NodeHandle nh("~");
00242   image_transport::ImageTransport it(nh);
00243 
00244   dynamic_reconfigure::Server<aruco_ros::ArucoThresholdConfig> server;
00245   dynamic_reconfigure::Server<aruco_ros::ArucoThresholdConfig>::CallbackType f_;
00246   f_ = boost::bind(&reconf_callback, _1, _2);
00247   server.setCallback(f_);
00248 
00249   normalizeImageIllumination = false;
00250 
00251   nh.param<bool>("image_is_rectified", useRectifiedImages, true);
00252   ROS_INFO_STREAM("Image is rectified: " << useRectifiedImages);
00253 
00254   image_transport::Subscriber image_sub = it.subscribe("/image", 1, &image_callback);
00255   cam_info_sub = nh.subscribe("/camera_info", 1, &cam_info_callback);
00256 
00257   cam_info_received = false;
00258   image_pub = it.advertise("result", 1);
00259   debug_pub = it.advertise("debug", 1);
00260   pose_pub1 = nh.advertise<geometry_msgs::Pose>("pose", 100);
00261   pose_pub2 = nh.advertise<geometry_msgs::Pose>("pose2", 100);
00262 
00263   nh.param<double>("marker_size", marker_size, 0.05);
00264   nh.param<int>("marker_id1", marker_id1, 582);
00265   nh.param<int>("marker_id2", marker_id2, 26);
00266   nh.param<bool>("normalizeImage", normalizeImageIllumination, true);
00267   nh.param<int>("dct_components_to_remove", dctComponentsToRemove, 2);
00268   if(dctComponentsToRemove == 0)
00269     normalizeImageIllumination = false;
00270   nh.param<std::string>("parent_name", parent_name, "");
00271   nh.param<std::string>("child_name1", child_name1, "");
00272   nh.param<std::string>("child_name2", child_name2, "");
00273 
00274   if(parent_name == "" || child_name1 == "" || child_name2 == "")
00275   {
00276     ROS_ERROR("parent_name and/or child_name was not set!");
00277     return -1;
00278   }
00279 
00280   ROS_INFO("Aruco node started with marker size of %f meters and marker ids to track: %d, %d",
00281            marker_size, marker_id1, marker_id2);
00282   ROS_INFO("Aruco node will publish pose to TF with (%s, %s) and (%s, %s) as (parent,child).",
00283            parent_name.c_str(), child_name1.c_str(), parent_name.c_str(), child_name2.c_str());
00284 
00285   ros::spin();
00286 }


aruco_ros
Author(s): Rafael Muñoz Salinas , Bence Magyar
autogenerated on Thu Aug 27 2015 12:26:22