5 #include <visp/vpImageConvert.h> 6 #include <visp/vpPixelMeterConversion.h> 7 #include <visp/vpImagePoint.h> 8 #include <visp/vpDisplayX.h> 9 #include <visp/vpPose.h> 10 #include <visp/vpMeterPixelConversion.h> 11 #include <visp/vpTrackingException.h> 12 #include <visp/vpImageIo.h> 13 #include <visp/vpRect.h> 14 #include <visp/vpMbKltTracker.h> 15 #include <visp/vpMbEdgeTracker.h> 17 #include "logfilewriter.hpp" 21 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0) 28 flashcode_center_(640/2,480/2),
31 flush_display_(flush_display){
32 std::cout <<
"starting tracker" << std::endl;
33 cvTrackingBox_init_ =
false;
36 cvTrackingBox_.width = 0;
37 cvTrackingBox_.height = 0;
40 tracker_->getCameraParameters(cam_);
46 for(
unsigned int i=0;i<points3D_outer_.size();i++){
48 p.setWorldCoordinates(
53 points3D_middle_.push_back(p);
60 varfile_ <<
"#These are variances and other data from the model based tracker in gnuplot format" << std::endl;
62 varfile_ <<
"iteration\tvar_x\var_y\tvar_z\tvar_wx\tvar_wy\var_wz";
64 varfile_ <<
"\tmbt_range";
66 varfile_ <<
"\tpose_tx\tpose_ty\tpose_tz\tpose_rx\tpose_ry\tpose_rz";
68 varfile_ <<
"\tcheckpoint_median";
70 varfile_ << std::endl;
76 for(hinkley_array_t::iterator i = hink_.begin();i!=hink_.end();i++)
81 vpMbEdgeTracker *tracker_me =
dynamic_cast<vpMbEdgeTracker*
>(tracker_);
83 tracker_me->getMovingEdge(tracker_me_config_);
85 std::cout <<
"error: could not init moving edges on tracker that doesn't support them." << std::endl;
90 tracker_->setCameraParameters(cam_);
93 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0) 135 const cv::Rect& Tracker_:: get_tracking_box<cv::Rect>(){
140 const vpRect& Tracker_:: get_tracking_box<vpRect>(){
145 return vpDisplay::getClick(evt.
I,
false);
154 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0) 157 cv::Mat rgba = cv::Mat((
int)evt.
I.getRows(), (int)evt.
I.getCols(), CV_8UC4, (
void*)evt.
I.bitmap);
159 cv::Mat bgr = cv::Mat((
int)evt.
I.getRows(), (int)evt.
I.getCols(), CV_8UC3);
160 cv::Mat alpha((
int)evt.
I.getRows(), (int)evt.
I.getCols(), CV_8UC1);
162 cv::Mat out[] = {bgr, alpha};
165 int from_to[] = { 0,2, 1,1, 2,0, 3,3 };
166 cv::mixChannels(&rgba, 1, out, 2, from_to, 4);
172 vpImageConvert::convert(evt.
I,
Igray_);
180 for (
size_t i=0; i<
detector_->getNbObjects(); i++) {
200 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0) 204 cv::Mat rgba = cv::Mat((
int)evt.
I.getRows(), (int)evt.
I.getCols(), CV_8UC4, (
void*)evt.
I.bitmap);
206 cv::Mat bgr = cv::Mat((
int)evt.
I.getRows(), (int)evt.
I.getCols(), CV_8UC3);
207 cv::Mat alpha((
int)evt.
I.getRows(), (int)evt.
I.getCols(), CV_8UC1);
209 cv::Mat out[] = {bgr, alpha};
212 int from_to[] = { 0,2, 1,1, 2,0, 3,3 };
213 cv::mixChannels(&rgba, 1, out, 2, from_to, 4);
217 cv::Mat subImage = cv::Mat(bgr,get_tracking_box<cv::Rect>()).clone();
219 double timeout =
cmd.
get_dmx_timeout()*(double)(get_tracking_box<cv::Rect>().width*get_tracking_box<cv::Rect>().height)/(double)(bgr.cols*bgr.rows);
220 return detector_->detect(subImage,(
unsigned int)timeout,get_tracking_box<cv::Rect>().
x,get_tracking_box<cv::Rect>().
y);
228 vpImageConvert::convert(evt.
I,
Igray_);
236 for (
size_t i=0; i<
detector_->getNbObjects(); i++) {
254 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0) 255 std::vector<cv::Point> polygon =
detector_->get_polygon();
256 double centerX = (double)(polygon[0].
x+polygon[1].
x+polygon[2].
x+polygon[3].
x)/4.;
257 double centerY = (double)(polygon[0].
y+polygon[1].
y+polygon[2].
y+polygon[3].
y)/4.;
260 for(
unsigned int i=0;i<
f_.size();i++){
262 vpImagePoint poly_pt(polygon[i].y,polygon[i].x);
264 vpPixelMeterConversion::convertPoint(
cam_, poly_pt, x, y);
271 std::vector< std::vector< vpImagePoint > > polygons =
detector_->getPolygon();
272 std::vector< vpImagePoint > polygon(4);
274 polygon = polygons[0];
281 for(
unsigned int i=0;i<
f_.size();i++){
284 vpPixelMeterConversion::convertPoint(
cam_, polygon[i], x, y);
295 vpImageConvert::convert(*
I_,
Igray_);
298 for(
unsigned int i=0;i<
f_.size();i++)
299 pose.addPoint(
f_[i]);
302 vpHomogeneousMatrix cMo_dem;
303 vpHomogeneousMatrix cMo_lag;
304 pose.computePose(vpPose::DEMENTHON, cMo_dem);
305 pose.computePose(vpPose::LAGRANGE, cMo_lag);
306 double residual_dem = pose.computeResidual(cMo_dem);
307 double residual_lag = pose.computeResidual(cMo_lag);
308 if (residual_dem < residual_lag)
312 pose.computePose(vpPose::VIRTUAL_VS,
cMo_);
315 catch(vpException& e) {
316 std::cout <<
"Pose computation failed: " << e.getStringMessage() << std::endl;
320 std::vector<vpImagePoint> model_inner_corner(4);
321 std::vector<vpImagePoint> model_outer_corner(4);
322 for(
unsigned int i=0;i<4;i++){
331 std::cout <<
"model inner corner: (" << model_inner_corner[i].get_i() <<
"," << model_inner_corner[i].get_j() <<
")" << std::endl;
341 vpCameraParameters cam;
350 tracker_->setCovarianceComputation(
true);
355 }
catch(vpException& e){
356 std::cout <<
"Tracking failed" << std::endl;
357 std::cout << e.getStringMessage() << std::endl;
369 vpImageConvert::convert(evt.
I,
Igray_);
380 for(
unsigned int i=0; i<6; i++)
384 for(
unsigned int i=0; i<6; i++){
388 std::cout <<
"Hinkley:detected jump!" << std::endl;
395 for(
unsigned int i=0;i<
covariance_.getRows();i++) {
409 vpPoseVector p(
cMo_);
410 for(
unsigned int i=0;i<p.getRows();i++)
418 double _u=0.,_v=0.,_u_inner=0.,_v_inner=0.;
419 point3D.project(
cMo_);
420 vpMeterPixelConversion::convertPoint(
cam_,point3D.get_x(),point3D.get_y(),_u,_v);
423 boost::accumulators::accumulator_set<
425 boost::accumulators::stats<
426 boost::accumulators::tag::median(boost::accumulators::with_p_square_quantile)
434 for(
int i=std::max(u-region_width,0);
435 i<std::min(u+region_width,(
int)evt.
I.getWidth());
437 for(
int j=std::max(v-region_height,0);
438 j<std::min(v+region_height,(
int)evt.
I.getHeight());
444 double checkpoints_median = boost::accumulators::median(acc);
446 writer.write(checkpoints_median);
451 }
catch(vpException& e){
452 std::cout <<
"Tracking lost" << std::endl;
461 std::vector<cv::Point> points;
463 vpImageConvert::convert(evt.
I,
Igray_);
465 boost::accumulators::accumulator_set<
467 boost::accumulators::stats<
468 boost::accumulators::tag::mean
476 double u=0.,v=0.,u_inner=0.,v_inner=0;
480 acc(std::abs(u-u_inner));
481 acc(std::abs(v-v_inner));
486 u = std::min(u,(
double)evt.
I.getWidth()-1);
488 v = std::min(v,(
double)evt.
I.getHeight()-1);
489 points.push_back(cv::Point(u,v));
495 vpMbEdgeTracker *tracker_me =
dynamic_cast<vpMbEdgeTracker*
>(
tracker_);
501 std::cout <<
"error: could not init moving edges on tracker that doesn't support them." << std::endl;
527 std::list<vpMbtDistanceLine*> linesList;
530 dynamic_cast<vpMbEdgeTracker*>(
tracker_)->getLline(linesList, 0);
532 std::list<vpMbtDistanceLine*>::iterator linesIterator = linesList.begin();
533 if (linesList.empty())
535 bool noVisibleLine =
true;
536 for (; linesIterator != linesList.end(); ++linesIterator)
538 vpMbtDistanceLine* line = *linesIterator;
540 #if VISP_VERSION_INT >= VP_VERSION_INT(3,0,0) // ViSP >= 3.0.0 541 if (line && line->isVisible() && ! line->meline.empty())
543 if (line && line->isVisible() && line->meline)
546 #if VISP_VERSION_INT >= VP_VERSION_INT(3,0,0) // ViSP >= 3.0.0 547 for(
unsigned int a = 0 ; a < line->meline.size() ; a++)
549 if(line->meline[a] != NULL) {
550 std::list<vpMeSite>::const_iterator sitesIterator = line->meline[a]->getMeList().begin();
551 if (line->meline[a]->getMeList().empty())
553 for (; sitesIterator != line->meline[a]->getMeList().end(); ++sitesIterator)
554 #elif VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
555 if (line->meline->getMeList().empty()) {
558 std::list<vpMeSite>::const_iterator sitesIterator = line->meline->getMeList().begin();
560 for (; sitesIterator != line->meline->getMeList().end(); ++sitesIterator)
562 if (line->meline->list.empty()) {
565 std::list<vpMeSite>::const_iterator sitesIterator = line->meline->list.begin();
567 for (; sitesIterator != line->meline->list.end(); ++sitesIterator)
570 visp_tracker::MovingEdgeSite movingEdgeSite;
571 movingEdgeSite.x = sitesIterator->ifloat;
572 movingEdgeSite.y = sitesIterator->jfloat;
573 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)// ViSP < 2.10.0 574 movingEdgeSite.suppress = sitesIterator->suppress;
576 sites->moving_edge_sites.push_back (movingEdgeSite);
578 noVisibleLine =
false;
580 #if VISP_VERSION_INT >= VP_VERSION_INT(3,0,0) // ViSP >= 3.0.0 595 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)// ViSP < 2.10.0 596 vpMbHiddenFaces<vpMbtKltPolygon> *poly_lst;
597 std::map<int, vpImagePoint> *map_klt;
600 poly_lst = &dynamic_cast<vpMbKltTracker*>(
tracker_)->getFaces();
602 for(
unsigned int i = 0 ; i < poly_lst->size() ; i++)
606 map_klt = &((*poly_lst)[i]->getCurrentPoints());
608 if(map_klt->size() > 3)
610 for (std::map<int, vpImagePoint>::iterator it=map_klt->begin(); it!=map_klt->end(); ++it)
612 visp_tracker::KltPoint kltPoint;
613 kltPoint.id = it->first;
614 kltPoint.i = it->second.get_i();
615 kltPoint.j = it->second.get_j();
616 klt->klt_points_positions.push_back (kltPoint);
621 #else // ViSP >= 2.10.0 622 std::list<vpMbtDistanceKltPoints*> *poly_lst;
623 std::map<int, vpImagePoint> *map_klt;
626 poly_lst = &
dynamic_cast<vpMbKltTracker*
>(
tracker_)->getFeaturesKlt();
628 for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=poly_lst->begin(); it!=poly_lst->end(); ++it){
629 map_klt = &((*it)->getCurrentPoints());
631 if((*it)->polygon->isVisible()){
632 if(map_klt->size() > 3)
634 for (std::map<int, vpImagePoint>::iterator it=map_klt->begin(); it!=map_klt->end(); ++it)
636 visp_tracker::KltPoint kltPoint;
637 kltPoint.id = it->first;
638 kltPoint.i = it->second.get_i();
639 kltPoint.j = it->second.get_j();
640 klt->klt_points_positions.push_back (kltPoint);
std::vector< vpPoint > & get_flashcode_points_3D()
std::string get_code_message() const
bool using_var_file() const
boost::accumulators::accumulator_set< double, boost::accumulators::stats< boost::accumulators::tag::median(boost::accumulators::with_p_square_quantile), boost::accumulators::tag::max, boost::accumulators::tag::mean > > var
bool input_selected(input_ready const &evt)
int get_dmx_timeout() const
boost::accumulators::accumulator_set< double, boost::accumulators::stats< boost::accumulators::tag::median(boost::accumulators::with_p_square_quantile), boost::accumulators::tag::max, boost::accumulators::tag::mean > > var_z
std::vector< vpPoint > & get_points3D_middle()
double get_var_limit() const
boost::accumulators::accumulator_set< double, boost::accumulators::stats< boost::accumulators::tag::median(boost::accumulators::with_p_square_quantile), boost::accumulators::tag::max, boost::accumulators::tag::mean > > var_x
bool model_detected(msm::front::none const &)
unsigned int get_adhoc_recovery_treshold() const
boost::accumulators::accumulator_set< double, boost::accumulators::stats< boost::accumulators::tag::median(boost::accumulators::with_p_square_quantile), boost::accumulators::tag::max, boost::accumulators::tag::mean > > var_y
std::vector< vpPoint > & get_inner_points_3D()
std::vector< vpPoint > & get_flashcode()
std::string get_xml_file() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
vpImage< vpRGBa > & get_I()
bool using_adhoc_recovery() const
Tracker_(CmdLine &cmd, vpDetectorBase *detector, vpMbTracker *tracker_, bool flush_display=true)
double get_adhoc_recovery_ratio() const
void find_flashcode_pos(input_ready const &evt)
std::vector< vpPoint > points3D_inner_
bool using_hinkley() const
std::vector< vpPoint > & get_points3D_outer()
boost::accumulators::accumulator_set< double, boost::accumulators::stats< boost::accumulators::tag::median(boost::accumulators::with_p_square_quantile), boost::accumulators::tag::max, boost::accumulators::tag::mean > > var_wz
vpDetectorBase & get_detector()
void track_model(input_ready const &evt)
boost::accumulators::accumulator_set< double, boost::accumulators::stats< boost::accumulators::tag::median(boost::accumulators::with_p_square_quantile), boost::accumulators::tag::max, boost::accumulators::tag::mean > > var_wy
boost::accumulators::accumulator_set< double, boost::accumulators::stats< boost::accumulators::tag::median(boost::accumulators::with_p_square_quantile), boost::accumulators::tag::max, boost::accumulators::tag::mean > > var_wx
std::vector< vpPoint > & get_points3D_inner()
bool using_mbt_dynamic_range()
statistics_t & get_statistics()
bool flashcode_redetected(input_ready const &evt)
bool mbt_success(input_ready const &evt)
bool log_checkpoints() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
#define ROS_WARN_STREAM(args)
std::vector< vpPoint > f_
vpCameraParameters & get_cam()
void set_code_message_index(const size_t &index)
int get_mbt_convergence_steps() const
std::vector< vpPoint > points3D_middle_
#define ROS_INFO_STREAM(args)
vpDetectorBase * detector_
bool no_input_selected(input_ready const &evt)
bool using_var_limit() const
bool flashcode_detected(input_ready const &evt)
double get_hinkley_alpha() const
vpImagePoint flashcode_center_
double get_mbt_dynamic_range() const
vpImage< unsigned char > Igray_
void updateKltPoints(visp_tracker::KltPointsPtr klt)
TRACKER_TYPE get_tracker_type() const
double get_hinkley_delta() const
double get_adhoc_recovery_size() const
void set_flush_display(bool val)
std::string get_var_file() const
std::vector< vpPoint > & get_outer_points_3D()
#define ROS_DEBUG_THROTTLE(period,...)
std::vector< vpPoint > points3D_outer_
void updateMovingEdgeSites(visp_tracker::MovingEdgeSitesPtr sites)
boost::accumulators::accumulator_set< double, boost::accumulators::stats< boost::accumulators::tag::median(boost::accumulators::with_p_square_quantile), boost::accumulators::tag::max, boost::accumulators::tag::mean > > checkpoints
std::string get_mbt_cad_file() const