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; } }
double GetMultiMarkerPose(IplImage *image, Pose &pose) { static bool init=true; if (init) { init=false; vector<int> id_vector; for(int i = 0; i < nof_markers; ++i) id_vector.push_back(i); // We make the initialization for MultiMarkerBundle using MultiMarkerInitializer // Each marker needs to be visible in at least two images and at most 64 image are used. multi_marker_init = new MultiMarkerInitializer(id_vector, 2, 64); pose.Reset(); multi_marker_init->PointCloudAdd(id_vector[0], marker_size, pose); multi_marker_bundle = new MultiMarkerBundle(id_vector); marker_detector.SetMarkerSize(marker_size); } double error = -1; if (!optimize_done) { if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)) { error = multi_marker_init->Update(marker_detector.markers, cam, pose); } } else { if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)) { error = multi_marker_bundle->Update(marker_detector.markers, cam, pose); if ((multi_marker_bundle->SetTrackMarkers(marker_detector, cam, pose, image) > 0) && (marker_detector.DetectAdditional(image, cam, false) > 0)) { error = multi_marker_bundle->Update(marker_detector.markers, cam, pose); } } } if (add_measurement) { cout << "Markers seen: " << marker_detector.markers->size() << "\n"; if (marker_detector.markers->size() >= 2) { cout<<"Adding measurement..."<<endl; multi_marker_init->MeasurementsAdd(marker_detector.markers); } else{ cout << "Not enough markers to capture measurement\n"; } add_measurement=false; } if (optimize) { cout<<"Initializing..."<<endl; if (!multi_marker_init->Initialize(cam)) { cout<<"Initialization failed, add some more measurements."<<endl; } else { // Reset the bundle adjuster. multi_marker_bundle->Reset(); multi_marker_bundle->MeasurementsReset(); // Copy all measurements into the bundle adjuster. for (int i = 0; i < multi_marker_init->getMeasurementCount(); ++i) { Pose p2; multi_marker_init->getMeasurementPose(i, cam, p2); const std::vector<MultiMarkerInitializer::MarkerMeasurement, Eigen::aligned_allocator<MultiMarkerInitializer::MarkerMeasurement> > markers = multi_marker_init->getMeasurementMarkers(i); multi_marker_bundle->MeasurementsAdd(&markers, p2); } // Initialize the bundle adjuster with initial marker poses. multi_marker_bundle->PointCloudCopy(multi_marker_init); cout<<"Optimizing..."<<endl; if (multi_marker_bundle->Optimize(cam, 0.01, 20)) { cout<<"Optimizing done"<<endl; optimize_done=true; } else { cout<<"Optimizing FAILED!"<<endl; } } optimize=false; } return error; }
void AddMarker(const char *id) { if (marker_type == 0) { MarkerData md(marker_side_len, content_res, margin_res); int side_len = int(marker_side_len*units+0.5); if (img == 0) { img = cvCreateImage(cvSize(side_len, side_len), IPL_DEPTH_8U, 1); filename.str(""); filename<<"MarkerData"; minx = (posx*units) - (marker_side_len*units/2.0); miny = (posy*units) - (marker_side_len*units/2.0); maxx = (posx*units) + (marker_side_len*units/2.0); maxy = (posy*units) + (marker_side_len*units/2.0); } else { double new_minx = (posx*units) - (marker_side_len*units/2.0); double new_miny = (posy*units) - (marker_side_len*units/2.0); double new_maxx = (posx*units) + (marker_side_len*units/2.0); double new_maxy = (posy*units) + (marker_side_len*units/2.0); if (minx < new_minx) new_minx = minx; if (miny < new_miny) new_miny = miny; if (maxx > new_maxx) new_maxx = maxx; if (maxy > new_maxy) new_maxy = maxy; IplImage *new_img = cvCreateImage(cvSize(int(new_maxx-new_minx+0.5), int(new_maxy-new_miny+0.5)), IPL_DEPTH_8U, 1); cvSet(new_img, cvScalar(255)); CvRect roi = cvRect(int(minx-new_minx+0.5), int(miny-new_miny+0.5), img->width, img->height); cvSetImageROI(new_img, roi); cvCopy(img, new_img); cvReleaseImage(&img); img = new_img; roi.x = int((posx*units) - (marker_side_len*units/2.0) - new_minx + 0.5); roi.y = int((posy*units) - (marker_side_len*units/2.0) - new_miny + 0.5); roi.width = int(marker_side_len*units+0.5); roi.height = int(marker_side_len*units+0.5); cvSetImageROI(img, roi); minx = new_minx; miny = new_miny; maxx = new_maxx; maxy = new_maxy; } if (marker_data_content_type == MarkerData::MARKER_CONTENT_TYPE_NUMBER) { int idi = atoi(id); md.SetContent(marker_data_content_type, idi, 0); if (filename.str().length()<64) filename<<"_"<<idi; Pose pose; pose.Reset(); pose.SetTranslation(posx, -posy, 0); multi_marker.PointCloudAdd(idi, marker_side_len, pose); } else { md.SetContent(marker_data_content_type, 0, id); const char *p = id; int counter=0; filename<<"_"; while(*p) { if (!isalnum(*p)) filename<<"_"; else filename<<(char)tolower(*p); p++; counter++; if (counter > 8) break; } } md.ScaleMarkerToImage(img); cvResetImageROI(img); } else if (marker_type == 1) { // Create and save MarkerArtoolkit marker (Note, that this doesn't support multi markers) MarkerArtoolkit md(marker_side_len, content_res, margin_res); int side_len = int(marker_side_len*units+0.5); if (img != 0) cvReleaseImage(&img); img = cvCreateImage(cvSize(side_len, side_len), IPL_DEPTH_8U, 1); filename.str(""); filename<<"MarkerArtoolkit"; md.SetContent(atoi(id)); filename<<"_"<<atoi(id); md.ScaleMarkerToImage(img); } }