SampleMultiMarkerBundle.cpp
Go to the documentation of this file.
00001 #include "CvTestbed.h"
00002 #include "MarkerDetector.h"
00003 #include "MultiMarkerBundle.h"
00004 #include "MultiMarkerInitializer.h"
00005 #include "Pose.h"
00006 #include "Shared.h"
00007 using namespace alvar;
00008 using namespace std;
00009 
00010 int visualize=1;
00011 const int nof_markers = 8;
00012 const double marker_size = 4;
00013 bool add_measurement=false;
00014 bool optimize = false;
00015 bool optimize_done = false;
00016 MarkerDetector<MarkerData> marker_detector;
00017 std::stringstream calibrationFilename;
00018 
00019 MultiMarkerInitializer *multi_marker_init=NULL;
00020 MultiMarkerBundle *multi_marker_bundle=NULL;
00021 double GetMultiMarkerPose(IplImage *image, Camera *cam, Pose &pose) {
00022     static bool init=true;
00023     if (init) {
00024         cout<<"Using manual multimarker approach with MultiMarkerInitializer & MultiMarkerBundle."<<endl;
00025         cout<<"Use 'p' for taking keyframes and 'o' for optimizing."<<endl;
00026         init=false;
00027         vector<int> id_vector;
00028         for(int i = 0; i < nof_markers; ++i)
00029             id_vector.push_back(i);
00030         // We make the initialization for MultiMarkerBundle using MultiMarkerInitializer
00031         // Each marker needs to be visible in at least two images and at most 64 image are used.
00032         multi_marker_init = new MultiMarkerInitializer(id_vector, 2, 64);
00033         pose.Reset();
00034         multi_marker_init->PointCloudAdd(id_vector[0], marker_size, pose);
00035         multi_marker_bundle = new MultiMarkerBundle(id_vector);
00036     }
00037 
00038     double error = -1;
00039     if (!optimize_done) {
00040         if (marker_detector.Detect(image, cam, true, (visualize == 1), 0.0)) {
00041             if (visualize == 2)
00042                 error = multi_marker_init->Update(marker_detector.markers, cam, pose, image);
00043             else
00044                 error = multi_marker_init->Update(marker_detector.markers, cam, pose);
00045         }
00046     } else {
00047         if (marker_detector.Detect(image, cam, true, (visualize == 1), 0.0)) {
00048             if (visualize == 2)
00049                 error = multi_marker_bundle->Update(marker_detector.markers, cam, pose, image);
00050             else 
00051                 error = multi_marker_bundle->Update(marker_detector.markers, cam, pose);
00052             if ((multi_marker_bundle->SetTrackMarkers(marker_detector, cam, pose, image) > 0) &&
00053                 (marker_detector.DetectAdditional(image, cam, (visualize == 3)) > 0))
00054             {
00055                 if (visualize == 3)
00056                     error = multi_marker_bundle->Update(marker_detector.markers, cam, pose, image);
00057                 else
00058                     error = multi_marker_bundle->Update(marker_detector.markers, cam, pose);
00059             }
00060         }
00061     }
00062 
00063     if (add_measurement) {
00064         if (marker_detector.markers->size() >= 2) {
00065             cout<<"Adding measurement..."<<endl;
00066             multi_marker_init->MeasurementsAdd(marker_detector.markers);
00067             add_measurement=false;
00068         }
00069     }
00070 
00071     if (optimize) {
00072         cout<<"Initializing..."<<endl;
00073         if (!multi_marker_init->Initialize(cam)) {
00074             cout<<"Initialization failed, add some more measurements."<<endl;
00075 
00076         } else {
00077             // Reset the bundle adjuster.
00078             multi_marker_bundle->Reset();
00079             multi_marker_bundle->MeasurementsReset();
00080             // Copy all measurements into the bundle adjuster.
00081             for (int i = 0; i < multi_marker_init->getMeasurementCount(); ++i) {
00082                 Pose pose;
00083                 multi_marker_init->getMeasurementPose(i, cam, pose);
00084                 const std::vector<MultiMarkerInitializer::MarkerMeasurement> markers 
00085                     = multi_marker_init->getMeasurementMarkers(i);
00086                 multi_marker_bundle->MeasurementsAdd(&markers, pose);
00087             }
00088             // Initialize the bundle adjuster with initial marker poses.
00089             multi_marker_bundle->PointCloudCopy(multi_marker_init);
00090             cout<<"Optimizing..."<<endl;
00091             if (multi_marker_bundle->Optimize(cam, 0.01, 20)) {
00092                 cout<<"Optimizing done"<<endl;
00093                 optimize_done=true;
00094 
00095             } else {
00096                 cout<<"Optimizing FAILED!"<<endl;
00097             }
00098         }
00099         optimize=false;
00100     }
00101     return error;
00102 }
00103 
00104 void videocallback(IplImage *image)
00105 {
00106     static Camera cam;
00107     static Pose pose;
00108     bool flip_image = (image->origin?true:false);
00109     if (flip_image) {
00110         cvFlip(image);
00111         image->origin = !image->origin;
00112     }
00113 
00114     static bool init = true;
00115     if (init)
00116     {
00117         init = false;
00118 
00119         // Initialize camera
00120         cout<<"Loading calibration: "<<calibrationFilename.str();
00121 
00122         if (cam.SetCalib(calibrationFilename.str().c_str(), image->width, image->height))
00123         {
00124             cout<<" [Ok]"<<endl;
00125         }
00126         else
00127         {
00128             cam.SetRes(image->width, image->height);
00129             cout<<" [Fail]"<<endl;
00130         }
00131 
00132         marker_detector.SetMarkerSize(marker_size);
00133     }
00134 
00135     // Manual approach (use 'p' for taking keyframes and 'o' for optimizing)
00136     double error = GetMultiMarkerPose(image, &cam, pose);
00137 
00138     // Visualize a blue marker at the origo.
00139     static Marker foo;
00140     foo.SetMarkerSize(marker_size);
00141     if ((error >= 0) && (error < 5))
00142     {
00143         foo.pose = pose;
00144     }
00145     foo.Visualize(image, &cam, CV_RGB(0,0,255));
00146 
00147     if (flip_image) {
00148         cvFlip(image);
00149         image->origin = !image->origin;
00150     }
00151 }
00152 
00153 int keycallback(int key)
00154 {
00155     static bool fixed = false;
00156 
00157     if(key == 'r')
00158     {
00159         cout<<"Reseting multi marker"<<endl;
00160             multi_marker_init->Reset();
00161             multi_marker_init->MeasurementsReset();
00162             multi_marker_bundle->Reset();
00163             multi_marker_bundle->MeasurementsReset();
00164             add_measurement = false;
00165             optimize        = false;
00166             optimize_done   = false;
00167     }
00168     else if(key == 'v')
00169     {
00170         visualize = (visualize+1)%3;
00171     }
00172     else if(key == 'l')
00173     {
00174         if(multi_marker_bundle->Load("mmarker.xml", FILE_FORMAT_XML))
00175         {
00176             cout<<"Multi marker loaded"<<endl;
00177             multi_marker_init->PointCloudCopy(multi_marker_bundle);
00178             optimize_done = true;
00179         }
00180         else
00181             cout<<"Cannot load multi marker"<<endl;
00182     }
00183     else if(key == 's')
00184     {
00185         if(multi_marker_bundle->Save("mmarker.xml", FILE_FORMAT_XML))
00186             cout<<"Multi marker saved"<<endl;
00187         else
00188             cout<<"Cannot save multi marker"<<endl;
00189     }
00190     else if(key == 'p')
00191     {
00192         add_measurement=true;
00193     }
00194     else if(key == 'o')
00195     {
00196         optimize=true;
00197     }
00198     else return key;
00199 
00200     return 0;
00201 }
00202 
00203 int main(int argc, char *argv[])
00204 {
00205     try {
00206         // Output usage message
00207         std::string filename(argv[0]);
00208         filename = filename.substr(filename.find_last_of('\\') + 1);
00209         std::cout << "SampleMultiMarkerBundle" << std::endl;
00210         std::cout << "=======================" << std::endl;
00211         std::cout << std::endl;
00212         std::cout << "Description:" << std::endl;
00213         std::cout << "  This is an example of how to use the 'MultiMarkerBundle' class to" << std::endl;
00214         std::cout << "  automatically deduce and optimize 'MultiMarker' setups." << std::endl;
00215         std::cout << std::endl;
00216         std::cout << "Usage:" << std::endl;
00217         std::cout << "  " << filename << " [device]" << std::endl;
00218         std::cout << std::endl;
00219         std::cout << "    device    integer selecting device from enumeration list (default 0)" << std::endl;
00220         std::cout << "              highgui capture devices are prefered" << std::endl;
00221         std::cout << std::endl;
00222         std::cout << "Keyboard Shortcuts:" << std::endl;
00223         std::cout << "  l: load marker configuration from mmarker.txt" << std::endl;
00224         std::cout << "  s: save marker configuration to mmarker.txt" << std::endl;
00225         std::cout << "  r: reset marker configuration" << std::endl;
00226         std::cout << "  p: add measurement" << std::endl;
00227         std::cout << "  o: optimize bundle" << std::endl;
00228         std::cout << "  q: quit" << std::endl;
00229         std::cout << std::endl;
00230 
00231         // Initialise CvTestbed
00232         CvTestbed::Instance().SetVideoCallback(videocallback);
00233         CvTestbed::Instance().SetKeyCallback(keycallback);
00234 
00235         // Enumerate possible capture plugins
00236         CaptureFactory::CapturePluginVector plugins = CaptureFactory::instance()->enumeratePlugins();
00237         if (plugins.size() < 1) {
00238             std::cout << "Could not find any capture plugins." << std::endl;
00239             return 0;
00240         }
00241 
00242         // Display capture plugins
00243         std::cout << "Available Plugins: ";
00244         outputEnumeratedPlugins(plugins);
00245         std::cout << std::endl;
00246 
00247         // Enumerate possible capture devices
00248         CaptureFactory::CaptureDeviceVector devices = CaptureFactory::instance()->enumerateDevices();
00249         if (devices.size() < 1) {
00250             std::cout << "Could not find any capture devices." << std::endl;
00251             return 0;
00252         }
00253 
00254         // Check command line argument for which device to use
00255         int selectedDevice = defaultDevice(devices);
00256         if (argc > 1) {
00257             selectedDevice = atoi(argv[1]);
00258         }
00259         if (selectedDevice >= (int)devices.size()) {
00260             selectedDevice = defaultDevice(devices);
00261         }
00262         
00263         // Display capture devices
00264         std::cout << "Enumerated Capture Devices:" << std::endl;
00265         outputEnumeratedDevices(devices, selectedDevice);
00266         std::cout << std::endl;
00267         
00268         // Create capture object from camera
00269         Capture *cap = CaptureFactory::instance()->createCapture(devices[selectedDevice]);
00270         std::string uniqueName = devices[selectedDevice].uniqueName();
00271 
00272         // Handle capture lifecycle and start video capture
00273         // Note that loadSettings/saveSettings are not supported by all plugins
00274         if (cap) {
00275             std::stringstream settingsFilename;
00276             settingsFilename << "camera_settings_" << uniqueName << ".xml";
00277             calibrationFilename << "camera_calibration_" << uniqueName << ".xml";
00278             
00279             cap->start();
00280             cap->setResolution(640, 480);
00281             
00282             if (cap->loadSettings(settingsFilename.str())) {
00283                 std::cout << "Loading settings: " << settingsFilename.str() << std::endl;
00284             }
00285 
00286             std::stringstream title;
00287             title << "SampleMultiMarkerBundle (" << cap->captureDevice().captureType() << ")";
00288 
00289             CvTestbed::Instance().StartVideo(cap, title.str().c_str());
00290 
00291             if (cap->saveSettings(settingsFilename.str())) {
00292                 std::cout << "Saving settings: " << settingsFilename.str() << std::endl;
00293             }
00294 
00295             cap->stop();
00296             delete cap;
00297         }
00298         else if (CvTestbed::Instance().StartVideo(0, argv[0])) {
00299         }
00300         else {
00301             std::cout << "Could not initialize the selected capture backend." << std::endl;
00302         }
00303 
00304         return 0;
00305     }
00306     catch (const std::exception &e) {
00307         std::cout << "Exception: " << e.what() << endl;
00308     }
00309     catch (...) {
00310         std::cout << "Exception: unknown" << std::endl;
00311     }
00312 }


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02