$search
00001 #include "cv.h" 00002 #include "highgui.h" 00003 #include "tracking.h" 00004 #include <visp/vpImageConvert.h> 00005 #include <visp/vpPixelMeterConversion.h> 00006 #include <visp/vpImagePoint.h> 00007 #include <visp/vpDisplayX.h> 00008 #include <visp/vpPose.h> 00009 #include <visp/vpMeterPixelConversion.h> 00010 #include <visp/vpTrackingException.h> 00011 #include <visp/vpImageIo.h> 00012 #include <visp/vpRect.h> 00013 #include "logfilewriter.hpp" 00014 00015 namespace tracking{ 00016 00017 Tracker_:: Tracker_(CmdLine& cmd, detectors::DetectorBase* detector,vpMbTracker* tracker,bool flush_display) : 00018 cmd(cmd), 00019 iter_(0), 00020 flashcode_center_(640/2,480/2), 00021 detector_(detector), 00022 tracker_(tracker), 00023 flush_display_(flush_display){ 00024 std::cout << "starting tracker" << std::endl; 00025 cvTrackingBox_init_ = false; 00026 cvTrackingBox_.x = 0; 00027 cvTrackingBox_.y = 0; 00028 cvTrackingBox_.width = 0; 00029 cvTrackingBox_.height = 0; 00030 00031 points3D_inner_ = cmd.get_inner_points_3D(); 00032 points3D_outer_ = cmd.get_outer_points_3D(); 00033 outer_points_3D_bcp_ = cmd.get_outer_points_3D(); 00034 if(cmd.using_adhoc_recovery() || cmd.log_checkpoints()){ 00035 for(unsigned int i=0;i<points3D_outer_.size();i++){ 00036 vpPoint p; 00037 p.setWorldCoordinates( 00038 (points3D_outer_[i].get_oX()+points3D_inner_[i].get_oX())*cmd.get_adhoc_recovery_ratio(), 00039 (points3D_outer_[i].get_oY()+points3D_inner_[i].get_oY())*cmd.get_adhoc_recovery_ratio(), 00040 (points3D_outer_[i].get_oZ()+points3D_inner_[i].get_oZ())*cmd.get_adhoc_recovery_ratio() 00041 ); 00042 points3D_middle_.push_back(p); 00043 } 00044 } 00045 f_ = cmd.get_flashcode_points_3D(); 00046 00047 if(cmd.using_var_file()){ 00048 varfile_.open(cmd.get_var_file().c_str(),std::ios::out); 00049 varfile_ << "#These are variances and other data from the model based tracker in gnuplot format" << std::endl; 00050 if(cmd.using_hinkley()) 00051 varfile_ << "iteration\tvar_x\var_y\tvar_z\tvar_wx\tvar_wy\var_wz"; 00052 if(cmd.using_mbt_dynamic_range()) 00053 varfile_ << "\tmbt_range"; 00054 if(cmd.log_pose()) 00055 varfile_ << "\tpose_tx\tpose_ty\tpose_tz\tpose_rx\tpose_ry\tpose_rz"; 00056 if(cmd.log_checkpoints()) 00057 varfile_ << "\tcheckpoint_median"; 00058 00059 varfile_ << std::endl; 00060 } 00061 00062 if(cmd.using_hinkley()){ 00063 if(cmd.get_verbose()) 00064 std::cout << "Initialising hinkley with alpha=" << cmd.get_hinkley_alpha() << " and delta=" << cmd.get_hinkley_delta() << std::endl; 00065 for(hinkley_array_t::iterator i = hink_.begin();i!=hink_.end();i++) 00066 i->init(cmd.get_hinkley_alpha(),cmd.get_hinkley_delta()); 00067 } 00068 00069 if(cmd.using_mbt_dynamic_range()){ 00070 vpMbEdgeTracker *tracker_me = dynamic_cast<vpMbEdgeTracker*>(tracker_); 00071 if(tracker_me) 00072 tracker_me->getMovingEdge(tracker_me_config_); 00073 else 00074 std::cout << "error: could not init moving edges on tracker that doesn't support them." << std::endl; 00075 } 00076 00077 tracker_->loadConfigFile(cmd.get_xml_file().c_str() ); // Load the configuration of the tracker 00078 tracker_->loadModel(cmd.get_wrl_file().c_str()); // load the 3d model, to read .wrl model the 3d party library coin is required, if coin is not installed .cao file can be used. 00079 } 00080 00081 detectors::DetectorBase& Tracker_:: get_detector(){ 00082 return *detector_; 00083 } 00084 00085 vpMbTracker& Tracker_:: get_mbt(){ 00086 return *tracker_; 00087 } 00088 00089 std::vector<vpPoint>& Tracker_:: get_points3D_inner(){ 00090 return points3D_inner_; 00091 } 00092 00093 std::vector<vpPoint>& Tracker_:: get_points3D_outer(){ 00094 return points3D_outer_; 00095 00096 } 00097 00098 std::vector<vpPoint>& Tracker_:: get_points3D_middle(){ 00099 return points3D_middle_; 00100 } 00101 00102 std::vector<vpPoint>& Tracker_:: get_flashcode(){ 00103 return f_; 00104 } 00105 00106 vpImage<vpRGBa>& Tracker_:: get_I(){ 00107 return *I_; 00108 } 00109 00110 vpCameraParameters& Tracker_:: get_cam(){ 00111 return cam_; 00112 } 00113 00114 CmdLine& Tracker_:: get_cmd(){ 00115 return cmd; 00116 } 00117 00118 template<> 00119 const cv::Rect& Tracker_:: get_tracking_box<cv::Rect>(){ 00120 return cvTrackingBox_; 00121 } 00122 00123 template<> 00124 const vpRect& Tracker_:: get_tracking_box<vpRect>(){ 00125 return vpTrackingBox_; 00126 } 00127 00128 bool Tracker_:: input_selected(input_ready const& evt){ 00129 return vpDisplay::getClick(evt.I,false); 00130 } 00131 00132 00133 bool Tracker_:: no_input_selected(input_ready const& evt){ 00134 return !input_selected(evt); 00135 } 00136 00137 bool Tracker_:: flashcode_detected(input_ready const& evt){ 00138 this->cam_ = evt.cam_; 00139 00140 cv::Mat rgba = cv::Mat((int)evt.I.getRows(), (int)evt.I.getCols(), CV_8UC4, (void*)evt.I.bitmap); 00141 00142 cv::Mat bgr = cv::Mat((int)evt.I.getRows(), (int)evt.I.getCols(), CV_8UC3); 00143 cv::Mat alpha((int)evt.I.getRows(), (int)evt.I.getCols(), CV_8UC1); 00144 00145 cv::Mat out[] = {bgr, alpha}; 00146 // rgba[0] -> bgr[2], rgba[1] -> bgr[1], 00147 // rgba[2] -> bgr[0], rgba[3] -> alpha[0] 00148 int from_to[] = { 0,2, 1,1, 2,0, 3,3 }; 00149 cv::mixChannels(&rgba, 1, out, 2, from_to, 4); 00150 00151 //vpImageConvert::convert(evt.I,bgr); 00152 00153 return detector_->detect(bgr,cmd.get_dmx_timeout(),0,0); 00154 } 00155 00156 /* 00157 * Detect flashcode in region delimited by the outer points of the model 00158 * The timeout is the default timeout times the surface ratio 00159 */ 00160 bool Tracker_:: flashcode_redetected(input_ready const& evt){ 00161 this->cam_ = evt.cam_; 00162 00163 //vpImageConvert::convert(evt.I,cvI); 00164 cv::Mat rgba = cv::Mat((int)evt.I.getRows(), (int)evt.I.getCols(), CV_8UC4, (void*)evt.I.bitmap); 00165 00166 cv::Mat bgr = cv::Mat((int)evt.I.getRows(), (int)evt.I.getCols(), CV_8UC3); 00167 cv::Mat alpha((int)evt.I.getRows(), (int)evt.I.getCols(), CV_8UC1); 00168 00169 cv::Mat out[] = {bgr, alpha}; 00170 // rgba[0] -> bgr[2], rgba[1] -> bgr[1], 00171 // rgba[2] -> bgr[0], rgba[3] -> alpha[0] 00172 int from_to[] = { 0,2, 1,1, 2,0, 3,3 }; 00173 cv::mixChannels(&rgba, 1, out, 2, from_to, 4); 00174 00175 if (cvTrackingBox_init_) 00176 { 00177 cv::Mat subImage = cv::Mat(bgr,get_tracking_box<cv::Rect>()).clone(); 00178 00179 double timeout = cmd.get_dmx_timeout()*(double)(get_tracking_box<cv::Rect>().width*get_tracking_box<cv::Rect>().height)/(double)(bgr.cols*bgr.rows); 00180 return detector_->detect(subImage,(unsigned int)timeout,get_tracking_box<cv::Rect>().x,get_tracking_box<cv::Rect>().y); 00181 } 00182 else 00183 { 00184 return detector_->detect(bgr,cmd.get_dmx_timeout(),0,0); 00185 } 00186 } 00187 00188 void Tracker_:: find_flashcode_pos(input_ready const& evt){ 00189 std::vector<cv::Point> polygon = detector_->get_polygon(); 00190 double centerX = (double)(polygon[0].x+polygon[1].x+polygon[2].x+polygon[3].x)/4.; 00191 double centerY = (double)(polygon[0].y+polygon[1].y+polygon[2].y+polygon[3].y)/4.; 00192 vpPixelMeterConversion::convertPoint(cam_, flashcode_center_, centerX, centerY); 00193 00194 for(unsigned int i=0;i<f_.size();i++){ 00195 double x=0, y=0; 00196 vpImagePoint poly_pt(polygon[i].y,polygon[i].x); 00197 00198 vpPixelMeterConversion::convertPoint(cam_, poly_pt, x, y); 00199 f_[i].set_x(x); 00200 f_[i].set_y(y); 00201 } 00202 I_ = _I = &(evt.I); 00203 } 00204 00205 00206 bool Tracker_:: model_detected(msm::front::none const&){ 00207 vpImageConvert::convert(*I_,Igray_); 00208 vpPose pose; 00209 00210 for(unsigned int i=0;i<f_.size();i++) 00211 pose.addPoint(f_[i]); 00212 00213 pose.computePose(vpPose::LAGRANGE,cMo_); 00214 pose.computePose(vpPose::VIRTUAL_VS,cMo_); 00215 //vpDisplay::displayFrame(*I_,cMo_,cam_,0.01,vpColor::none,2); 00216 00217 std::vector<vpImagePoint> model_inner_corner(4); 00218 std::vector<vpImagePoint> model_outer_corner(4); 00219 for(unsigned int i=0;i<4;i++){ 00220 points3D_outer_[i].project(cMo_); 00221 points3D_inner_[i].project(cMo_); 00222 if(cmd.using_adhoc_recovery() || cmd.log_checkpoints()) 00223 points3D_middle_[i].project(cMo_); 00224 vpMeterPixelConversion::convertPoint(cam_,points3D_outer_[i].get_x(),points3D_outer_[i].get_y(),model_outer_corner[i]); 00225 vpMeterPixelConversion::convertPoint(cam_,points3D_inner_[i].get_x(),points3D_inner_[i].get_y(),model_inner_corner[i]); 00226 00227 if(cmd.get_verbose()){ 00228 std::cout << "model inner corner: (" << model_inner_corner[i].get_i() << "," << model_inner_corner[i].get_j() << ")" << std::endl; 00229 } 00230 } 00231 00232 try{ 00233 if(cmd.get_tracker_type()==CmdLine::MBT){ 00234 vpMbEdgeTracker* me_tracker = dynamic_cast<vpMbEdgeTracker*>(tracker_); 00235 me_tracker->resetTracker(); 00236 me_tracker->loadConfigFile(cmd.get_xml_file().c_str() ); 00237 me_tracker->loadModel(cmd.get_wrl_file().c_str()); 00238 } 00239 00240 tracker_->initFromPose(Igray_,cMo_); 00241 00242 tracker_->track(Igray_); // track the object on this image 00243 tracker_->getPose(cMo_); // get the pose 00244 tracker_->setCovarianceComputation(true); 00245 for(int i=0;i<cmd.get_mbt_convergence_steps();i++){ 00246 tracker_->track(Igray_); // track the object on this image 00247 tracker_->getPose(cMo_); // get the pose 00248 } 00249 }catch(vpException& e){ 00250 std::cout << "Tracking failed" << std::endl; 00251 std::cout << e.getStringMessage() << std::endl; 00252 return false; 00253 } 00254 //vpDisplay::getClick(*I_); 00255 return true; 00256 } 00257 00258 bool Tracker_:: mbt_success(input_ready const& evt){ 00259 iter_ = evt.frame; 00260 try{ 00261 LogFileWriter writer(varfile_); //the destructor of this class will act as a finally statement 00262 vpImageConvert::convert(evt.I,Igray_); 00263 tracker_->track(Igray_); // track the object on this image 00264 tracker_->getPose(cMo_); 00265 vpMatrix mat = tracker_->getCovarianceMatrix(); 00266 if(cmd.using_var_file()){ 00267 writer.write(iter_); 00268 for(unsigned int i=0;i<mat.getRows();i++) 00269 writer.write(mat[i][i]); 00270 } 00271 if(cmd.using_var_limit()) 00272 for(unsigned int i=0; i<6; i++) 00273 if(mat[i][i]>cmd.get_var_limit()) 00274 return false; 00275 if(cmd.using_hinkley()) 00276 for(unsigned int i=0; i<6; i++){ 00277 if(hink_[i].testDownUpwardJump(mat[i][i]) != vpHinkley::noJump){ 00278 writer.write(mat[i][i]); 00279 if(cmd.get_verbose()) 00280 std::cout << "Hinkley:detected jump!" << std::endl; 00281 return false; 00282 } 00283 } 00284 if(cmd.using_var_file() && cmd.using_mbt_dynamic_range()) 00285 writer.write(tracker_me_config_.getRange()); 00286 00287 00288 00289 for(unsigned int i=0;i<mat.getRows();i++) 00290 statistics.var(mat[i][i]); 00291 00292 if(mat.getRows() == 6){ //if the covariance matrix is set 00293 statistics.var_x(mat[0][0]); 00294 statistics.var_y(mat[1][1]); 00295 statistics.var_z(mat[2][2]); 00296 statistics.var_wx(mat[3][3]); 00297 statistics.var_wy(mat[4][4]); 00298 statistics.var_wz(mat[5][5]); 00299 } 00300 00301 if(cmd.using_var_file() && cmd.log_pose()){ 00302 vpPoseVector p(cMo_); 00303 for(unsigned int i=0;i<p.getRows();i++) 00304 writer.write(p[i]); 00305 } 00306 00307 if(cmd.using_adhoc_recovery() || cmd.log_checkpoints()){ 00308 for(unsigned int p=0;p<points3D_middle_.size();p++){ 00309 vpPoint& point3D = points3D_middle_[p]; 00310 00311 double _u=0.,_v=0.,_u_inner=0.,_v_inner=0.; 00312 point3D.project(cMo_); 00313 vpMeterPixelConversion::convertPoint(cam_,point3D.get_x(),point3D.get_y(),_u,_v); 00314 vpMeterPixelConversion::convertPoint(cam_,points3D_inner_[p].get_x(),points3D_inner_[p].get_y(),_u_inner,_v_inner); 00315 00316 boost::accumulators::accumulator_set< 00317 unsigned char, 00318 boost::accumulators::stats< 00319 boost::accumulators::tag::median(boost::accumulators::with_p_square_quantile) 00320 > 00321 > acc; 00322 00323 int region_width= std::max((int)(std::abs(_u-_u_inner)*cmd.get_adhoc_recovery_size()),1); 00324 int region_height=std::max((int)(std::abs(_v-_v_inner)*cmd.get_adhoc_recovery_size()),1); 00325 int u=(int)_u; 00326 int v=(int)_v; 00327 for(int i=std::max(u-region_width,0); 00328 i<std::min(u+region_width,(int)evt.I.getWidth()); 00329 i++){ 00330 for(int j=std::max(v-region_height,0); 00331 j<std::min(v+region_height,(int)evt.I.getHeight()); 00332 j++){ 00333 acc(Igray_[j][i]); 00334 statistics.checkpoints(Igray_[j][i]); 00335 } 00336 } 00337 double checkpoints_median = boost::accumulators::median(acc); 00338 if(cmd.using_var_file() && cmd.log_checkpoints()) 00339 writer.write(checkpoints_median); 00340 if( cmd.using_adhoc_recovery() && (unsigned int)checkpoints_median>cmd.get_adhoc_recovery_treshold() ) 00341 return false; 00342 } 00343 00344 00345 } 00346 00347 }catch(vpException& e){ 00348 std::cout << "Tracking lost" << std::endl; 00349 return false; 00350 } 00351 return true; 00352 } 00353 00354 void Tracker_:: track_model(input_ready const& evt){ 00355 this->cam_ = evt.cam_; 00356 std::vector<cv::Point> points; 00357 I_ = _I = &(evt.I); 00358 vpImageConvert::convert(evt.I,Igray_); 00359 boost::accumulators::accumulator_set< 00360 double, 00361 boost::accumulators::stats< 00362 boost::accumulators::tag::mean 00363 > 00364 > acc; 00365 for(unsigned int i=0;i<points3D_outer_.size();i++){ 00366 points3D_outer_[i].project(cMo_); 00367 points3D_inner_[i].project(cMo_); 00368 00369 double u=0.,v=0.,u_inner=0.,v_inner=0; 00370 vpMeterPixelConversion::convertPoint(cam_,points3D_outer_[i].get_x(),points3D_outer_[i].get_y(),u,v); 00371 vpMeterPixelConversion::convertPoint(cam_,points3D_inner_[i].get_x(),points3D_inner_[i].get_y(),u_inner,v_inner); 00372 points.push_back(cv::Point(u,v)); 00373 00374 acc(std::abs(u-u_inner)); 00375 acc(std::abs(v-v_inner)); 00376 } 00377 00378 if(cmd.using_mbt_dynamic_range()){ 00379 int range = (const unsigned int)(boost::accumulators::mean(acc)*cmd.get_mbt_dynamic_range()); 00380 00381 vpMbEdgeTracker *tracker_me = dynamic_cast<vpMbEdgeTracker*>(tracker_); 00382 if(tracker_me){ 00383 tracker_me->getMovingEdge(tracker_me_config_); 00384 tracker_me_config_.setRange(range); 00385 tracker_me->setMovingEdge(tracker_me_config_); 00386 }else 00387 std::cout << "error: could not init moving edges on tracker that doesn't support them." << std::endl; 00388 } 00389 cvTrackingBox_init_ = true; 00390 cvTrackingBox_ = cv::boundingRect(cv::Mat(points)); 00391 int s_x = cvTrackingBox_.x, 00392 s_y = cvTrackingBox_.y, 00393 d_x = cvTrackingBox_.x + cvTrackingBox_.width, 00394 d_y = cvTrackingBox_.y + cvTrackingBox_.height; 00395 s_x = std::max(s_x,0); 00396 s_y = std::max(s_y,0); 00397 d_x = std::min(d_x,(int)evt.I.getWidth()); 00398 d_y = std::min(d_y,(int)evt.I.getHeight()); 00399 cvTrackingBox_.x = s_x; 00400 cvTrackingBox_.y = s_y; 00401 cvTrackingBox_.width = d_x - s_x; 00402 cvTrackingBox_.height = d_y - s_y; 00403 vpTrackingBox_.setRect(cvTrackingBox_.x,cvTrackingBox_.y,cvTrackingBox_.width,cvTrackingBox_.height); 00404 } 00405 00406 Tracker_::statistics_t& Tracker_:: get_statistics(){ 00407 return statistics; 00408 } 00409 00410 void Tracker_:: set_flush_display(bool val){ 00411 flush_display_ = val; 00412 } 00413 00414 bool Tracker_:: get_flush_display(){ 00415 return flush_display_; 00416 } 00417 } 00418