ndt_fuser_from_depth.cpp
Go to the documentation of this file.
00001 // Evaluation using TUM data set.
00002 #include <ndt_fuser.h>
00003 
00004 #include <iostream>
00005 #include <boost/thread/thread.hpp>
00006 #include <boost/program_options.hpp>
00007 
00008 #include <pointcloud_utils.h>
00009 #include <cv.h>
00010 #include <highgui.h>
00011 #include <pcl/io/io.h>
00012 #include <pcl/io/pcd_io.h>
00013 #include <pcl/common/transforms.h>
00014 
00015 using namespace pcl;
00016 using namespace cv;
00017 using namespace std;
00018 using namespace lslgeneric;
00019 
00020 namespace po = boost::program_options;
00021 
00022 inline
00023 std::vector<std::string> split_string(const std::string &str, const std::string &separator)
00024 {
00025     std::string::size_type it = 0;
00026     std::vector<std::string> values;
00027     bool stop = false;
00028     while (!stop)
00029     {
00030         std::string::size_type start = it;
00031         it = str.find(separator, it+1); // Gets the next comma.
00032         if (it == std::string::npos) // didn't find any new comma, return the remaining string.
00033         {
00034             values.push_back(str.substr(start, std::string::npos));
00035             stop = true;
00036         }
00037         else
00038         {
00039             it++; // We want the character after the comma.
00040             values.push_back(str.substr(start, it-start-1));
00041         }
00042     }
00043     return values;
00044 }
00045 
00046 
00047 vector<pair<string,string> > load_associations(const string &association_file, vector<string> &timestamps, int skip_nb)
00048 {
00049     // Generated by associate.py rgb.txt depth.txt (always this order)
00050     vector<pair<string, string> > ret;
00051     std::ifstream file(association_file.c_str());
00052     std::string line;
00053     bool skip = false;
00054     int skip_counter = 0;
00055     while(std::getline (file,line))
00056     {
00057         vector<string> splits = split_string(line, std::string(" "));
00058         if (splits.size() < 4)
00059         {
00060             cerr << "couldn't parse : " << line << endl;
00061             continue;
00062         }
00063         if (!skip)
00064         {
00065             ret.push_back(pair<string,string>(splits[1], splits[3])); // rgb, depth
00066             timestamps.push_back(splits[0]); // Timestamp for the rgb.
00067         }
00068 /*
00069         if (skip_counter > skip_nb)
00070         {
00071             skip = false;
00072             skip_counter = 0;
00073         }
00074         else
00075         {
00076             skip = true;
00077             skip_counter++;
00078         }
00079         */
00080     }
00081     return ret;
00082 }
00083 double getDoubleTime()
00084 {
00085     struct timeval time;
00086     gettimeofday(&time,NULL);
00087     return time.tv_sec + time.tv_usec * 1e-6;
00088 }
00089 
00090 
00091 
00092 
00093 int main(int argc, char** argv)
00094 {
00095     cout << "-----------------------------------------------------------------" << endl;
00096     cout << "Evaluations of frame to model tracking with ndt d2d registration" << endl;
00097     cout << "-----------------------------------------------------------------" << endl;
00098 
00099     string association_name;
00100     string output_name;
00101     string times_name;
00102     double scale, max_inldist_xy, max_inldist_z;
00103     Eigen::Matrix<double,6,1> pose_increment_v;
00104     int nb_ransac;
00105     int nb_points;
00106     size_t support_size;
00107     size_t max_nb_frames;
00108     double max_var;
00109     double current_res;
00110     int delay_vis;
00111     double img_scale;
00112     double detector_thresh;
00113     int skip_nb = 0;
00114     int freiburg_camera_nb;
00115     double max_kp_dist, min_kp_dist;
00116     double trim_factor;
00117     double resolution;
00118     double size_x,size_y,size_z;
00119     Eigen::Affine3d pose_, T0;
00120     pose_.setIdentity();
00121     T0.setIdentity();
00122     
00123     po::options_description desc("Allowed options");
00124     desc.add_options()
00125     ("help", "produce help message")
00126     ("debug", "print debug output")
00127     ("visualize", "visualize the output")
00128     ("init-gt", "use the first pose of gt as the start pose of the transform track")
00129     ("association", po::value<string>(&association_name), "association file name (generated by association.py script)")
00130     ("output", po::value<string>(&output_name), "file name of the estimation output (to be used with the evaluation python script)")
00131     ("time_output", po::value<string>(&times_name), "file name for saving the per-frame processing time (to be used with the evaluation python script)")
00132     ("scale", po::value<double>(&scale)->default_value(0.0002), "depth scale (depth = scale * pixel value)")
00133     ("freiburg_camera_nb", po::value<int>(&freiburg_camera_nb)->default_value(1), "the camera used for the evaluation 1-> freiburg camera #1, 2-> freiburg camera #2")
00134     ("resolution", po::value<double>(&resolution)->default_value(0.1), "iresolution of the ndt model")
00135     ("map_size_x", po::value<double>(&size_x)->default_value(5.), "x size of the map")
00136     ("map_size_y", po::value<double>(&size_y)->default_value(5.), "y size of the map")
00137     ("map_size_z", po::value<double>(&size_z)->default_value(5.), "z size of the map")
00138     ;
00139 
00140 
00141     po::variables_map vm;
00142     po::store(po::parse_command_line(argc, argv, desc), vm);
00143     po::notify(vm);
00144 
00145     if (!vm.count("output") || !vm.count("association"))
00146     {
00147         cout << "Missing association / output file names.\n";
00148         cout << desc << "\n";
00149         return 1;
00150     }
00151     if (vm.count("help"))
00152     {
00153         cout << desc << "\n";
00154         return 1;
00155     }
00156     bool debug = vm.count("debug");
00157     bool visualize = vm.count("visualize");
00158 
00159     if (times_name == "")
00160     {
00161         times_name = output_name + ".times";
00162     }
00163     
00164     
00165     if (vm.count("init-gt"))
00166     {
00167         cout << "reading gt from groundtrut.txt\n";
00168         std::ifstream file("groundtruth.txt");
00169         std::string line;
00170         double dx,dy,dz,qx,qy,qz,qw;
00171         while(std::getline (file,line))
00172         {
00173             vector<string> splits = split_string(line, std::string(" "));
00174             if (splits.size() == 0) return -1;
00175             if(splits[0][0] == '#') continue;
00176             if(splits.size() < 8) continue;
00177             dx = atof(splits[1].c_str());
00178             dy = atof(splits[2].c_str());
00179             dz = atof(splits[3].c_str());
00180             qx = atof(splits[4].c_str());
00181             qy = atof(splits[5].c_str());
00182             qz = atof(splits[6].c_str());
00183             qw = atof(splits[7].c_str());
00184             break;
00185         }
00186         cerr<<"using init pose of "<<dx<<" "<<dy<<" "<<dz<<" "<<qx<<" "<<qy<<" "<<qz<<" "<<qw<<endl;
00187         Eigen::Quaterniond quat(qx,qy,qz,qw);
00188         pose_ = Eigen::Translation<double,3>(dx,dy,dz)*quat;
00189     }
00190 
00191     vector<string> timestamps;
00192     vector<pair<string,string> > associations = load_associations(association_name, timestamps, skip_nb);
00193     if (debug)
00194     {
00195         for (size_t i = 0; i < associations.size(); i++)
00196         {
00197             cout << "associations : " << associations[i].first << " <-> " << associations[i].second << endl;
00198         }
00199         cout << "loaded : " << associations.size() << " associations" << endl;
00200     }
00201 
00202     double fx, fy, cx, cy, ds;
00203     std::vector<double> dist(5);
00204     switch (freiburg_camera_nb)
00205     {
00206     case 1:
00207         fx = 517.3;
00208         fy = 516.5;
00209         cx = 318.6;
00210         cy = 255.3;
00211         dist[0] = 0.2624;
00212         dist[1] = -0.9531;
00213         dist[2] = -0.0054;
00214         dist[3] = 0.0026;
00215         dist[4] = 1.1633;
00216         ds = 1.035; // Depth scaling factor.
00217         break;
00218     case 2:
00219         fx = 520.9;
00220         fy = 521.0;
00221         cx = 325.1;
00222         cy = 249.7;
00223         dist[0] = 0.2312;
00224         dist[1] = -0.7849;
00225         dist[2] = -0.0033;
00226         dist[3] = -0.0001;
00227         dist[4] = 0.9172;
00228         ds = 1.031;
00229         break;
00230     default:
00231         cerr << "unknown camera number : " << freiburg_camera_nb << endl;
00232         return 1;
00233     }
00234 
00235 
00236     double t1, t2;
00237 
00238     lslgeneric::DepthCamera<pcl::PointXYZ> cameraparams(fx,fy,cx,cy,dist,ds*scale,false);
00239 
00240     typedef Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> EigenTransform;
00241     std::vector<EigenTransform, Eigen::aligned_allocator<EigenTransform> > transformVector;
00242 
00243     std::ofstream file_times(times_name.c_str(), ios::out);
00244     if (!file_times)
00245     {
00246         cerr << "Failed to open times file : " << times_name << endl;
00247         return -1;
00248     }
00249 
00250     file_times<<"times = [";
00251     NDTFuser<pcl::PointXYZ> fuser(resolution,size_x,size_y,size_z,visualize);
00252     pcl::PointCloud<pcl::PointXYZ> pc;
00253     cv::Mat depth_img;
00254 
00255     // Loop through all the associations pairs
00256     for (size_t i = 0; i < associations.size(); i++)
00257     {
00258         std::cout << "iter " << i << "(" << associations.size() << ")" << std::endl;
00259         depth_img = cv::imread(associations[i].second, CV_LOAD_IMAGE_ANYDEPTH); // CV_LOAD_IMAGE_ANYDEPTH is important to load the 16bits image
00260         t1 = getDoubleTime();
00261         if (i == 0)
00262         {
00263             cameraparams.setupDepthPointCloudLookUpTable(depth_img.size());
00264             cameraparams.convertDepthImageToPointCloud(depth_img,pc);
00265             fuser.initialize(pose_,pc);
00266             transformVector.push_back(pose_);
00267             t2 = getDoubleTime();
00268             file_times << t2-t1 <<", ";
00269             
00270             continue;
00271         }
00272             
00273         cameraparams.convertDepthImageToPointCloud(depth_img,pc);
00274         pose_ = fuser.update(T0,pc);
00275         transformVector.push_back(pose_);
00276 
00277         t2 = getDoubleTime();
00278         file_times << t2-t1 <<", ";
00279     }
00280     file_times<<"];\n";
00281     
00282     // Write out the transformations
00283     assert(associations.size() == transformVector.size());
00284     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> global_transform;
00285     global_transform.setIdentity();
00286 
00287 
00288     std::ofstream file(output_name.c_str(), ios::out);
00289     if (!file)
00290     {
00291         cerr << "Failed to create file : " << output_name << endl;
00292         return -1;
00293     }
00294 
00295     for (size_t i = 0; i < transformVector.size(); i++)
00296     {
00297         global_transform = transformVector[i];
00298         Eigen::Quaternion<double> tmp(global_transform.rotation());
00299         cout << timestamps[i] << " " << global_transform.translation().transpose() << " " << tmp.x() << " " << tmp.y() << " " << tmp.z() << " " << tmp.w() << endl;
00300         file << timestamps[i] << " " << global_transform.translation().transpose() << " " << tmp.x() << " " << tmp.y() << " " << tmp.z() << " " << tmp.w() << endl;
00301     }
00302     file.close();
00303 
00304     return 0;
00305 }


ndt_fuser
Author(s): Todor Stoyanov and Jari Saarinen
autogenerated on Mon Jan 6 2014 11:35:58