SamplePointcloud.cpp
Go to the documentation of this file.
00001 #include "CvTestbed.h"
00002 #include "GlutViewer.h"
00003 #include "SfM.h"
00004 #include "Shared.h"
00005 using namespace alvar;
00006 using namespace std;
00007 
00008 const double marker_size=1;
00009 bool init=true;
00010 SimpleSfM *sfm;
00011 std::stringstream calibrationFilename;
00012 
00013 // Own drawable 3D cross on features
00014 struct OwnDrawable : public Drawable {
00015     virtual void Draw() {
00016         const double scale = 0.2;
00017         glPushMatrix();
00018         glMultMatrixd(gl_mat);
00019         glColor3d(color[0], color[1], color[2]);
00020         glBegin(GL_LINES);
00021             glVertex3f(0.0, 0.0, -scale);
00022             glVertex3f(0.0, 0.0, scale);
00023             glVertex3f(0.0, -scale, 0.0);
00024             glVertex3f(0.0, scale, 0.0);
00025             glVertex3f(-scale, 0.0, 0.0);
00026             glVertex3f(scale, 0.0, 0.0);
00027         glEnd();
00028         glPopMatrix();
00029     }
00030 };
00031 Drawable d_marker;
00032 OwnDrawable d_points[1000];
00033 int own_drawable_count;
00034 
00035 bool reset=false;
00036 void videocallback(IplImage *image)
00037 {
00038     static IplImage *rgb = 0;
00039     static IplImage* bg_image = 0;
00040 
00041     bool flip_image = (image->origin?true:false);
00042     if (flip_image) {
00043         cvFlip(image);
00044         image->origin = !image->origin;
00045     }
00046 
00047     if (init) {
00048         init = false;
00049         sfm->Clear();
00050         cout<<"Loading calibration: "<<calibrationFilename.str();
00051         if (sfm->GetCamera()->SetCalib(calibrationFilename.str().c_str(), image->width, image->height)) {
00052             cout<<" [Ok]"<<endl;
00053         } else {
00054             sfm->GetCamera()->SetRes(image->width, image->height);
00055             cout<<" [Fail]"<<endl;
00056         }
00057         double p[16];
00058         sfm->GetCamera()->GetOpenglProjectionMatrix(p,image->width,image->height);
00059         GlutViewer::SetGlProjectionMatrix(p);
00060         d_marker.SetScale(marker_size*2);
00061         rgb = CvTestbed::Instance().CreateImageWithProto("RGB", image, 0, 3);
00062         CvTestbed::Instance().ToggleImageVisible(0, 0);
00063         bg_image = CvTestbed::Instance().CreateImage("BG texture", cvSize(512,512),8, 3);
00064         bg_image->origin = 0;
00065 
00066         sfm->SetScale(10);
00067         if (sfm->AddMultiMarker("mmarker.xml", FILE_FORMAT_XML)) {
00068             std::cout<<"Using MultiMarker defined in mmarker.xml."<<std::endl;
00069         } else {
00070             std::cout<<"Couldn't load mmarker.xml. Using default 'SampleMultiMarker' setup."<<std::endl;
00071             Pose pose;
00072             pose.Reset();
00073             sfm->AddMarker(0, marker_size*2, pose);
00074             pose.SetTranslation(-marker_size*2.5, +marker_size*1.5, 0);
00075             sfm->AddMarker(1, marker_size, pose);
00076             pose.SetTranslation(+marker_size*2.5, +marker_size*1.5, 0);
00077             sfm->AddMarker(2, marker_size, pose);
00078             pose.SetTranslation(-marker_size*2.5, -marker_size*1.5, 0);
00079             sfm->AddMarker(3, marker_size, pose);
00080             pose.SetTranslation(+marker_size*2.5, -marker_size*1.5, 0);
00081             sfm->AddMarker(4, marker_size, pose);
00082         }
00083         sfm->SetResetPoint();
00084     }
00085     if (reset) {
00086         sfm->Reset();
00087         reset = false;
00088     }
00089 
00090     //if (sfm->UpdateRotationsOnly(image)) {
00091     //if (sfm->UpdateTriangulateOnly(image)) {
00092     if (sfm->Update(image, false, true, 7.f, 15.f)) {
00093         // Draw the camera (The GlutViewer has little weirdness here...)q
00094         Pose pose = *(sfm->GetPose());
00095         double gl[16];
00096         pose.GetMatrixGL(gl, true);
00097         GlutViewer::SetGlModelviewMatrix(gl);
00098         pose.Invert();
00099         pose.GetMatrixGL(d_marker.gl_mat, false);
00100         GlutViewer::DrawableClear();
00101         GlutViewer::DrawableAdd(&d_marker);
00102         own_drawable_count=0;
00103 
00104         // Draw features
00105         std::map<int, SimpleSfM::Feature>::iterator iter;
00106         iter = sfm->container.begin();
00107         for(;iter != sfm->container.end(); iter++) {
00108                         if (sfm->container_triangulated.find(iter->first) !=  sfm->container_triangulated.end()) continue;
00109             if (iter->second.has_p3d) 
00110             { 
00111                 if (own_drawable_count < 1000) {
00112                     memset(d_points[own_drawable_count].gl_mat, 0, 16*sizeof(double));
00113                     d_points[own_drawable_count].gl_mat[0]  = 1;
00114                     d_points[own_drawable_count].gl_mat[5]  = 1;
00115                     d_points[own_drawable_count].gl_mat[10] = 1;
00116                     d_points[own_drawable_count].gl_mat[15] = 1;
00117                     d_points[own_drawable_count].gl_mat[12] = iter->second.p3d.x;
00118                     d_points[own_drawable_count].gl_mat[13] = iter->second.p3d.y;
00119                     d_points[own_drawable_count].gl_mat[14] = iter->second.p3d.z;
00120                     if (iter->second.type_id == 0) d_points[own_drawable_count].SetColor(1,0,0);
00121                     else d_points[own_drawable_count].SetColor(0,1,0);
00122                     GlutViewer::DrawableAdd(&(d_points[own_drawable_count]));
00123                     own_drawable_count++;
00124                 }
00125             }
00126         }
00127 
00128                 // Draw triangulated features
00129         iter = sfm->container_triangulated.begin();
00130         for(;iter != sfm->container_triangulated.end(); iter++) {
00131             if (iter->second.has_p3d) 
00132             { 
00133                 if (own_drawable_count < 1000) {
00134                     memset(d_points[own_drawable_count].gl_mat, 0, 16*sizeof(double));
00135                     d_points[own_drawable_count].gl_mat[0]  = 1;
00136                     d_points[own_drawable_count].gl_mat[5]  = 1;
00137                     d_points[own_drawable_count].gl_mat[10] = 1;
00138                     d_points[own_drawable_count].gl_mat[15] = 1;
00139                     d_points[own_drawable_count].gl_mat[12] = iter->second.p3d.x;
00140                     d_points[own_drawable_count].gl_mat[13] = iter->second.p3d.y;
00141                     d_points[own_drawable_count].gl_mat[14] = iter->second.p3d.z;
00142                     /*if (iter->second.type_id == 0) d_points[own_drawable_count].SetColor(1,0,1);
00143                     else*/ d_points[own_drawable_count].SetColor(0,0,1);
00144                     GlutViewer::DrawableAdd(&(d_points[own_drawable_count]));
00145                     own_drawable_count++;
00146                 }
00147             }
00148         }
00149     }
00150     if (image->nChannels == 1) cvCvtColor(image, rgb, CV_GRAY2RGB);
00151     else if (image->nChannels == 3) cvCopy(image, rgb);
00152 
00153     // Draw video on GlutViewer background
00154     cvResize(rgb, bg_image);
00155     GlutViewer::SetVideo(bg_image);
00156 
00157     // Draw debug info to the rgb
00158     sfm->Draw(rgb);
00159 
00160     if (flip_image) {
00161         cvFlip(image);
00162         image->origin = !image->origin;
00163     }
00164 }
00165 
00166 
00167 int keycallback(int key)
00168 {
00169     if(key == 'r')
00170     {
00171         reset = true;
00172     }
00173     else return key;
00174 
00175     return 0;
00176 }
00177 
00178 int main(int argc, char *argv[])
00179 {
00180     try {
00181         // Output usage message
00182         std::string filename(argv[0]);
00183         filename = filename.substr(filename.find_last_of('\\') + 1);
00184         std::cout << "SamplePointcloud" << std::endl;
00185         std::cout << "================" << std::endl;
00186         std::cout << std::endl;
00187         std::cout << "Description:" << std::endl;
00188         std::cout << "  This example shows simple structure from motion approach that can be "<< std::endl;
00189         std::cout << "  used to track environment beyond an multimarker setup. To get this   "<< std::endl;
00190         std::cout << "  example work properly be sure to calibrate your camera and tune it   "<< std::endl;
00191         std::cout << "  to have fast framerate without motion blur.                          "<< std::endl;
00192         std::cout << std::endl;
00193         std::cout << "  There are two possible approaches Update() and UpdateRotationsOnly()."<< std::endl;
00194         std::cout << "  By default the Update() is used but you can easily uncomment the     "<< std::endl;
00195         std::cout << "  other one if needed."<< std::endl;
00196         std::cout << std::endl;
00197         std::cout << "Usage:" << std::endl;
00198         std::cout << "  " << filename << " [device]" << std::endl;
00199         std::cout << std::endl;
00200         std::cout << "    device    integer selecting device from enumeration list (default 0)" << std::endl;
00201         std::cout << "              highgui capture devices are prefered" << std::endl;
00202         std::cout << std::endl;
00203         std::cout << "Keyboard Shortcuts:" << std::endl;
00204         std::cout << "  r: reset" << std::endl;
00205         std::cout << "  q: quit" << std::endl;
00206         std::cout << std::endl;
00207 
00208         // Initialise GlutViewer and CvTestbed
00209         GlutViewer::Start(argc, argv, 640, 480, 100);
00210         CvTestbed::Instance().SetKeyCallback(keycallback);
00211         CvTestbed::Instance().SetVideoCallback(videocallback);
00212         sfm = new SimpleSfM();
00213 
00214         // Enumerate possible capture plugins
00215         CaptureFactory::CapturePluginVector plugins = CaptureFactory::instance()->enumeratePlugins();
00216         if (plugins.size() < 1) {
00217             std::cout << "Could not find any capture plugins." << std::endl;
00218             return 0;
00219         }
00220 
00221         // Display capture plugins
00222         std::cout << "Available Plugins: ";
00223         outputEnumeratedPlugins(plugins);
00224         std::cout << std::endl;
00225 
00226         // Enumerate possible capture devices
00227         CaptureFactory::CaptureDeviceVector devices = CaptureFactory::instance()->enumerateDevices();
00228         if (devices.size() < 1) {
00229             std::cout << "Could not find any capture devices." << std::endl;
00230             return 0;
00231         }
00232 
00233         // Check command line argument for which device to use
00234         int selectedDevice = defaultDevice(devices);
00235         if (argc > 1) {
00236             selectedDevice = atoi(argv[1]);
00237         }
00238         if (selectedDevice >= (int)devices.size()) {
00239             selectedDevice = defaultDevice(devices);
00240         }
00241         
00242         // Display capture devices
00243         std::cout << "Enumerated Capture Devices:" << std::endl;
00244         outputEnumeratedDevices(devices, selectedDevice);
00245         std::cout << std::endl;
00246         
00247         // Create capture object from camera
00248         Capture *cap = CaptureFactory::instance()->createCapture(devices[selectedDevice]);
00249         std::string uniqueName = devices[selectedDevice].uniqueName();
00250 
00251         // Handle capture lifecycle and start video capture
00252         // Note that loadSettings/saveSettings are not supported by all plugins
00253         if (cap) {
00254             std::stringstream settingsFilename;
00255             settingsFilename << "camera_settings_" << uniqueName << ".xml";
00256             calibrationFilename << "camera_calibration_" << uniqueName << ".xml";
00257             
00258             cap->start();
00259             cap->setResolution(640, 480);
00260             
00261             if (cap->loadSettings(settingsFilename.str())) {
00262                 std::cout << "Loading settings: " << settingsFilename.str() << std::endl;
00263             }
00264 
00265             std::stringstream title;
00266             title << "SamplePointcloud (" << cap->captureDevice().captureType() << ")";
00267 
00268             CvTestbed::Instance().StartVideo(cap, title.str().c_str());
00269 
00270             if (cap->saveSettings(settingsFilename.str())) {
00271                 std::cout << "Saving settings: " << settingsFilename.str() << std::endl;
00272             }
00273 
00274             cap->stop();
00275             delete cap; cap = NULL;
00276         }
00277         else if (CvTestbed::Instance().StartVideo(0, argv[0])) {
00278         }
00279         else {
00280             std::cout << "Could not initialize the selected capture backend." << std::endl;
00281         }
00282         delete sfm; sfm = NULL;
00283 
00284         return 0;
00285     }
00286     catch (const std::exception &e) {
00287         std::cout << "Exception: " << e.what() << endl;
00288     }
00289     catch (...) {
00290         std::cout << "Exception: unknown" << std::endl;
00291     }
00292 }


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