aruco_ros/src/simple_double.cpp
Go to the documentation of this file.
1 /*****************************
2 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without modification, are
5 permitted provided that the following conditions are met:
6 
7  1. Redistributions of source code must retain the above copyright notice, this list of
8  conditions and the following disclaimer.
9 
10  2. Redistributions in binary form must reproduce the above copyright notice, this list
11  of conditions and the following disclaimer in the documentation and/or other materials
12  provided with the distribution.
13 
14 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
15 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
16 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
17 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
18 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
19 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
20 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
21 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
22 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 
24 The views and conclusions contained in the software and documentation are those of the
25 authors and should not be interpreted as representing official policies, either expressed
26 or implied, of Rafael Muñoz Salinas.
27 ********************************/
28 
37 #include <iostream>
38 #include <aruco/aruco.h>
39 #include <aruco/cvdrawingutils.h>
40 
41 #include <ros/ros.h>
43 #include <cv_bridge/cv_bridge.h>
45 #include <aruco_ros/aruco_ros_utils.h>
47 #include <tf/transform_datatypes.h>
48 
49 #include <dynamic_reconfigure/server.h>
50 #include <aruco_ros/ArucoThresholdConfig.h>
51 
52 using namespace cv;
53 using namespace aruco;
54 
55 cv::Mat inImage;
60 vector<Marker> markers;
67 std::string child_name1;
68 std::string parent_name;
69 std::string child_name2;
70 
71 double marker_size;
74 
75 void image_callback(const sensor_msgs::ImageConstPtr& msg)
76 {
77  double ticksBefore = cv::getTickCount();
78  static tf::TransformBroadcaster br;
80  {
81  ros::Time curr_stamp(ros::Time::now());
82  cv_bridge::CvImagePtr cv_ptr;
83  try
84  {
86  inImage = cv_ptr->image;
87 
89  {
90  ROS_WARN("normalizeImageIllumination is unimplemented!");
91  //cv::Mat inImageNorm;
92  //pal_vision_util::dctNormalization(inImage, inImageNorm, dctComponentsToRemove);
93  //inImage = inImageNorm;
94  }
95 
96  //detection results will go into "markers"
97  markers.clear();
98  //Ok, let's detect
99  mDetector.detect(inImage, markers, camParam, marker_size);
100  //for each marker, draw info and its boundaries in the image
101  for(unsigned int i=0; i<markers.size(); ++i)
102  {
103  // only publishing the selected marker
104  if ( markers[i].id == marker_id1 )
105  {
107  br.sendTransform(tf::StampedTransform(transform, curr_stamp,
109  geometry_msgs::Pose poseMsg;
110  tf::poseTFToMsg(transform, poseMsg);
111  pose_pub1.publish(poseMsg);
112  }
113  else if ( markers[i].id == marker_id2 )
114  {
116  br.sendTransform(tf::StampedTransform(transform, curr_stamp,
118  geometry_msgs::Pose poseMsg;
119  tf::poseTFToMsg(transform, poseMsg);
120  pose_pub2.publish(poseMsg);
121  }
122 
123  // but drawing all the detected markers
124  markers[i].draw(inImage,Scalar(0,0,255),2);
125  }
126 
127  //paint a circle in the center of the image
128  cv::circle(inImage, cv::Point(inImage.cols/2, inImage.rows/2), 4, cv::Scalar(0,255,0), 1);
129 
130  if ( markers.size() == 2 )
131  {
132  float x[2], y[2], u[2], v[2];
133  for (unsigned int i = 0; i < 2; ++i)
134  {
135  ROS_DEBUG_STREAM("Marker(" << i << ") at camera coordinates = ("
136  << markers[i].Tvec.at<float>(0,0) << ", "
137  << markers[i].Tvec.at<float>(1,0) << ", "
138  << markers[i].Tvec.at<float>(2,0));
139  //normalized coordinates of the marker
140  x[i] = markers[i].Tvec.at<float>(0,0)/markers[i].Tvec.at<float>(2,0);
141  y[i] = markers[i].Tvec.at<float>(1,0)/markers[i].Tvec.at<float>(2,0);
142  //undistorted pixel
143  u[i] = x[i]*camParam.CameraMatrix.at<float>(0,0) +
144  camParam.CameraMatrix.at<float>(0,2);
145  v[i] = y[i]*camParam.CameraMatrix.at<float>(1,1) +
146  camParam.CameraMatrix.at<float>(1,2);
147  }
148 
149  ROS_DEBUG_STREAM("Mid point between the two markers in the image = ("
150  << (x[0]+x[1])/2 << ", " << (y[0]+y[1])/2 << ")");
151 
152  // //paint a circle in the mid point of the normalized coordinates of both markers
153  // cv::circle(inImage,
154  // cv::Point((u[0]+u[1])/2, (v[0]+v[1])/2),
155  // 3, cv::Scalar(0,0,255), cv::FILLED);
156 
157 
158  //compute the midpoint in 3D:
159  float midPoint3D[3]; //3D point
160  for (unsigned int i = 0; i < 3; ++i )
161  midPoint3D[i] = ( markers[0].Tvec.at<float>(i,0) +
162  markers[1].Tvec.at<float>(i,0) ) / 2;
163  //now project the 3D mid point to normalized coordinates
164  float midPointNormalized[2];
165  midPointNormalized[0] = midPoint3D[0]/midPoint3D[2]; //x
166  midPointNormalized[1] = midPoint3D[1]/midPoint3D[2]; //y
167  u[0] = midPointNormalized[0]*camParam.CameraMatrix.at<float>(0,0) +
168  camParam.CameraMatrix.at<float>(0,2);
169  v[0] = midPointNormalized[1]*camParam.CameraMatrix.at<float>(1,1) +
170  camParam.CameraMatrix.at<float>(1,2);
171 
172  ROS_DEBUG_STREAM("3D Mid point between the two markers in undistorted pixel coordinates = ("
173  << u[0] << ", " << v[0] << ")");
174 
175  //paint a circle in the mid point of the normalized coordinates of both markers
176  cv::circle(inImage,
177  cv::Point(u[0], v[0]),
178  3, cv::Scalar(0,0,255), cv::FILLED);
179 
180  }
181 
182  //draw a 3d cube in each marker if there is 3d info
183  if(camParam.isValid() && marker_size>0)
184  {
185  for(unsigned int i=0; i<markers.size(); ++i)
186  {
187  CvDrawingUtils::draw3dCube(inImage, markers[i], camParam);
188  }
189  }
190 
191  if(image_pub.getNumSubscribers() > 0)
192  {
193  //show input with augmented information
194  cv_bridge::CvImage out_msg;
195  out_msg.header.stamp = curr_stamp;
197  out_msg.image = inImage;
198  image_pub.publish(out_msg.toImageMsg());
199  }
200 
201  if(debug_pub.getNumSubscribers() > 0)
202  {
203  //show also the internal image resulting from the threshold operation
204  cv_bridge::CvImage debug_msg;
205  debug_msg.header.stamp = curr_stamp;
207  debug_msg.image = mDetector.getThresholdedImage();
208  debug_pub.publish(debug_msg.toImageMsg());
209  }
210 
211  ROS_DEBUG("runtime: %f ms",
212  1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
213  }
214  catch (cv_bridge::Exception& e)
215  {
216  ROS_ERROR("cv_bridge exception: %s", e.what());
217  return;
218  }
219  }
220 }
221 
222 // wait for one camerainfo, then shut down that subscriber
223 void cam_info_callback(const sensor_msgs::CameraInfo &msg)
224 {
226  cam_info_received = true;
227  cam_info_sub.shutdown();
228 }
229 
230 void reconf_callback(aruco_ros::ArucoThresholdConfig &config, uint32_t level)
231 {
232  mDetector.setThresholdParams(config.param1,config.param2);
233  normalizeImageIllumination = config.normalizeImage;
234  dctComponentsToRemove = config.dctComponentsToRemove;
235 }
236 
237 int main(int argc,char **argv)
238 {
239  ros::init(argc, argv, "aruco_simple");
240  ros::NodeHandle nh("~");
242 
243  dynamic_reconfigure::Server<aruco_ros::ArucoThresholdConfig> server;
244  dynamic_reconfigure::Server<aruco_ros::ArucoThresholdConfig>::CallbackType f_;
245  f_ = boost::bind(&reconf_callback, _1, _2);
246  server.setCallback(f_);
247 
249 
250  nh.param<bool>("image_is_rectified", useRectifiedImages, true);
251  ROS_INFO_STREAM("Image is rectified: " << useRectifiedImages);
252 
253  image_transport::Subscriber image_sub = it.subscribe("/image", 1, &image_callback);
254  cam_info_sub = nh.subscribe("/camera_info", 1, &cam_info_callback);
255 
256  cam_info_received = false;
257  image_pub = it.advertise("result", 1);
258  debug_pub = it.advertise("debug", 1);
259  pose_pub1 = nh.advertise<geometry_msgs::Pose>("pose", 100);
260  pose_pub2 = nh.advertise<geometry_msgs::Pose>("pose2", 100);
261 
262  nh.param<double>("marker_size", marker_size, 0.05);
263  nh.param<int>("marker_id1", marker_id1, 582);
264  nh.param<int>("marker_id2", marker_id2, 26);
265  nh.param<bool>("normalizeImage", normalizeImageIllumination, true);
266  nh.param<int>("dct_components_to_remove", dctComponentsToRemove, 2);
267  if(dctComponentsToRemove == 0)
268  normalizeImageIllumination = false;
269  nh.param<std::string>("parent_name", parent_name, "");
270  nh.param<std::string>("child_name1", child_name1, "");
271  nh.param<std::string>("child_name2", child_name2, "");
272 
273  if(parent_name == "" || child_name1 == "" || child_name2 == "")
274  {
275  ROS_ERROR("parent_name and/or child_name was not set!");
276  return -1;
277  }
278 
279  ROS_INFO("Aruco node started with marker size of %f meters and marker ids to track: %d, %d",
280  marker_size, marker_id1, marker_id2);
281  ROS_INFO("Aruco node will publish pose to TF with (%s, %s) and (%s, %s) as (parent,child).",
282  parent_name.c_str(), child_name1.c_str(), parent_name.c_str(), child_name2.c_str());
283 
284  ros::spin();
285 }
double marker_size
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
void detect(const cv::Mat &input, std::vector< Marker > &detectedMarkers, cv::Mat camMatrix=cv::Mat(), cv::Mat distCoeff=cv::Mat(), float markerSizeMeters=-1, bool setYPerpendicular=true)
void setThresholdParams(double param1, double param2)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void reconf_callback(aruco_ros::ArucoThresholdConfig &config, uint32_t level)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::string encoding
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
void image_callback(const sensor_msgs::ImageConstPtr &msg)
std::string child_name1
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
uint32_t getNumSubscribers() const
ROSCPP_DECL void spin(Spinner &spinner)
pcl::PointCloud< myPointXYZRID > transform(pcl::PointCloud< myPointXYZRID > pc, float x, float y, float z, float rot_x, float rot_y, float rot_z)
ros::Subscriber cam_info_sub
int dctComponentsToRemove
tf::Transform arucoMarker2Tf(const aruco::Marker &marker, bool rotate_marker_axis=true)
MarkerDetector mDetector
void publish(const sensor_msgs::Image &message) const
ros::Publisher pose_pub2
std::string parent_name
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
image_transport::Publisher image_pub
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void sendTransform(const StampedTransform &transform)
bool normalizeImageIllumination
image_transport::Publisher debug_pub
bool useRectifiedImages
TFSIMD_FORCE_INLINE const tfScalar & x() const
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Main class for marker detection.
aruco::CameraParameters rosCameraInfo2ArucoCamParams(const sensor_msgs::CameraInfo &cam_info, bool useRectifiedParameters)
rosCameraInfo2ArucoCamParams gets the camera intrinsics from a CameraInfo message and copies them to ...
#define ROS_DEBUG_STREAM(args)
vector< Marker > markers
void cam_info_callback(const sensor_msgs::CameraInfo &msg)
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
#define ROS_INFO_STREAM(args)
std::string child_name2
config
static Time now()
bool cam_info_received
ros::Publisher pose_pub1
#define ROS_ERROR(...)
sensor_msgs::ImagePtr toImageMsg() const
aruco::CameraParameters camParam
std_msgs::Header header
#define ROS_DEBUG(...)


lidar_camera_calibration
Author(s):
autogenerated on Sat Feb 6 2021 03:39:37