int MarkerDetectorImpl::DetectAdditional(IplImage *image, Camera *cam, bool visualize, double max_track_error) { assert(image->origin == 0); // Currently only top-left origin supported if(!labeling) return -1; double error=-1; int orientation; int count=0; vector<vector<PointDouble> >& blob_corners = labeling->blob_corners; 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(); ++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); _markers_push_back(mn); count++; blob_corners[track_i].clear(); // We don't want to handle this again... if (visualize) { mn->Visualize(image, cam, CV_RGB(0,255,255)); } } } return count; }
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(); }