FindMarkerBundlesNoKinect.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 
42 #include "ar_track_alvar/Shared.h"
43 #include <cv_bridge/cv_bridge.h>
44 #include <ar_track_alvar_msgs/AlvarMarker.h>
45 #include <ar_track_alvar_msgs/AlvarMarkers.h>
46 #include <tf/transform_listener.h>
49 
50 using namespace alvar;
51 using namespace std;
52 
53 #define MAIN_MARKER 1
54 #define VISIBLE_MARKER 2
55 #define GHOST_MARKER 3
56 
62 ar_track_alvar_msgs::AlvarMarkers arPoseMarkers_;
68 int *master_id;
70 std::vector<int> *bundle_indices;
71 bool init = true;
72 
73 double marker_size;
76 std::string cam_image_topic;
77 std::string cam_info_topic;
78 std::string output_frame;
79 int n_bundles = 0;
80 
81 void GetMultiMarkerPoses(IplImage *image);
82 void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg);
83 void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_msg, tf::StampedTransform &CamToOutput, visualization_msgs::Marker *rvizMarker, ar_track_alvar_msgs::AlvarMarker *ar_pose_marker);
84 
85 
86 // Updates the bundlePoses of the multi_marker_bundles by detecting markers and using all markers in a bundle to infer the master tag's position
87 void GetMultiMarkerPoses(IplImage *image) {
88 
89  if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)){
90  for(int i=0; i<n_bundles; i++)
91  multi_marker_bundles[i]->Update(marker_detector.markers, cam, bundlePoses[i]);
92 
93  if(marker_detector.DetectAdditional(image, cam, false) > 0){
94  for(int i=0; i<n_bundles; i++){
95  if ((multi_marker_bundles[i]->SetTrackMarkers(marker_detector, cam, bundlePoses[i], image) > 0))
96  multi_marker_bundles[i]->Update(marker_detector.markers, cam, bundlePoses[i]);
97  }
98  }
99  }
100 }
101 
102 
103 // Given the pose of a marker, builds the appropriate ROS messages for later publishing
104 void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_msg, tf::StampedTransform &CamToOutput, visualization_msgs::Marker *rvizMarker, ar_track_alvar_msgs::AlvarMarker *ar_pose_marker){
105  double px,py,pz,qx,qy,qz,qw;
106 
107  px = p.translation[0]/100.0;
108  py = p.translation[1]/100.0;
109  pz = p.translation[2]/100.0;
110  qx = p.quaternion[1];
111  qy = p.quaternion[2];
112  qz = p.quaternion[3];
113  qw = p.quaternion[0];
114 
115  //Get the marker pose in the camera frame
116  tf::Quaternion rotation (qx,qy,qz,qw);
117  tf::Vector3 origin (px,py,pz);
118  tf::Transform t (rotation, origin); //transform from cam to marker
119 
120  tf::Vector3 markerOrigin (0, 0, 0);
121  tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
122  tf::Transform markerPose = t * m;
123 
124  //Publish the cam to marker transform for main marker in each bundle
125  if(type==MAIN_MARKER){
126  std::string markerFrame = "ar_marker_";
127  std::stringstream out;
128  out << id;
129  std::string id_string = out.str();
130  markerFrame += id_string;
131  tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
132  tf_broadcaster->sendTransform(camToMarker);
133  }
134 
135  //Create the rviz visualization message
136  tf::poseTFToMsg (markerPose, rvizMarker->pose);
137  rvizMarker->header.frame_id = image_msg->header.frame_id;
138  rvizMarker->header.stamp = image_msg->header.stamp;
139  rvizMarker->id = id;
140 
141  rvizMarker->scale.x = 1.0 * marker_size/100.0;
142  rvizMarker->scale.y = 1.0 * marker_size/100.0;
143  rvizMarker->scale.z = 0.2 * marker_size/100.0;
144 
145  if(type==MAIN_MARKER)
146  rvizMarker->ns = "main_shapes";
147  else
148  rvizMarker->ns = "basic_shapes";
149 
150 
151  rvizMarker->type = visualization_msgs::Marker::CUBE;
152  rvizMarker->action = visualization_msgs::Marker::ADD;
153 
154  //Determine a color and opacity, based on marker type
155  if(type==MAIN_MARKER){
156  rvizMarker->color.r = 1.0f;
157  rvizMarker->color.g = 0.0f;
158  rvizMarker->color.b = 0.0f;
159  rvizMarker->color.a = 1.0;
160  }
161  else if(type==VISIBLE_MARKER){
162  rvizMarker->color.r = 0.0f;
163  rvizMarker->color.g = 1.0f;
164  rvizMarker->color.b = 0.0f;
165  rvizMarker->color.a = 0.7;
166  }
167  else if(type==GHOST_MARKER){
168  rvizMarker->color.r = 0.0f;
169  rvizMarker->color.g = 0.0f;
170  rvizMarker->color.b = 1.0f;
171  rvizMarker->color.a = 0.5;
172  }
173 
174  rvizMarker->lifetime = ros::Duration (1.0);
175 
176  // Only publish the pose of the master tag in each bundle, since that's all we really care about aside from visualization
177  if(type==MAIN_MARKER){
178  //Take the pose of the tag in the camera frame and convert to the output frame (usually torso_lift_link for the PR2)
179  tf::Transform tagPoseOutput = CamToOutput * markerPose;
180 
181  //Create the pose marker message
182  tf::poseTFToMsg (tagPoseOutput, ar_pose_marker->pose.pose);
183  ar_pose_marker->header.frame_id = output_frame;
184  ar_pose_marker->header.stamp = image_msg->header.stamp;
185  ar_pose_marker->id = id;
186  }
187  else
188  ar_pose_marker = NULL;
189 }
190 
191 
192 //Callback to handle getting video frames and processing them
193 void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
194 {
195  //If we've already gotten the cam info, then go ahead
196  if(cam->getCamInfo_){
197  try{
198  //Get the transformation from the Camera to the output frame for this image capture
199  tf::StampedTransform CamToOutput;
200  try{
201  tf_listener->waitForTransform(output_frame, image_msg->header.frame_id, image_msg->header.stamp, ros::Duration(1.0));
202  tf_listener->lookupTransform(output_frame, image_msg->header.frame_id, image_msg->header.stamp, CamToOutput);
203  }
204  catch (tf::TransformException ex){
205  ROS_ERROR("%s",ex.what());
206  }
207 
208  visualization_msgs::Marker rvizMarker;
209  ar_track_alvar_msgs::AlvarMarker ar_pose_marker;
210  arPoseMarkers_.markers.clear ();
211 
212 
213  //Convert the image
215 
216  //Get the estimated pose of the main markers by using all the markers in each bundle
217 
218  // GetMultiMarkersPoses expects an IplImage*, but as of ros groovy, cv_bridge gives
219  // us a cv::Mat. I'm too lazy to change to cv::Mat throughout right now, so I
220  // do this conversion here -jbinney
221  IplImage ipl_image = cv_ptr_->image;
222  GetMultiMarkerPoses(&ipl_image);
223 
224  //Draw the observed markers that are visible and note which bundles have at least 1 marker seen
225  for(int i=0; i<n_bundles; i++)
226  bundles_seen[i] = false;
227 
228  for (size_t i=0; i<marker_detector.markers->size(); i++)
229  {
230  int id = (*(marker_detector.markers))[i].GetId();
231 
232  // Draw if id is valid
233  if(id >= 0){
234 
235  //Mark the bundle that marker belongs to as "seen"
236  for(int j=0; j<n_bundles; j++){
237  for(int k=0; k<bundle_indices[j].size(); k++){
238  if(bundle_indices[j][k] == id){
239  bundles_seen[j] = true;
240  break;
241  }
242  }
243  }
244 
245  // Don't draw if it is a master tag...we do this later, a bit differently
246  bool should_draw = true;
247  for(int i=0; i<n_bundles; i++){
248  if(id == master_id[i]) should_draw = false;
249  }
250  if(should_draw){
251  Pose p = (*(marker_detector.markers))[i].pose;
252  makeMarkerMsgs(VISIBLE_MARKER, id, p, image_msg, CamToOutput, &rvizMarker, &ar_pose_marker);
253  rvizMarkerPub_.publish (rvizMarker);
254  }
255  }
256  }
257 
258  //Draw the main markers, whether they are visible or not -- but only if at least 1 marker from their bundle is currently seen
259  for(int i=0; i<n_bundles; i++)
260  {
261  if(bundles_seen[i] == true){
262  makeMarkerMsgs(MAIN_MARKER, master_id[i], bundlePoses[i], image_msg, CamToOutput, &rvizMarker, &ar_pose_marker);
263  rvizMarkerPub_.publish (rvizMarker);
264  arPoseMarkers_.markers.push_back (ar_pose_marker);
265  }
266  }
267 
268  //Publish the marker messages
269  arMarkerPub_.publish (arPoseMarkers_);
270  }
271  catch (cv_bridge::Exception& e){
272  ROS_ERROR ("Could not convert from '%s' to 'rgb8'.", image_msg->encoding.c_str ());
273  }
274  }
275 }
276 
277 
278 
279 int main(int argc, char *argv[])
280 {
281  ros::init (argc, argv, "marker_detect");
282  ros::NodeHandle n;
283 
284  if(argc < 8){
285  std::cout << std::endl;
286  cout << "Not enough arguments provided." << endl;
287  cout << "Usage: ./findMarkerBundles <marker size in cm> <max new marker error> <max track error> <cam image topic> <cam info topic> <output frame> <list of bundle XML files...>" << endl;
288  std::cout << std::endl;
289  return 0;
290  }
291 
292  // Get params from command line
293  marker_size = atof(argv[1]);
294  max_new_marker_error = atof(argv[2]);
295  max_track_error = atof(argv[3]);
296  cam_image_topic = argv[4];
297  cam_info_topic = argv[5];
298  output_frame = argv[6];
299  int n_args_before_list = 7;
300  n_bundles = argc - n_args_before_list;
301 
302  marker_detector.SetMarkerSize(marker_size);
303  multi_marker_bundles = new MultiMarkerBundle*[n_bundles];
304  bundlePoses = new Pose[n_bundles];
305  master_id = new int[n_bundles];
306  bundle_indices = new std::vector<int>[n_bundles];
307  bundles_seen = new bool[n_bundles];
308 
309  // Load the marker bundle XML files
310  for(int i=0; i<n_bundles; i++){
311  bundlePoses[i].Reset();
312  MultiMarker loadHelper;
313  if(loadHelper.Load(argv[i + n_args_before_list], FILE_FORMAT_XML)){
314  vector<int> id_vector = loadHelper.getIndices();
315  multi_marker_bundles[i] = new MultiMarkerBundle(id_vector);
316  multi_marker_bundles[i]->Load(argv[i + n_args_before_list], FILE_FORMAT_XML);
317  master_id[i] = multi_marker_bundles[i]->getMasterId();
318  bundle_indices[i] = multi_marker_bundles[i]->getIndices();
319  }
320  else{
321  cout<<"Cannot load file "<< argv[i + n_args_before_list] << endl;
322  return 0;
323  }
324  }
325 
326  // Set up camera, listeners, and broadcasters
327  cam = new Camera(n, cam_info_topic);
328  tf_listener = new tf::TransformListener(n);
329  tf_broadcaster = new tf::TransformBroadcaster();
330  arMarkerPub_ = n.advertise < ar_track_alvar_msgs::AlvarMarkers > ("ar_pose_marker", 0);
331  rvizMarkerPub_ = n.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
332 
333  //Give tf a chance to catch up before the camera callback starts asking for transforms
334  ros::Duration(1.0).sleep();
335  ros::spinOnce();
336 
337  //Subscribe to topics and set up callbacks
338  ROS_INFO ("Subscribing to image topic");
340  cam_sub_ = it_.subscribe (cam_image_topic, 1, &getCapCallback);
341 
342  ros::spin();
343 
344  return 0;
345 }
Main ALVAR namespace.
Definition: Alvar.h:174
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())
std::string cam_info_topic
void publish(const boost::shared_ptr< M > &message) const
void getCapCallback(const sensor_msgs::ImageConstPtr &image_msg)
#define VISIBLE_MARKER
Camera * cam
void SetMarkerSize(double _edge_length=1, int _res=5, double _margin=2)
bool sleep() const
This file implements a generic marker detector.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
unsigned char * image
Definition: GlutViewer.cpp:155
void GetMultiMarkerPoses(IplImage *image)
Base class for using MultiMarker.
Definition: MultiMarker.h:47
int DetectAdditional(IplImage *image, Camera *cam, bool visualize=false, double max_track_error=0.2)
Simple Camera class for calculating distortions, orientation or projections with pre-calibrated camer...
Definition: Camera.h:82
MarkerDetector< MarkerData > marker_detector
ROSCPP_DECL void spin(Spinner &spinner)
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
std::vector< M, Eigen::aligned_allocator< M > > * markers
std::string cam_image_topic
ar_track_alvar_msgs::AlvarMarkers arPoseMarkers_
MarkerDetector for detecting markers of type M
void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_msg, tf::StampedTransform &CamToOutput, visualization_msgs::Marker *rvizMarker, ar_track_alvar_msgs::AlvarMarker *ar_pose_marker)
static const Quaternion & getIdentity()
#define ROS_INFO(...)
void Reset()
Definition: Pose.cpp:69
double max_new_marker_error
std::vector< int > * bundle_indices
Multi marker that uses bundle adjustment to refine the 3D positions of the markers (point cloud)...
MultiMarkerBundle ** multi_marker_bundles
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void sendTransform(const StampedTransform &transform)
Pose representation derived from the Rotation class
Definition: Pose.h:50
#define MAIN_MARKER
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Pose * bundlePoses
tf::TransformListener * tf_listener
XML file format.
Definition: FileFormat.h:66
double marker_size
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
#define GHOST_MARKER
double max_track_error
ros::Publisher arMarkerPub_
double quaternion[4]
Definition: Rotation.h:48
tf::TransformBroadcaster * tf_broadcaster
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
This file implements a initialization algorithm to create a multi-marker field configuration.
bool Load(const char *fname, FILE_FORMAT format=FILE_FORMAT_DEFAULT)
Loads multi marker configuration from a text file.
ros::Publisher rvizMarkerPub_
int main(int argc, char *argv[])
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
image_transport::Subscriber cam_sub_
bool * bundles_seen
This file implements an algorithm to create a multi-marker field configuration.
cv_bridge::CvImagePtr cv_ptr_
double translation[4]
Definition: Pose.h:54
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
std::string output_frame
std::vector< int > getIndices()
Definition: MultiMarker.h:176
bool getCamInfo_
Definition: Camera.h:92


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