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(); }
void videocallback(IplImage *image) { static Camera cam; Pose pose; bool flip_image = (image->origin?true:false); if (flip_image) { cvFlip(image); image->origin = !image->origin; } static bool init = true; if (init) { init = false; // Initialize camera cout<<"Loading calibration: "<<calibrationFilename.str(); if (cam.SetCalib(calibrationFilename.str().c_str(), image->width, image->height)) { cout<<" [Ok]"<<endl; } else { cam.SetRes(image->width, image->height); cout<<" [Fail]"<<endl; } vector<int> id_vector; for(int i = 0; i < nof_markers; ++i) id_vector.push_back(i); // We make the initialization for MultiMarkerBundle using a fixed marker field (can be printed from ALVAR.pdf) marker_detector.SetMarkerSize(marker_size); marker_detector.SetMarkerSizeForId(0, marker_size*2); multi_marker = new MultiMarker(id_vector); pose.Reset(); multi_marker->PointCloudAdd(0, marker_size*2, pose); pose.SetTranslation(-marker_size*2.5, +marker_size*1.5, 0); multi_marker->PointCloudAdd(1, marker_size, pose); pose.SetTranslation(+marker_size*2.5, +marker_size*1.5, 0); multi_marker->PointCloudAdd(2, marker_size, pose); pose.SetTranslation(-marker_size*2.5, -marker_size*1.5, 0); multi_marker->PointCloudAdd(3, marker_size, pose); pose.SetTranslation(+marker_size*2.5, -marker_size*1.5, 0); multi_marker->PointCloudAdd(4, marker_size, pose); } double error=-1; if (marker_detector.Detect(image, &cam, true, (visualize == 1), 0.0)) { if (detect_additional) { error = multi_marker->Update(marker_detector.markers, &cam, pose); multi_marker->SetTrackMarkers(marker_detector, &cam, pose, visualize ? image : NULL); marker_detector.DetectAdditional(image, &cam, (visualize == 1)); } if (visualize == 2) error = multi_marker->Update(marker_detector.markers, &cam, pose, image); else error = multi_marker->Update(marker_detector.markers, &cam, pose); } static Marker foo; foo.SetMarkerSize(marker_size*4); if ((error >= 0) && (error < 5)) { foo.pose = pose; } foo.Visualize(image, &cam); if (flip_image) { cvFlip(image); image->origin = !image->origin; } }