00001 #include "cmd_line.h"
00002 #include <iostream>
00003 #include <fstream>
00004 #include <visp/vpConfig.h>
00005 #include <visp/vpMbEdgeTracker.h>
00006
00007 void CmdLine::common(){
00008 po::options_description general("General options");
00009
00010 general.add_options()
00011 ("dmtxonly,d", "only detect the datamatrix")
00012 ("video-camera,C", "video from camera")
00013 ("video-source,s", po::value<std::string>(&video_channel_)->default_value("/dev/video1"),"video source. For example /dev/video1")
00014 ("data-directory,D", po::value<std::string>(&data_dir_)->default_value("./data/"),"directory from which to load images")
00015 ("video-input-path,J", po::value<std::string>(&input_file_pattern_)->default_value("/images/%08d.jpg"),"input video file path relative to the data directory")
00016 ("video-output-path,L", po::value<std::string>(&log_file_pattern_),"output video file path relative to the data directory")
00017 ("single-image,I", po::value<std::string>(&single_image_name_),"load this single image (relative to data dir)")
00018 ("pattern-name,P", po::value<std::string>(&pattern_name_)->default_value("pattern"),"name of xml,init and wrl files")
00019 ("detector-type,r", po::value<std::string>()->default_value("zbar"),"Type of your detector that will be used for initialisation/recovery. zbar for QRcodes and more, dmtx for flashcodes.")
00020 ("tracker-type,t", po::value<std::string>()->default_value("klt_mbt"),"Type of tracker. mbt_klt for hybrid: mbt+klt, mbt for model based, klt for klt-based")
00021 ("verbose,v", po::value< bool >(&verbose_)->default_value(false)->composing(), "Enable or disable additional printings")
00022 ("dmx-detector-timeout,T", po::value<int>(&dmx_timeout_)->default_value(1000), "timeout for datamatrix detection in ms")
00023 ("config-file,c", po::value<std::string>(&config_file)->default_value("./data/config.cfg"), "config file for the program")
00024 ("show-fps,f", po::value< bool >(&show_fps_)->default_value(false)->composing(), "show framerate")
00025 ("show-plot,p", po::value< bool >(&show_plot_)->default_value(false)->composing(), "show variances graph")
00026
00027 ("help", "produce help message")
00028 ;
00029
00030 po::options_description configuration("Configuration");
00031 configuration.add_options()
00032 ("flashcode-coordinates,F",
00033 po::value< std::vector<double> >(&flashcode_coordinates)->multitoken()->composing(),
00034 "3D coordinates of the flashcode in clockwise order")
00035 ("inner-coordinates,i",
00036 po::value< std::vector<double> >(&inner_coordinates)->multitoken()->composing(),
00037 "3D coordinates of the inner region in clockwise order")
00038 ("outer-coordinates,o",
00039 po::value< std::vector<double> >(&outer_coordinates)->multitoken()->composing(),
00040 "3D coordinates of the outer region in clockwise order")
00041 ("variance-file,V", po::value< std::string >(&var_file_)->composing(), "file to store variance values")
00042 ("variance-limit,l", po::value< double >(&var_limit_)->composing(),
00043 "above this limit the tracker will be considered lost and the pattern will be detected with the flascode")
00044 ("mbt-convergence-steps,S", po::value< int >(&mbt_convergence_steps_)->default_value(1)->composing(),
00045 "when a new model is detected, how many tracking iterations should the tracker perform so the model matches the projection.")
00046 ("hinkley-range,H",
00047 po::value< std::vector<double> >(&hinkley_range_)->multitoken()->composing(),
00048 "pair of alpha, delta values describing the two hinkley tresholds")
00049 ("mbt-dynamic-range,R", po::value< double >(&mbt_dynamic_range_)->composing(),
00050 "Adapt mbt range to symbol size. The width of the outer black corner is multiplied by this value to get the mbt range. Try 0.2")
00051 ("ad-hoc-recovery,W", po::value< bool >(&adhoc_recovery_)->default_value(true)->composing(), "Enable or disable ad-hoc recovery")
00052 ("ad-hoc-recovery-display,D", po::value< bool >(&adhoc_recovery_display_)->default_value(false)->composing(), "Enable or disable ad-hoc recovery display")
00053 ("ad-hoc-recovery-ratio,y", po::value< double >(&adhoc_recovery_ratio_)->default_value(0.5)->composing(),
00054 "use ad-hoc recovery based on the model. The tracker will look for black pixels at ratio*[pattern size] from the center")
00055 ("ad-hoc-recovery-size,w", po::value< double >(&adhoc_recovery_size_)->default_value(0.5)->composing(),
00056 "fraction of the black outer band size. The control points (those that should be black and in that way check tracking is still there).")
00057 ("ad-hoc-recovery-threshold,Y", po::value< unsigned int >(&adhoc_recovery_treshold_)->default_value(100)->composing(),
00058 "Threshold over which the point is considered out of the black area of the object")
00059 ("log-checkpoints,g","log checkpoints in the log file")
00060 ("log-pose,q", po::value< bool >(&log_pose_)->default_value(false)->composing(),"log pose in the log file")
00061 ;
00062 prog_args.add(general);
00063 prog_args.add(configuration);
00064 }
00065 void CmdLine::loadConfig(std::string& config_file){
00066 std::ifstream in( config_file.c_str() );
00067 po::store(po::parse_config_file(in,prog_args,false), vm_);
00068 po::notify(vm_);
00069 in.close();
00070
00071 for(unsigned int i =0;i<flashcode_coordinates.size()/3;i++){
00072 vpPoint p;
00073 p.setWorldCoordinates(flashcode_coordinates[i*3],flashcode_coordinates[i*3+1],flashcode_coordinates[i*3+2]);
00074 flashcode_points_3D_.push_back(p);
00075 }
00076
00077 for(unsigned int i =0;i<inner_coordinates.size()/3;i++){
00078 vpPoint p;
00079 p.setWorldCoordinates(inner_coordinates[i*3],inner_coordinates[i*3+1],inner_coordinates[i*3+2]);
00080 inner_points_3D_.push_back(p);
00081 }
00082
00083 for(unsigned int i =0;i<outer_coordinates.size()/3;i++){
00084 vpPoint p;
00085 p.setWorldCoordinates(outer_coordinates[i*3],outer_coordinates[i*3+1],outer_coordinates[i*3+2]);
00086 outer_points_3D_.push_back(p);
00087 }
00088
00089 if(get_verbose()){
00090 std::cout << "Loaded " << flashcode_points_3D_.size() << " flashcode extremity points, " << inner_points_3D_.size() << " inner contour points and " << outer_points_3D_.size() << " outer contour points." << std::endl;
00091 std::cout << "Tracker set to:";
00092 switch(get_tracker_type()){
00093 case MBT:
00094 std::cout << "model based tracker";
00095 break;
00096 case KLT_MBT:
00097 std::cout << "hybrid (mbt+klt)";
00098 break;
00099 case KLT:
00100 std::cout << "tracker with klt points";
00101 break;
00102 }
00103 std::cout << std::endl;
00104
00105 std::cout << "Detector set to:";
00106 switch(get_detector_type()){
00107 case ZBAR:
00108 std::cout << "QR code";
00109 break;
00110 case DMTX:
00111 std::cout << "Datamatrix (flashcode)";
00112 break;
00113 }
00114 std::cout << std::endl;
00115
00116 }
00117
00118 if(using_var_file())
00119 std::cout << "Using variance file:" << get_var_file() << std::endl;
00120 if (vm_.count("help")) {
00121 std::cout << prog_args << std::endl;
00122 should_exit_ = true;
00123
00124 }
00125 }
00126 CmdLine:: CmdLine(std::string& config_file) : should_exit_(false) {
00127 this->config_file = config_file;
00128 common();
00129 loadConfig(config_file);
00130 }
00131
00132 CmdLine:: CmdLine(int argc,char**argv) : should_exit_(false) {
00133 common();
00134
00135
00136 po::store(po::parse_command_line(argc, argv, prog_args), vm_);
00137 po::notify(vm_);
00138 if(get_verbose())
00139 std::cout << "Loading config from:" << config_file.c_str() << std::endl;
00140
00141 loadConfig(config_file);
00142
00143 }
00144
00145 vpCameraParameters CmdLine::get_cam_calib_params() const{
00146 vpCameraParameters cam;
00147 vpMbEdgeTracker tmptrack;
00148 tmptrack.loadConfigFile(get_xml_file().c_str() );
00149 tmptrack.getCameraParameters(cam);
00150 return cam;
00151 }
00152
00153 std::string CmdLine::get_log_file_pattern() const{
00154 return log_file_pattern_;
00155 }
00156
00157 std::string CmdLine::get_input_file_pattern() const{
00158 return input_file_pattern_;
00159 }
00160
00161 bool CmdLine:: show_plot() const{
00162 return show_plot_;
00163 }
00164
00165 bool CmdLine:: using_hinkley() const{
00166 return vm_.count("hinkley-range")>0 && hinkley_range_.size()==2;
00167 }
00168
00169 double CmdLine:: get_hinkley_alpha() const{
00170 if(!using_hinkley())
00171 throw std::exception();
00172 return hinkley_range_[0];
00173 }
00174
00175 double CmdLine:: get_hinkley_delta() const{
00176 if(!using_hinkley())
00177 throw std::exception();
00178 return hinkley_range_[1];
00179 }
00180
00181 int CmdLine:: get_mbt_convergence_steps() const{
00182 return mbt_convergence_steps_;
00183 }
00184
00185 double CmdLine:: get_mbt_dynamic_range() const{
00186 return mbt_dynamic_range_;
00187 }
00188
00189 bool CmdLine:: using_mbt_dynamic_range(){
00190 return vm_.count("mbt-dynamic-range")>0;
00191 }
00192
00193 double CmdLine:: get_var_limit() const{
00194 return var_limit_;
00195 }
00196
00197 bool CmdLine:: using_var_limit() const{
00198 return vm_.count("variance-limit")>0;
00199 }
00200
00201 std::string CmdLine:: get_var_file() const{
00202 return var_file_;
00203 }
00204
00205 bool CmdLine:: using_var_file() const{
00206 return vm_.count("variance-file")>0;
00207 }
00208
00209 bool CmdLine:: logging_video() const{
00210 return vm_.count("video-output-path")>0;
00211 }
00212
00213 bool CmdLine:: dmtx_only() const{
00214 return vm_.count("dmtxonly")>0;
00215 }
00216
00217 bool CmdLine:: should_exit() const{
00218 return should_exit_;
00219 }
00220
00221 std::string CmdLine:: get_video_channel() const{
00222 return video_channel_;
00223 }
00224
00225 bool CmdLine:: show_fps() const{
00226 return show_fps_;
00227 }
00228
00229 bool CmdLine:: get_verbose() const{
00230 return verbose_;
00231 }
00232
00233 int CmdLine:: get_dmx_timeout() const{
00234 return dmx_timeout_;
00235 }
00236
00237 double CmdLine:: get_inner_ratio() const{
00238 return inner_ratio_;
00239 }
00240
00241 double CmdLine:: get_outer_ratio() const{
00242 return outer_ratio_;
00243 }
00244
00245 bool CmdLine:: using_data_dir() const{
00246 return vm_.count("data-directory")>0;
00247 }
00248
00249 bool CmdLine:: using_video_camera() const{
00250 return vm_.count("video-camera")>0;
00251 }
00252
00253 std::string CmdLine:: get_data_dir() const{
00254 return data_dir_;
00255 }
00256
00257 std::string CmdLine:: get_pattern_name() const{
00258 return pattern_name_;
00259 }
00260
00261 std::string CmdLine:: get_wrl_file() const{
00262 return get_data_dir() + get_pattern_name() + std::string(".wrl");
00263 }
00264
00265 std::string CmdLine:: get_xml_file() const{
00266 return get_data_dir() + get_pattern_name() + std::string(".xml");
00267 }
00268
00269 std::string CmdLine:: get_init_file() const{
00270 return get_data_dir() + get_pattern_name() + std::string(".init");
00271 }
00272
00273 bool CmdLine:: using_single_image() const{
00274 return vm_.count("single-image")>0;
00275 }
00276
00277 std::string CmdLine:: get_single_image_path() const{
00278 return get_data_dir() + single_image_name_;
00279 }
00280
00281 std::vector<vpPoint>& CmdLine:: get_flashcode_points_3D() {
00282 return flashcode_points_3D_;
00283 }
00284
00285 std::vector<vpPoint>& CmdLine:: get_inner_points_3D() {
00286 return inner_points_3D_;
00287 }
00288
00289 std::vector<vpPoint>& CmdLine:: get_outer_points_3D() {
00290 return outer_points_3D_;
00291 }
00292
00293 CmdLine::DETECTOR_TYPE CmdLine:: get_detector_type() const{
00294 if(vm_["detector-type"].as<std::string>()=="zbar")
00295 return CmdLine::ZBAR;
00296 else
00297 return CmdLine::DMTX;
00298 }
00299
00300 CmdLine::TRACKER_TYPE CmdLine:: get_tracker_type() const{
00301 if(vm_["tracker-type"].as<std::string>()=="mbt")
00302 return CmdLine::MBT;
00303 else if(vm_["tracker-type"].as<std::string>()=="klt")
00304 return CmdLine::KLT;
00305 else
00306 return CmdLine::KLT_MBT;
00307 }
00308
00309
00310 double CmdLine:: get_adhoc_recovery_size() const{
00311 return adhoc_recovery_size_;
00312 }
00313
00314 double CmdLine:: get_adhoc_recovery_ratio() const{
00315 return adhoc_recovery_ratio_;
00316 }
00317
00318 unsigned int CmdLine:: get_adhoc_recovery_treshold() const{
00319 return adhoc_recovery_treshold_;
00320 }
00321
00322 bool CmdLine:: get_adhoc_recovery_display() const {
00323 return adhoc_recovery_display_;
00324 }
00325
00326 bool CmdLine:: using_adhoc_recovery() const{
00327 return adhoc_recovery_;
00328 }
00329
00330 bool CmdLine:: log_checkpoints() const{
00331 return vm_.count("log-checkpoints")>0;
00332 }
00333
00334 bool CmdLine:: log_pose() const{
00335 return log_pose_;
00336 }
00337
00338 void CmdLine:: set_data_directory(std::string dir){
00339 data_dir_ = dir;
00340 }
00341