IndividualMarkers.cpp
Go to the documentation of this file.
1 /*
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2012, Scott Niekum
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without
8  modification, are permitted provided that the following conditions
9  are met:
10 
11  * Redistributions of source code must retain the above copyright
12  notice, this list of conditions and the following disclaimer.
13  * Redistributions in binary form must reproduce the above
14  copyright notice, this list of conditions and the following
15  disclaimer in the documentation and/or other materials provided
16  with the distribution.
17  * Neither the name of the Willow Garage nor the names of its
18  contributors may be used to endorse or promote products derived
19  from this software without specific prior written permission.
20 
21  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  POSSIBILITY OF SUCH DAMAGE.
33 
34  author: Scott Niekum
35 */
36 
37 
40 #include "ar_track_alvar/Shared.h"
41 #include <cv_bridge/cv_bridge.h>
42 #include <ar_track_alvar_msgs/AlvarMarker.h>
43 #include <ar_track_alvar_msgs/AlvarMarkers.h>
44 #include <tf/transform_listener.h>
48 #include <dynamic_reconfigure/server.h>
49 #include <ar_track_alvar/ParamsConfig.h>
50 #include <Eigen/StdVector>
51 
52 namespace gm=geometry_msgs;
53 namespace ata=ar_track_alvar;
54 
55 typedef pcl::PointXYZRGB ARPoint;
56 typedef pcl::PointCloud<ARPoint> ARCloud;
57 
58 using namespace alvar;
59 using namespace std;
60 using boost::make_shared;
61 
62 bool init=true;
70 ar_track_alvar_msgs::AlvarMarkers arPoseMarkers_;
71 visualization_msgs::Marker rvizMarker_;
75 
76 bool enableSwitched = false;
77 bool enabled = true;
80 double marker_size;
83 std::string cam_image_topic;
84 std::string cam_info_topic;
85 std::string output_frame;
86 int marker_resolution = 5; // default marker resolution
87 int marker_margin = 2; // default marker margin
88 
89 
90 //Debugging utility function
91 void draw3dPoints(ARCloud::Ptr cloud, string frame, int color, int id, double rad)
92 {
93  visualization_msgs::Marker rvizMarker;
94 
95  rvizMarker.header.frame_id = frame;
96  rvizMarker.header.stamp = ros::Time::now();
97  rvizMarker.id = id;
98  rvizMarker.ns = "3dpts";
99 
100  rvizMarker.scale.x = rad;
101  rvizMarker.scale.y = rad;
102  rvizMarker.scale.z = rad;
103 
104  rvizMarker.type = visualization_msgs::Marker::SPHERE_LIST;
105  rvizMarker.action = visualization_msgs::Marker::ADD;
106 
107  if(color==1){
108  rvizMarker.color.r = 0.0f;
109  rvizMarker.color.g = 1.0f;
110  rvizMarker.color.b = 1.0f;
111  rvizMarker.color.a = 1.0;
112  }
113  if(color==2){
114  rvizMarker.color.r = 1.0f;
115  rvizMarker.color.g = 0.0f;
116  rvizMarker.color.b = 1.0f;
117  rvizMarker.color.a = 1.0;
118  }
119  if(color==3){
120  rvizMarker.color.r = 1.0f;
121  rvizMarker.color.g = 1.0f;
122  rvizMarker.color.b = 0.0f;
123  rvizMarker.color.a = 1.0;
124  }
125 
126  gm::Point p;
127  for(int i=0; i<cloud->points.size(); i++){
128  p.x = cloud->points[i].x;
129  p.y = cloud->points[i].y;
130  p.z = cloud->points[i].z;
131  rvizMarker.points.push_back(p);
132  }
133 
134  rvizMarker.lifetime = ros::Duration (1.0);
135  rvizMarkerPub2_.publish (rvizMarker);
136 }
137 
138 
139 void drawArrow(gm::Point start, tf::Matrix3x3 mat, string frame, int color, int id)
140 {
141  visualization_msgs::Marker rvizMarker;
142 
143  rvizMarker.header.frame_id = frame;
144  rvizMarker.header.stamp = ros::Time::now();
145  rvizMarker.id = id;
146  rvizMarker.ns = "arrow";
147 
148  rvizMarker.scale.x = 0.01;
149  rvizMarker.scale.y = 0.01;
150  rvizMarker.scale.z = 0.1;
151 
152  rvizMarker.type = visualization_msgs::Marker::ARROW;
153  rvizMarker.action = visualization_msgs::Marker::ADD;
154 
155  for(int i=0; i<3; i++){
156  rvizMarker.points.clear();
157  rvizMarker.points.push_back(start);
158  gm::Point end;
159  end.x = start.x + mat[0][i];
160  end.y = start.y + mat[1][i];
161  end.z = start.z + mat[2][i];
162  rvizMarker.points.push_back(end);
163  rvizMarker.id += 10*i;
164  rvizMarker.lifetime = ros::Duration (1.0);
165 
166  if(color==1){
167  rvizMarker.color.r = 1.0f;
168  rvizMarker.color.g = 0.0f;
169  rvizMarker.color.b = 0.0f;
170  rvizMarker.color.a = 1.0;
171  }
172  if(color==2){
173  rvizMarker.color.r = 0.0f;
174  rvizMarker.color.g = 1.0f;
175  rvizMarker.color.b = 0.0f;
176  rvizMarker.color.a = 1.0;
177  }
178  if(color==3){
179  rvizMarker.color.r = 0.0f;
180  rvizMarker.color.g = 0.0f;
181  rvizMarker.color.b = 1.0f;
182  rvizMarker.color.a = 1.0;
183  }
184  color += 1;
185 
186  rvizMarkerPub2_.publish (rvizMarker);
187  }
188 }
189 
190 
191 int PlaneFitPoseImprovement(int id, const ARCloud &corners_3D, ARCloud::Ptr selected_points, const ARCloud &cloud, Pose &p){
192 
193  ata::PlaneFitResult res = ata::fitPlane(selected_points);
194  gm::PoseStamped pose;
195  pose.header.stamp = pcl_conversions::fromPCL(cloud.header).stamp;
196  pose.header.frame_id = cloud.header.frame_id;
197  pose.pose.position = ata::centroid(*res.inliers);
198 
199  draw3dPoints(selected_points, cloud.header.frame_id, 1, id, 0.005);
200 
201  //Get 2 points that point forward in marker x direction
202  int i1,i2;
203  if(isnan(corners_3D[0].x) || isnan(corners_3D[0].y) || isnan(corners_3D[0].z) ||
204  isnan(corners_3D[3].x) || isnan(corners_3D[3].y) || isnan(corners_3D[3].z))
205  {
206  if(isnan(corners_3D[1].x) || isnan(corners_3D[1].y) || isnan(corners_3D[1].z) ||
207  isnan(corners_3D[2].x) || isnan(corners_3D[2].y) || isnan(corners_3D[2].z))
208  {
209  return -1;
210  }
211  else{
212  i1 = 1;
213  i2 = 2;
214  }
215  }
216  else{
217  i1 = 0;
218  i2 = 3;
219  }
220 
221  //Get 2 points the point forward in marker y direction
222  int i3,i4;
223  if(isnan(corners_3D[0].x) || isnan(corners_3D[0].y) || isnan(corners_3D[0].z) ||
224  isnan(corners_3D[1].x) || isnan(corners_3D[1].y) || isnan(corners_3D[1].z))
225  {
226  if(isnan(corners_3D[3].x) || isnan(corners_3D[3].y) || isnan(corners_3D[3].z) ||
227  isnan(corners_3D[2].x) || isnan(corners_3D[2].y) || isnan(corners_3D[2].z))
228  {
229  return -1;
230  }
231  else{
232  i3 = 2;
233  i4 = 3;
234  }
235  }
236  else{
237  i3 = 1;
238  i4 = 0;
239  }
240 
241  ARCloud::Ptr orient_points(new ARCloud());
242  orient_points->points.push_back(corners_3D[i1]);
243  draw3dPoints(orient_points, cloud.header.frame_id, 3, id+1000, 0.008);
244 
245  orient_points->clear();
246  orient_points->points.push_back(corners_3D[i2]);
247  draw3dPoints(orient_points, cloud.header.frame_id, 2, id+2000, 0.008);
248 
249  int succ;
250  succ = ata::extractOrientation(res.coeffs, corners_3D[i1], corners_3D[i2], corners_3D[i3], corners_3D[i4], pose.pose.orientation);
251  if(succ < 0) return -1;
252 
253  tf::Matrix3x3 mat;
254  succ = ata::extractFrame(res.coeffs, corners_3D[i1], corners_3D[i2], corners_3D[i3], corners_3D[i4], mat);
255  if(succ < 0) return -1;
256 
257  drawArrow(pose.pose.position, mat, cloud.header.frame_id, 1, id);
258 
259  p.translation[0] = pose.pose.position.x * 100.0;
260  p.translation[1] = pose.pose.position.y * 100.0;
261  p.translation[2] = pose.pose.position.z * 100.0;
262  p.quaternion[1] = pose.pose.orientation.x;
263  p.quaternion[2] = pose.pose.orientation.y;
264  p.quaternion[3] = pose.pose.orientation.z;
265  p.quaternion[0] = pose.pose.orientation.w;
266 
267  return 0;
268 }
269 
270 
271 void GetMarkerPoses(IplImage *image, ARCloud &cloud) {
272 
273  //Detect and track the markers
274  if (marker_detector.Detect(image, cam, true, false, max_new_marker_error,
275  max_track_error, CVSEQ, true))
276  {
277  ROS_DEBUG_STREAM("--------------------------");
278  for (size_t i=0; i<marker_detector.markers->size(); i++)
279  {
280  vector<cv::Point, Eigen::aligned_allocator<cv::Point> > pixels;
281  Marker *m = &((*marker_detector.markers)[i]);
282  int id = m->GetId();
283  ROS_DEBUG_STREAM("******* ID: " << id);
284 
285  int resol = m->GetRes();
286  int ori = m->ros_orientation;
287 
288  PointDouble pt1, pt2, pt3, pt4;
289  pt4 = m->ros_marker_points_img[0];
290  pt3 = m->ros_marker_points_img[resol-1];
291  pt1 = m->ros_marker_points_img[(resol*resol)-resol];
292  pt2 = m->ros_marker_points_img[(resol*resol)-1];
293 
294  m->ros_corners_3D[0] = cloud(pt1.x, pt1.y);
295  m->ros_corners_3D[1] = cloud(pt2.x, pt2.y);
296  m->ros_corners_3D[2] = cloud(pt3.x, pt3.y);
297  m->ros_corners_3D[3] = cloud(pt4.x, pt4.y);
298 
299  if(ori >= 0 && ori < 4){
300  if(ori != 0){
301  std::rotate(m->ros_corners_3D.begin(), m->ros_corners_3D.begin() + ori, m->ros_corners_3D.end());
302  }
303  }
304  else
305  ROS_ERROR("FindMarkerBundles: Bad Orientation: %i for ID: %i", ori, id);
306 
307  //Get the 3D marker points
308  BOOST_FOREACH (const PointDouble& p, m->ros_marker_points_img)
309  pixels.push_back(cv::Point(p.x, p.y));
310  ARCloud::Ptr selected_points = ata::filterCloud(cloud, pixels);
311 
312  //Use the kinect data to find a plane and pose for the marker
313  int ret = PlaneFitPoseImprovement(i, m->ros_corners_3D, selected_points, cloud, m->pose);
314  }
315  }
316 }
317 
318 
319 
320 void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
321 {
322  sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
323 
324  // If desired, use the frame in the message's header.
326  output_frame = msg->header.frame_id;
327 
328  //If we've already gotten the cam info, then go ahead
329  if(cam->getCamInfo_){
330  //Convert cloud to PCL
331  ARCloud cloud;
332  pcl::fromROSMsg(*msg, cloud);
333 
334  //Get an OpenCV image from the cloud
335  pcl::toROSMsg (*msg, *image_msg);
336  image_msg->header.stamp = msg->header.stamp;
337  image_msg->header.frame_id = msg->header.frame_id;
338 
339 
340  //Convert the image
342 
343  //Get the estimated pose of the main markers by using all the markers in each bundle
344 
345  // GetMultiMarkersPoses expects an IplImage*, but as of ros groovy, cv_bridge gives
346  // us a cv::Mat. I'm too lazy to change to cv::Mat throughout right now, so I
347  // do this conversion here -jbinney
348  IplImage ipl_image = cv_ptr_->image;
349 
350 
351  //Use the kinect to improve the pose
352  Pose ret_pose;
353  GetMarkerPoses(&ipl_image, cloud);
354 
355  tf::StampedTransform CamToOutput;
356  if (image_msg->header.frame_id == output_frame) {
357  CamToOutput.setIdentity();
358  } else {
359  try {
360  tf_listener->waitForTransform(output_frame, image_msg->header.frame_id,
361  image_msg->header.stamp, ros::Duration(1.0));
362  tf_listener->lookupTransform(output_frame, image_msg->header.frame_id,
363  image_msg->header.stamp, CamToOutput);
364  } catch (tf::TransformException ex) {
365  ROS_ERROR("%s",ex.what());
366  }
367  }
368 
369  try{
370 
371 
372  arPoseMarkers_.markers.clear ();
373  for (size_t i=0; i<marker_detector.markers->size(); i++)
374  {
375  //Get the pose relative to the camera
376  int id = (*(marker_detector.markers))[i].GetId();
377  Pose p = (*(marker_detector.markers))[i].pose;
378 
379  double px = p.translation[0]/100.0;
380  double py = p.translation[1]/100.0;
381  double pz = p.translation[2]/100.0;
382  double qx = p.quaternion[1];
383  double qy = p.quaternion[2];
384  double qz = p.quaternion[3];
385  double qw = p.quaternion[0];
386 
387  tf::Quaternion rotation (qx,qy,qz,qw);
388  tf::Vector3 origin (px,py,pz);
389  tf::Transform t (rotation, origin);
390  tf::Vector3 markerOrigin (0, 0, 0);
391  tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
392  tf::Transform markerPose = t * m; // marker pose in the camera frame
393 
394  //Publish the transform from the camera to the marker
395  std::string markerFrame = "ar_marker_";
396  std::stringstream out;
397  out << id;
398  std::string id_string = out.str();
399  markerFrame += id_string;
400  tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
401  tf_broadcaster->sendTransform(camToMarker);
402 
403  //Create the rviz visualization messages
404  tf::poseTFToMsg (markerPose, rvizMarker_.pose);
405  rvizMarker_.header.frame_id = image_msg->header.frame_id;
406  rvizMarker_.header.stamp = image_msg->header.stamp;
407  rvizMarker_.id = id;
408 
409  rvizMarker_.scale.x = 1.0 * marker_size/100.0;
410  rvizMarker_.scale.y = 1.0 * marker_size/100.0;
411  rvizMarker_.scale.z = 0.2 * marker_size/100.0;
412  rvizMarker_.ns = "basic_shapes";
413  rvizMarker_.type = visualization_msgs::Marker::CUBE;
414  rvizMarker_.action = visualization_msgs::Marker::ADD;
415  switch (id)
416  {
417  case 0:
418  rvizMarker_.color.r = 0.0f;
419  rvizMarker_.color.g = 0.0f;
420  rvizMarker_.color.b = 1.0f;
421  rvizMarker_.color.a = 1.0;
422  break;
423  case 1:
424  rvizMarker_.color.r = 1.0f;
425  rvizMarker_.color.g = 0.0f;
426  rvizMarker_.color.b = 0.0f;
427  rvizMarker_.color.a = 1.0;
428  break;
429  case 2:
430  rvizMarker_.color.r = 0.0f;
431  rvizMarker_.color.g = 1.0f;
432  rvizMarker_.color.b = 0.0f;
433  rvizMarker_.color.a = 1.0;
434  break;
435  case 3:
436  rvizMarker_.color.r = 0.0f;
437  rvizMarker_.color.g = 0.5f;
438  rvizMarker_.color.b = 0.5f;
439  rvizMarker_.color.a = 1.0;
440  break;
441  case 4:
442  rvizMarker_.color.r = 0.5f;
443  rvizMarker_.color.g = 0.5f;
444  rvizMarker_.color.b = 0.0;
445  rvizMarker_.color.a = 1.0;
446  break;
447  default:
448  rvizMarker_.color.r = 0.5f;
449  rvizMarker_.color.g = 0.0f;
450  rvizMarker_.color.b = 0.5f;
451  rvizMarker_.color.a = 1.0;
452  break;
453  }
454  rvizMarker_.lifetime = ros::Duration (1.0);
455  rvizMarkerPub_.publish (rvizMarker_);
456 
457  //Get the pose of the tag in the camera frame, then the output frame (usually torso)
458  tf::Transform tagPoseOutput = CamToOutput * markerPose;
459 
460  //Create the pose marker messages
461  ar_track_alvar_msgs::AlvarMarker ar_pose_marker;
462  tf::poseTFToMsg (tagPoseOutput, ar_pose_marker.pose.pose);
463  ar_pose_marker.header.frame_id = output_frame;
464  ar_pose_marker.header.stamp = image_msg->header.stamp;
465  ar_pose_marker.id = id;
466  arPoseMarkers_.markers.push_back (ar_pose_marker);
467  }
468  arPoseMarkers_.header.stamp = image_msg->header.stamp;
469  arMarkerPub_.publish (arPoseMarkers_);
470  }
471  catch (cv_bridge::Exception& e){
472  ROS_ERROR ("Could not convert from '%s' to 'rgb8'.", image_msg->encoding.c_str ());
473  }
474  }
475 }
476 
477 void configCallback(ar_track_alvar::ParamsConfig &config, uint32_t level)
478 {
479  ROS_INFO("AR tracker reconfigured: %s %.2f %.2f %.2f %.2f", config.enabled ? "ENABLED" : "DISABLED",
480  config.max_frequency, config.marker_size, config.max_new_marker_error, config.max_track_error);
481 
482  enableSwitched = enabled != config.enabled;
483 
484  enabled = config.enabled;
485  max_frequency = config.max_frequency;
486  marker_size = config.marker_size;
487  max_new_marker_error = config.max_new_marker_error;
488  max_track_error = config.max_track_error;
489 }
490 
491 int main(int argc, char *argv[])
492 {
493  ros::init (argc, argv, "marker_detect");
494  ros::NodeHandle n, pn("~");
495 
496  if(argc > 1) {
497  ROS_WARN("Command line arguments are deprecated. Consider using ROS parameters and remappings.");
498 
499  if(argc < 7){
500  std::cout << std::endl;
501  cout << "Not enough arguments provided." << endl;
502  cout << "Usage: ./individualMarkers <marker size in cm> <max new marker error> <max track error> "
503  << "<cam image topic> <cam info topic> <output frame> [ <max frequency> <marker_resolution> <marker_margin>]";
504  std::cout << std::endl;
505  return 0;
506  }
507 
508  // Get params from command line
509  marker_size = atof(argv[1]);
510  max_new_marker_error = atof(argv[2]);
511  max_track_error = atof(argv[3]);
512  cam_image_topic = argv[4];
513  cam_info_topic = argv[5];
514  output_frame = argv[6];
515 
516  if (argc > 7) {
517  max_frequency = atof(argv[7]);
518  pn.setParam("max_frequency", max_frequency);
519  }
520  if (argc > 8)
521  marker_resolution = atoi(argv[8]);
522  if (argc > 9)
523  marker_margin = atoi(argv[9]);
524 
525  } else {
526  // Get params from ros param server.
527  pn.param("marker_size", marker_size, 10.0);
528  pn.param("max_new_marker_error", max_new_marker_error, 0.08);
529  pn.param("max_track_error", max_track_error, 0.2);
530  pn.param("max_frequency", max_frequency, 8.0);
531  pn.setParam("max_frequency", max_frequency); // in case it was not set.
532  pn.param("marker_resolution", marker_resolution, 5);
533  pn.param("marker_margin", marker_margin, 2);
534  pn.param("output_frame_from_msg", output_frame_from_msg, false);
535 
536  if (!output_frame_from_msg && !pn.getParam("output_frame", output_frame)) {
537  ROS_ERROR("Param 'output_frame' has to be set if the output frame is not "
538  "derived from the point cloud message.");
539  exit(EXIT_FAILURE);
540  }
541 
542  // Camera input topics. Use remapping to map to your camera topics.
543  cam_image_topic = "camera_image";
544  cam_info_topic = "camera_info";
545  }
546 
547  // Set dynamically configurable parameters so they don't get replaced by default values
548  pn.setParam("marker_size", marker_size);
549  pn.setParam("max_new_marker_error", max_new_marker_error);
550  pn.setParam("max_track_error", max_track_error);
551 
553 
554  cam = new Camera(n, cam_info_topic);
555 
556  if (!output_frame_from_msg) {
557  // TF listener is only required when output frame != camera frame.
558  tf_listener = new tf::TransformListener(n);
559  }
560  tf_broadcaster = new tf::TransformBroadcaster();
561  arMarkerPub_ = n.advertise < ar_track_alvar_msgs::AlvarMarkers > ("ar_pose_marker", 0);
562  rvizMarkerPub_ = n.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
563  rvizMarkerPub2_ = n.advertise < visualization_msgs::Marker > ("ARmarker_points", 0);
564 
565  // Prepare dynamic reconfiguration
566  dynamic_reconfigure::Server < ar_track_alvar::ParamsConfig > server;
567  dynamic_reconfigure::Server<ar_track_alvar::ParamsConfig>::CallbackType f;
568 
569  f = boost::bind(&configCallback, _1, _2);
570  server.setCallback(f);
571 
572  //Give tf a chance to catch up before the camera callback starts asking for transforms
573  // It will also reconfigure parameters for the first time, setting the default values
574  ros::Duration(1.0).sleep();
575  ros::spinOnce();
576 
577  if (enabled == true)
578  {
579  // This always happens, as enable is true by default
580  ROS_INFO("Subscribing to image topic");
581  cloud_sub_ = n.subscribe(cam_image_topic, 1, &getPointCloudCallback);
582  }
583 
584  // Run at the configured rate, discarding pointcloud msgs if necessary
585  ros::Rate rate(max_frequency);
586 
587  while (ros::ok())
588  {
589  ros::spinOnce();
590  rate.sleep();
591 
592  if (std::abs((rate.expectedCycleTime() - ros::Duration(1.0 / max_frequency)).toSec()) > 0.001)
593  {
594  // Change rate dynamically; if must be above 0, as 0 will provoke a segfault on next spinOnce
595  ROS_DEBUG("Changing frequency from %.2f to %.2f", 1.0 / rate.expectedCycleTime().toSec(), max_frequency);
596  rate = ros::Rate(max_frequency);
597  }
598 
599  if (enableSwitched == true)
600  {
601  // Enable/disable switch: subscribe/unsubscribe to make use of pointcloud processing nodelet
602  // lazy publishing policy; in CPU-scarce computer as TurtleBot's laptop this is a huge saving
603  if (enabled == false)
604  cloud_sub_.shutdown();
605  else
606  cloud_sub_ = n.subscribe(cam_image_topic, 1, &getPointCloudCallback);
607 
608  enableSwitched = false;
609  }
610  }
611 
612  return 0;
613 }
Main ALVAR namespace.
Definition: Alvar.h:174
bool output_frame_from_msg
double max_new_marker_error
int ros_orientation
Definition: Marker.h:181
int GetRes() const
Definition: Marker.h:126
std::string cam_info_topic
bool init
ros::Subscriber cloud_sub_
void publish(const boost::shared_ptr< M > &message) const
f
int main(int argc, char *argv[])
virtual unsigned long GetId() const
Get id for this marker This is used e.g. in MarkerDetector to associate a marker id with an appropria...
Definition: Marker.h:118
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
pcl::PointCloud< ARPoint > ARCloud
void SetMarkerSize(double _edge_length=1, int _res=5, double _margin=2)
int extractOrientation(const pcl::ModelCoefficients &coeffs, const ARPoint &p1, const ARPoint &p2, const ARPoint &p3, const ARPoint &p4, geometry_msgs::Quaternion &retQ)
bool sleep() const
image_transport::Subscriber cam_sub_
This file implements a generic marker detector.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ARCloud::Ptr filterCloud(const ARCloud &cloud, const std::vector< cv::Point, Eigen::aligned_allocator< cv::Point > > &pixels)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
unsigned char * image
Definition: GlutViewer.cpp:155
int PlaneFitPoseImprovement(int id, const ARCloud &corners_3D, ARCloud::Ptr selected_points, const ARCloud &cloud, Pose &p)
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
Simple Camera class for calculating distortions, orientation or projections with pre-calibrated camer...
Definition: Camera.h:82
MarkerDetector< MarkerData > marker_detector
void setIdentity()
pcl::PointXYZRGB ARPoint
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
tf::TransformBroadcaster * tf_broadcaster
std::vector< M, Eigen::aligned_allocator< M > > * markers
std::string cam_image_topic
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
MarkerDetector for detecting markers of type M
Pose pose
The current marker Pose.
Definition: Marker.h:136
float rad
Definition: GlutViewer.cpp:146
Duration expectedCycleTime() const
static const Quaternion & getIdentity()
#define ROS_INFO(...)
tf::TransformListener * tf_listener
bool param(const std::string &param_name, T &param_val, const T &default_val) const
int marker_resolution
double max_frequency
geometry_msgs::Point centroid(const ARCloud &points)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
ROSCPP_DECL bool ok()
void sendTransform(const StampedTransform &transform)
Pose representation derived from the Rotation class
Definition: Pose.h:50
TFSIMD_FORCE_INLINE const tfScalar & x() const
std::vector< PointDouble > ros_marker_points_img
Marker points in image coordinates.
Definition: Marker.h:179
pcl::PointCloud< ARPoint > ARCloud
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Basic 2D Marker functionality.
Definition: Marker.h:52
int marker_margin
Camera * cam
#define ROS_DEBUG_STREAM(args)
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
TFSIMD_FORCE_INLINE const tfScalar & z() const
bool sleep()
double quaternion[4]
Definition: Rotation.h:48
std::string output_frame
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
void drawArrow(gm::Point start, tf::Matrix3x3 mat, string frame, int color, int id)
ALVAR_EXPORT Point< CvPoint2D64f > PointDouble
The default double point type.
Definition: Util.h:108
bool getParam(const std::string &key, std::string &s) const
void draw3dPoints(ARCloud::Ptr cloud, string frame, int color, int id, double rad)
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
int Detect(IplImage *image, Camera *cam, bool track=false, bool visualize=false, double max_new_marker_error=0.08, double max_track_error=0.2, LabelingMethod labeling_method=CVSEQ, bool update_pose=true)
Detect Marker &#39;s from image
int extractFrame(const pcl::ModelCoefficients &coeffs, const ARPoint &p1, const ARPoint &p2, const ARPoint &p3, const ARPoint &p4, tf::Matrix3x3 &retmat)
double max_track_error
static Time now()
pcl::ModelCoefficients coeffs
const int res
cv_bridge::CvImagePtr cv_ptr_
void GetMarkerPoses(IplImage *image, ARCloud &cloud)
double marker_size
ar_track_alvar_msgs::AlvarMarkers arPoseMarkers_
double translation[4]
Definition: Pose.h:54
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
bool enableSwitched
ros::Publisher rvizMarkerPub_
ros::Publisher arMarkerPub_
ros::Publisher rvizMarkerPub2_
visualization_msgs::Marker rvizMarker_
void configCallback(ar_track_alvar::ParamsConfig &config, uint32_t level)
ar_track_alvar::ARCloud ros_corners_3D
Definition: Marker.h:180
PlaneFitResult fitPlane(ARCloud::ConstPtr cloud)
void getPointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
bool enabled
#define ROS_DEBUG(...)
bool getCamInfo_
Definition: Camera.h:92


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Jun 6 2019 19:27:23