vslam_system: run_object.cpp Source File

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 
00036 //
00037 // runs a sequence of stereo images into an SBA engine for close-up objects
00038 // visualize in rviz
00039 //
00040 
00041 #include <ros/ros.h>
00042 #include <visualization_msgs/Marker.h>
00043 #include <visualization_msgs/MarkerArray.h>
00044 #include <geometry_msgs/Point.h>
00045 
00046 #include <vslam_system/vslam.h>
00047 #include <posest/pe3d.h>
00048 #include <sba/sba.h>
00049 #include <sba/sba_file_io.h>
00050 #include <frame_common/frame.h>
00051 #include <boost/shared_ptr.hpp>
00052 #include <cstdio>
00053 #include <fstream>
00054 #include <dirent.h>
00055 #include <fnmatch.h>
00056 
00057 #include <opencv/highgui.h>
00058 
00059 using namespace std;
00060 using namespace sba;
00061 using namespace frame_common;
00062 using namespace Eigen;
00063 
00064 // visual output of matches
00065 #define VISMATCH
00066 
00067 // elapsed time in milliseconds
00068 #include <sys/time.h>
00069 static double mstime()
00070 {
00071   timeval tv;
00072   gettimeofday(&tv,NULL);
00073   long long ts = tv.tv_sec;
00074   ts *= 1000000;
00075   ts += tv.tv_usec;
00076   return (double)ts*.001;
00077 }
00078 
00079 // Names of left and right files in directory (with wildcards)
00080 char *lreg, *rreg, *dreg;
00081 
00082 // Filters for scandir
00083 int getleft(struct dirent const *entry)
00084 {
00085   if (!fnmatch(lreg,entry->d_name,0))
00086     return 1;
00087   return 0;
00088 }
00089 
00090 int getright(struct dirent const *entry)
00091 {
00092   if (!fnmatch(rreg,entry->d_name,0))
00093     return 1;
00094   return 0;
00095 }
00096 
00097 
00098 int getidir(struct dirent const *entry)
00099 {
00100   if (!fnmatch(dreg,entry->d_name,0))
00101     return 1;
00102   return 0;
00103 }
00104 
00105 
00106 
00107 // draw the graph on rviz
00108 void
00109 drawgraph(const SysSBA &sba, const ros::Publisher &cam_pub, const ros::Publisher &pt_pub, int dec
00110           /*SysSPA &spa, ros::Publisher &cst_pub, ros::Publisher &link_pub*/)
00111 {
00112   visualization_msgs::Marker cammark, ptmark, cstmark;
00113   cammark.header.frame_id = "/pgraph";
00114   cammark.header.stamp = ros::Time();
00115   cammark.ns = "pgraph";
00116   cammark.id = 0;
00117   cammark.action = visualization_msgs::Marker::ADD;
00118   cammark.pose.position.x = 0;
00119   cammark.pose.position.y = 0;
00120   cammark.pose.position.z = 0;
00121   cammark.pose.orientation.x = 0.0;
00122   cammark.pose.orientation.y = 0.0;
00123   cammark.pose.orientation.z = 0.0;
00124   cammark.pose.orientation.w = 1.0;
00125   cammark.scale.x = 0.001;
00126   cammark.scale.y = 0.001;
00127   cammark.scale.z = 0.001;
00128   cammark.color.r = 0.0f;
00129   cammark.color.g = 1.0f;
00130   cammark.color.b = 1.0f;
00131   cammark.color.a = 1.0f;
00132   cammark.lifetime = ros::Duration();
00133   cammark.type = visualization_msgs::Marker::LINE_LIST;
00134 
00135   ptmark = cammark;
00136   ptmark.color.r = 1.0f;
00137   ptmark.color.g = 0.0f;
00138   ptmark.color.b = 0.0f;
00139   ptmark.color.a = 0.5f;
00140   ptmark.scale.x = 0.002;
00141   ptmark.scale.y = 0.002;
00142   ptmark.scale.z = 0.002;
00143   ptmark.type = visualization_msgs::Marker::POINTS;
00144 
00145   cstmark = cammark;
00146   cstmark.color.r = 1.0f;
00147   cstmark.color.g = 1.0f;
00148   cstmark.color.b = 0.0f;
00149   cstmark.color.a = 1.0f;
00150   cstmark.scale.x = 0.03;
00151   cstmark.scale.y = 0.1;
00152   cstmark.scale.z = 0.1;
00153   cstmark.type = visualization_msgs::Marker::LINE_LIST;
00154 
00155 
00156   // draw points, decimated
00157   int npts = sba.tracks.size();
00158 
00159   cout << "Number of points to draw: " << npts << endl;
00160   if (npts <= 0) return;
00161 
00162 
00163   ptmark.points.resize(npts/dec+1);
00164   for (int i=0, ii=0; i<npts; i+=dec, ii++)
00165     {
00166       const Vector4d &pt = sba.tracks[i].point;
00167       ptmark.points[ii].x = pt(0);
00168       ptmark.points[ii].y = pt(2);
00169       ptmark.points[ii].z = -pt(1);
00170     }
00171 
00172   // draw cameras
00173   int ncams = sba.nodes.size();
00174   cammark.points.resize(ncams*6);
00175   for (int i=0, ii=0; i<ncams; i++)
00176     {
00177       const Node &nd = sba.nodes[i];
00178       Vector3d opt;
00179       Matrix<double,3,4> tr;
00180       transformF2W(tr,nd.trans,Quaternion<double>(nd.qrot));
00181 
00182       cammark.points[ii].x = nd.trans.x();
00183       cammark.points[ii].y = nd.trans.z();
00184       cammark.points[ii++].z = -nd.trans.y();
00185       opt = tr*Vector4d(0,0,0.1,1);
00186       cammark.points[ii].x = opt.x();
00187       cammark.points[ii].y = opt.z();
00188       cammark.points[ii++].z = -opt.y();
00189 
00190       cammark.points[ii].x = nd.trans.x();
00191       cammark.points[ii].y = nd.trans.z();
00192       cammark.points[ii++].z = -nd.trans.y();
00193       opt = tr*Vector4d(0.05,0,0,1);
00194       cammark.points[ii].x = opt.x();
00195       cammark.points[ii].y = opt.z();
00196       cammark.points[ii++].z = -opt.y();
00197 
00198       cammark.points[ii].x = nd.trans.x();
00199       cammark.points[ii].y = nd.trans.z();
00200       cammark.points[ii++].z = -nd.trans.y();
00201       opt = tr*Vector4d(0,0.05,0,1);
00202       cammark.points[ii].x = opt.x();
00203       cammark.points[ii].y = opt.z();
00204       cammark.points[ii++].z = -opt.y();
00205     }
00206 
00207 #if 0
00208   // draw SPA constraints
00209   int ncons = spa.p2cons.size();
00210   cstmark.points.resize(ncons*6);
00211 
00212   for (int i=0, ii=0; i<ncons; i++)
00213     {
00214       ConP2 &con = spa.p2cons[i];
00215       Node &nd0 = spa.nodes[con.ndr];
00216       Node &nd1 = spa.nodes[con.nd1];
00217 
00218       Node &nd = spa.nodes[i];
00219       Vector3d opt;
00220       Matrix<double,3,4> tr;
00221       transformF2W(tr,nd.trans,Quaternion<double>(nd.qrot));
00222 
00223       cstmark.points[ii].x = nd.trans.x();
00224       cstmark.points[ii].y = nd.trans.z();
00225       cstmark.points[ii++].z = -nd.trans.y();
00226       opt = tr*Vector4d(0,0,0.3,1);
00227       cstmark.points[ii].x = opt.x();
00228       cstmark.points[ii].y = opt.z();
00229       cstmark.points[ii++].z = -opt.y();
00230 
00231       cstmark.points[ii].x = nd.trans.x();
00232       cstmark.points[ii].y = nd.trans.z();
00233       cstmark.points[ii++].z = -nd.trans.y();
00234       opt = tr*Vector4d(0.2,0,0,1);
00235       cstmark.points[ii].x = opt.x();
00236       cstmark.points[ii].y = opt.z();
00237       cstmark.points[ii++].z = -opt.y();
00238 
00239       cstmark.points[ii].x = nd.trans.x();
00240       cstmark.points[ii].y = nd.trans.z();
00241       cstmark.points[ii++].z = -nd.trans.y();
00242       opt = tr*Vector4d(0,0.1,0,1);
00243       cstmark.points[ii].x = opt.x();
00244       cstmark.points[ii].y = opt.z();
00245       cstmark.points[ii++].z = -opt.y();
00246 
00247 #if 0
00248       cstmark.points[ii].x = nd0.trans.x();
00249       cstmark.points[ii].y= nd0.trans.z();
00250       cstmark.points[ii++].z = -nd0.trans.y();
00251       cstmark.points[ii].x = nd1.trans.x();
00252       cstmark.points[ii].y = nd1.trans.z();
00253       cstmark.points[ii++].z = -nd1.trans.y();
00254 #endif
00255     }
00256 #endif // SPA
00257 
00258   cam_pub.publish(cammark);
00259   pt_pub.publish(ptmark);
00260   //cst_pub.publish(cstmark);
00261 }
00262 
00263 
00264 
00265 // main loop
00266 
00267 int main(int argc, char** argv)
00268 {
00269   if (argc < 5)
00270     {
00271       printf("Args are: <param file> <image dir> <left image file template> <right image file template> "
00272              "<vocab tree file> <vocab weights file> <calonder trees file>\n");
00273       exit(0);
00274     }
00275 
00276   // get camera parameters, in the form: fx fy cx cy tx
00277   fstream fstr;
00278   fstr.open(argv[1],fstream::in);
00279   if (!fstr.is_open())
00280     {
00281       printf("Can't open camera file %s\n",argv[1]);
00282       exit(0);
00283     }
00284   CamParams camp;
00285   fstr >> camp.fx;
00286   fstr >> camp.fy;
00287   fstr >> camp.cx;
00288   fstr >> camp.cy;
00289   fstr >> camp.tx;
00290 
00291   cout << "Cam params: " << camp.fx << " " << camp.fy << " " << camp.cx
00292        << " " << camp.cy << " " << camp.tx << endl;
00293 
00294 
00295   // set up directories
00296   struct dirent **lims, **rims, **dirs;
00297   int nlim, nrim, ndirs;
00298   string dname = argv[2];
00299   if (!dname.compare(dname.size()-1,1,"/")) // see if slash at end
00300     dname.erase(dname.size()-1);
00301 
00302   string dirfn = dname.substr(dname.rfind("/")+1);
00303   string tdir = dname.substr(0,dname.rfind("/")+1);
00304   cout << "Top directory is " << tdir << endl;
00305   cout << "Search directory name is " << dirfn << endl;
00306   dreg = (char *)dirfn.c_str();
00307 
00308   ndirs = scandir(tdir.c_str(),&dirs,getidir,alphasort);
00309   printf("Found %d directories\n", ndirs);
00310   printf("%s\n",dirs[0]->d_name);
00311 
00312   // set up vslam structures
00313   vslam::VslamSystem vslam(argv[5], argv[6]);
00314   typedef cv::CalonderDescriptorExtractor<float> Calonder;
00315   vslam.frame_processor_.setFrameDescriptor(new Calonder(argv[7]));
00316   vslam.setNDisp(192);
00317   vslam.setPlaceInliers(80);
00318   vslam.setKeyInliers(80);
00319   vslam.setKeyDist(.01);
00320   vslam.setKeyAngle(.005);
00321 
00322 
00323 #if 0
00324   SysSPA spa;                   // SPA system
00325   int spaFrameId = -1;          // current frame of SPA system
00326   int ndi0 = 0;                 // current node of SPA system
00327 #endif
00328   
00329   // set up markers for visualization
00330   ros::init(argc, argv, "VisBundler");
00331   ros::NodeHandle nh ("~");
00332   ros::Publisher pt_pub = nh.advertise<visualization_msgs::Marker>("points", 0);
00333   ros::Publisher cam_pub = nh.advertise<visualization_msgs::Marker>("cameras", 0);
00334   ros::Publisher cst_pub = nh.advertise<visualization_msgs::Marker>("constraints", 0);
00335   ros::Publisher link_pub = nh.advertise<visualization_msgs::Marker>("links", 0);
00336 
00337   // for RANSAC
00338   srand(mstime());
00339 
00340   // window
00341 #ifdef VISMATCH  
00342   const std::string window_name = "VO tracks";
00343   cv::namedWindow(window_name,0);
00344   cv::Mat display;
00345 #endif
00346 
00347   int iter = 0;
00348   lreg = argv[3];
00349   rreg = argv[4];
00350 
00351 
00352   // loop over directories
00353   for (int dd=0; dd<ndirs; dd++)
00354     {
00355       char dir[2048];
00356       sprintf(dir,"%s%s",tdir.c_str(),dirs[dd]->d_name);
00357       printf("Current directory: %s\n", dir);
00358 
00359       // get left/right image file names, sorted
00360       nlim = scandir(dir,&lims,getleft,alphasort);
00361       printf("Found %d left images\n", nlim);
00362       printf("%s\n",lims[0]->d_name);
00363 
00364       nrim = scandir(dir,&rims,getright,alphasort);
00365       printf("Found %d right images\n", nrim);
00366       printf("%s\n",rims[0]->d_name);
00367 
00368       if (nlim != nrim)
00369         {
00370           printf("Number of left/right images does not match: %d vs. %d\n", nlim, nrim);
00371           exit(0);
00372         }
00373 
00374 
00375 
00376       // loop over each stereo pair, adding it to the system
00377       for (int ii=0; ii<nlim; iter++, ii++)
00378         {
00379           // Load images
00380           char fn[2048];
00381           sprintf(fn,"%s/%s",dir,lims[ii]->d_name);
00382           cv::Mat image1 = cv::imread(fn,0);
00383           sprintf(fn,"%s/%s",dir,rims[ii]->d_name);
00384           cv::Mat image1r = cv::imread(fn,0);
00385 
00386 #if 0
00387           printf("Image size: %d x %d\n", image1.cols, image1.rows);
00388           printf("Image size: %d x %d\n", image1r.cols, image1r.rows);
00389 #endif
00390 
00391           if (image1.rows == 0 || image1r.rows == 0)
00392             exit(0);
00393 
00394           // Add stereo pair to the system
00395           bool is_keyframe = vslam.addFrame(camp, image1, image1r);
00396 
00397 
00398           if (is_keyframe)
00399             {
00401               int n = vslam.sba_.nodes.size();
00402               int np = vslam.sba_.tracks.size();
00403 
00404               // draw graph
00405               //              cout << "drawing with " << n << " nodes and " << np << " points..." << endl << endl;
00406 #if 1
00407               if (n%2 == 0)
00408                 drawgraph(vslam.sba_, cam_pub, pt_pub, 1/*, spa, cst_pub, link_pub*/); // every nth point
00409 #endif
00410 
00411               // write file out
00412               if (0 && n > 10 && n%500 == 0)
00413                 {
00414                   char fn[1024];
00415                   sprintf(fn,"newcollege%d", n);
00416                   sba::writeLourakisFile(fn, vslam.sba_);
00417                   vslam.sba_.doSBA(1,1.0e-4,0);
00418                   sba::writeSparseA(fn, vslam.sba_);
00419                 }
00420 
00421 #if 1
00422               int nnsba = 10;
00423               if (n > 4 && n%nnsba == 0)
00424                 {
00425                   cout << "Running large SBA" << endl;
00426                   vslam.refine();
00427                 }
00428 #endif
00429             }
00430 
00431 #ifdef VISMATCH
00432           // Visualize
00433           if (1)
00434             {
00435               drawVOtracks(image1, vslam.vo_.frames, display);
00436               cv::imshow(window_name, display);
00437               cv::waitKey(10);
00438             }
00439 #endif
00440 
00441           if (!nh.ok())
00442             return 0;
00443         }
00444     }
00445   return 0;
00446 }