C++ Marker::GetId方法代码示例

void GetMarkerPoses(IplImage *image, ARCloud &cloud) {

  //Detect and track the markers
  if (marker_detector.Detect(image, cam, true, false, max_new_marker_error,
			     max_track_error, CVSEQ, true)) 
    {
      printf("\n--------------------------\n\n");
      for (size_t i=0; i<marker_detector.markers->size(); i++)
     	{
	  vector<cv::Point, Eigen::aligned_allocator<cv::Point> > pixels;
	  Marker *m = &((*marker_detector.markers)[i]);
	  int id = m->GetId();
	  cout << "******* ID: " << id << endl;

	  int resol = m->GetRes();
	  int ori = m->ros_orientation;
      
	  PointDouble pt1, pt2, pt3, pt4;
	  pt4 = m->ros_marker_points_img[0];
	  pt3 = m->ros_marker_points_img[resol-1];
	  pt1 = m->ros_marker_points_img[(resol*resol)-resol];
	  pt2 = m->ros_marker_points_img[(resol*resol)-1];
	  
	  m->ros_corners_3D[0] = cloud(pt1.x, pt1.y);
	  m->ros_corners_3D[1] = cloud(pt2.x, pt2.y);
	  m->ros_corners_3D[2] = cloud(pt3.x, pt3.y);
	  m->ros_corners_3D[3] = cloud(pt4.x, pt4.y);
	  
	  if(ori >= 0 && ori < 4){
	    if(ori != 0){
	      std::rotate(m->ros_corners_3D.begin(), m->ros_corners_3D.begin() + ori, m->ros_corners_3D.end());
	    }
	  }
	  else
	    ROS_ERROR("FindMarkerBundles: Bad Orientation: %i for ID: %i", ori, id);

	  //Get the 3D marker points
	  BOOST_FOREACH (const PointDouble& p, m->ros_marker_points_img)
	    pixels.push_back(cv::Point(p.x, p.y));	  
	  ARCloud::Ptr selected_points = ata::filterCloud(cloud, pixels);

	  //Use the kinect data to find a plane and pose for the marker
	  int ret = PlaneFitPoseImprovement(i, m->ros_corners_3D, selected_points, cloud, m->pose);	
	}
    }

	int MarkerDetectorImpl::Detect(IplImage *image,
			   Camera *cam,
			   bool track,
			   bool visualize,
			   double max_new_marker_error,
			   double max_track_error,
			   LabelingMethod labeling_method,
			   bool update_pose)
	{
		assert(image->origin == 0); // Currently only top-left origin supported
		double error=-1;

		// Swap marker tables
		_swap_marker_tables();
		_markers_clear();

		switch(labeling_method)
		{
			case CVSEQ :
		
				if(!labeling)
					labeling = new LabelingCvSeq();
				((LabelingCvSeq*)labeling)->SetOptions(detect_pose_grayscale);
				break;
		}

		labeling->SetCamera(cam);
		labeling->LabelSquares(image, visualize);
		vector<vector<PointDouble> >& blob_corners = labeling->blob_corners;
		IplImage* gray = labeling->gray;

		int orientation;

		// When tracking we find the best matching blob and test if it is near enough?
		if (track) {
			for (size_t ii=0; ii<_track_markers_size(); ii++) {
				Marker *mn = _track_markers_at(ii);
				if (mn->GetError(Marker::DECODE_ERROR|Marker::MARGIN_ERROR) > 0) continue; // We track only perfectly decoded markers
				int track_i=-1;
				int track_orientation=0;
				double track_error=1e200;
				for(unsigned i = 0; i < blob_corners.size()/*blobs_ret.size()*/; ++i) {
					if (blob_corners[i].empty()) continue;
					mn->CompareCorners(blob_corners[i], &orientation, &error);
					if (error < track_error) {
						track_i = i;
						track_orientation = orientation;
						track_error = error;
					}
				}
				if (track_error <= max_track_error) {
					mn->SetError(Marker::DECODE_ERROR, 0);
					mn->SetError(Marker::MARGIN_ERROR, 0);
					mn->SetError(Marker::TRACK_ERROR, track_error);
					mn->UpdatePose(blob_corners[track_i], cam, track_orientation, update_pose);
					_markers_push_back(mn);
					blob_corners[track_i].clear(); // We don't want to handle this again...
					if (visualize) mn->Visualize(image, cam, CV_RGB(255,255,0));
				}
			}
		}

		// Now we go through the rest of the blobs -- in case there are new markers...
		for(size_t i = 0; i < blob_corners.size(); ++i)
		{
			if (blob_corners[i].empty()) continue;

			Marker *mn = new_M(edge_length, res, margin);
			if (mn->UpdateContent(blob_corners[i], gray, cam) &&
			    mn->DecodeContent(&orientation) &&
				(mn->GetError(Marker::MARGIN_ERROR | Marker::DECODE_ERROR) <= max_new_marker_error))
			{
				if (map_edge_length.find(mn->GetId()) != map_edge_length.end()) {
					mn->SetMarkerSize(map_edge_length[mn->GetId()], res, margin);
				}
				mn->UpdatePose(blob_corners[i], cam, orientation, update_pose);
				_markers_push_back(mn);
 
				if (visualize) mn->Visualize(image, cam, CV_RGB(255,0,0));
			}
			delete mn;
		}

		return (int) _markers_size();
	}

// 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
void GetMultiMarkerPoses(IplImage *image, ARCloud &cloud) {

  for(int i=0; i<n_bundles; i++){
    master_visible[i] = false;
    bundles_seen[i] = 0;
  }
  
  //Detect and track the markers
  if (marker_detector.Detect(image, cam, true, false, max_new_marker_error,
			     max_track_error, CVSEQ, true)) 
    {
      //printf("\n--------------------------\n\n");
      for (size_t i=0; i<marker_detector.markers->size(); i++)
    	{
	  vector<cv::Point, Eigen::aligned_allocator<cv::Point> > pixels;
	  Marker *m = &((*marker_detector.markers)[i]);
	  int id = m->GetId();
	  //cout << "******* ID: " << id << endl;
      
	  //Get the 3D points of the outer corners
          /*
	  PointDouble corner0 = m->marker_corners_img[0];
	  PointDouble corner1 = m->marker_corners_img[1];
	  PointDouble corner2 = m->marker_corners_img[2];
	  PointDouble corner3 = m->marker_corners_img[3];
	  m->ros_corners_3D[0] = cloud(corner0.x, corner0.y);
	  m->ros_corners_3D[1] = cloud(corner1.x, corner1.y);
	  m->ros_corners_3D[2] = cloud(corner2.x, corner2.y);
	  m->ros_corners_3D[3] = cloud(corner3.x, corner3.y);
	  */
          
	  //Get the 3D inner corner points - more stable than outer corners that can "fall off" object
	  int resol = m->GetRes();
	  int ori = m->ros_orientation;
      
	  PointDouble pt1, pt2, pt3, pt4;
	  pt4 = m->ros_marker_points_img[0];
	  pt3 = m->ros_marker_points_img[resol-1];
	  pt1 = m->ros_marker_points_img[(resol*resol)-resol];
	  pt2 = m->ros_marker_points_img[(resol*resol)-1];
	  
	  m->ros_corners_3D[0] = cloud(pt1.x, pt1.y);
	  m->ros_corners_3D[1] = cloud(pt2.x, pt2.y);
	  m->ros_corners_3D[2] = cloud(pt3.x, pt3.y);
	  m->ros_corners_3D[3] = cloud(pt4.x, pt4.y);
	  
	  if(ori >= 0 && ori < 4){
	    if(ori != 0){
	      std::rotate(m->ros_corners_3D.begin(), m->ros_corners_3D.begin() + ori, m->ros_corners_3D.end());
	    }
	  }
	  else
	    ROS_ERROR("FindMarkerBundles: Bad Orientation: %i for ID: %i", ori, id);

	  //Check if we have spotted a master tag
	  int master_ind = -1;
	  for(int j=0; j<n_bundles; j++){
	    if(id == master_id[j])
	      master_visible[j] = true; 
	    master_ind = j;
	  }

	  //Mark the bundle that marker belongs to as "seen"
	  int bundle_ind = -1;
	  for(int j=0; j<n_bundles; j++){
	    for(int k=0; k<bundle_indices[j].size(); k++){
	      if(bundle_indices[j][k] == id){
            bundle_ind = j;
            bundles_seen[j] += 1;
            break;
          }
        }
      }

	  //Get the 3D marker points
	  BOOST_FOREACH (const PointDouble& p, m->ros_marker_points_img)
	    pixels.push_back(cv::Point(p.x, p.y));	  
	  ARCloud::Ptr selected_points = ata::filterCloud(cloud, pixels);

	  //Use the kinect data to find a plane and pose for the marker
	  int ret = PlaneFitPoseImprovement(i, m->ros_corners_3D, selected_points, cloud, m->pose);
            
	  //If the plane fit fails...
	  if(ret < 0){
		//Mark this tag as invalid
		m->valid = false;
	    //If this was a master tag, reset its visibility
	    if(master_ind >= 0)
	      master_visible[master_ind] = false;
	    //decrement the number of markers seen in this bundle
	    bundles_seen[bundle_ind] -= 1;
	      
	  }
	  else
		m->valid = true;
	}	

      //For each master tag, infer the 3D position of its corners from other visible tags
//.........这里部分代码省略.........