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