inline void trackFeatures_checkResponses<TSimpleFeatureList>(TSimpleFeatureList &featureList,const CImage &cur_gray,const float minimum_KLT_response,const unsigned int KLT_response_half_win,const unsigned int max_x, const unsigned int max_y)
{
if (featureList.empty()) return;
for (int N = featureList.size()-1; N>=0 ; --N)
{
TSimpleFeature & ft = featureList[N];
if (ft.track_status!=status_TRACKED)
continue; // Skip if it's not correctly tracked.
const unsigned int x = ft.pt.x;
const unsigned int y = ft.pt.y;
if (x>KLT_response_half_win && y>KLT_response_half_win && x<max_x && y<max_y)
{ // Update response:
ft.response = cur_gray.KLT_response(x,y,KLT_response_half_win);
// Is it good enough? http://grooveshark.com/s/Goonies+Are+Good+Enough/2beBfO?src=5
if (ft.response<minimum_KLT_response)
{ // Nope!
ft.track_status = status_LOST;
}
}
else
{ // Out of bounds
ft.response = 0;
ft.track_status = status_OOB;
}
}
} // end of trackFeatures_checkResponses<>
/** Transform a TSimpleFeatureList into a TKeyPointList (opencv compatible)
*/
void m_convert_featureList_to_keypointList( const TSimpleFeatureList & featList, TKeyPointList & kpList )
{
kpList.resize( featList.size() );
TKeyPointList::iterator it1 = kpList.begin();
TSimpleFeatureList::const_iterator it2 = featList.begin();
for( ; it1 != kpList.end(); ++it1, ++it2 )
{
it1->pt.x = it2->pt.x;
it1->pt.y = it2->pt.y;
it1->response = it2->response;
} // end-for
} // end--m_convert_featureList_to_keypointList
void fast_corner_detect(
const IplImage* I, TSimpleFeatureList& corners, int barrier, uint8_t octave,
std::vector<size_t>* out_feats_index_by_row)
{
auto ptr = reinterpret_cast<CVD::byte* >(I->imageData);
CVD::BasicImage<CVD::byte> img(ptr, {I->width, I->height}, I->widthStep);
std::vector<CVD::ImageRef> outputs;
//reerve enough corners for every pixel
outputs.reserve(I->width * I->height);
F(img, outputs, barrier);
for(auto & output : outputs)
{
corners.push_back_fast(output.x << octave, output.y << octave);
}
if(out_feats_index_by_row)
{
auto & counters = *out_feats_index_by_row;
counters.assign(I->height, 0);
for(auto & output : outputs)
{
counters[output.y]++;
}
}
}
inline size_t trackFeatures_deleteOOB(
TSimpleFeatureList &trackedFeats,
const size_t img_width, const size_t img_height,
const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
{
if (trackedFeats.empty()) return 0;
std::vector<size_t> survival_idxs;
const size_t N = trackedFeats.size();
// 1st: Build list of survival indexes:
survival_idxs.reserve(N);
for (size_t i=0;i<N;i++)
{
const TSimpleFeature &ft = trackedFeats[i];
const TFeatureTrackStatus status = ft.track_status;
bool eras = (status_TRACKED!=status && status_IDLE!=status);
if (!eras)
{
// Also, check if it's too close to the image border:
const int x= ft.pt.x;
const int y= ft.pt.y;
if (x<MIN_DIST_MARGIN_TO_STOP_TRACKING || y<MIN_DIST_MARGIN_TO_STOP_TRACKING ||
x>(img_width-MIN_DIST_MARGIN_TO_STOP_TRACKING) ||
y>(img_height-MIN_DIST_MARGIN_TO_STOP_TRACKING))
{
eras = true;
}
}
if (!eras) survival_idxs.push_back(i);
}
// 2nd: Build updated list:
const size_t N2 = survival_idxs.size();
const size_t n_removed = N-N2;
for (size_t i=0;i<N2;i++)
{
if (survival_idxs[i]!=i)
trackedFeats[i] = trackedFeats[ survival_idxs[i] ];
}
trackedFeats.resize(N2);
return n_removed;
} // end of trackFeatures_deleteOOB
void CFeatureExtraction::detectFeatures_SSE2_FASTER12(const CImage &img, TSimpleFeatureList & corners, const int threshold, bool append_to_list, uint8_t octave,std::vector<size_t> * out_feats_index_by_row)
{
#if MRPT_HAS_OPENCV
const IplImage *IPL = img.getAs<IplImage>();
ASSERTDEB_(IPL && IPL->nChannels==1)
if (!append_to_list) corners.clear();
fast_corner_detect_12 (IPL,corners,threshold,octave,out_feats_index_by_row);
#else
THROW_EXCEPTION("MRPT built without OpenCV support!")
#endif
}
// N_fast = 9, 10, 12
void CFeatureExtraction::extractFeaturesFASTER_N(
const int N_fast,
const mrpt::utils::CImage & inImg,
CFeatureList & feats,
unsigned int init_ID,
unsigned int nDesiredFeatures,
const TImageROI & ROI ) const
{
MRPT_START
#if MRPT_HAS_OPENCV
// Make sure we operate on a gray-scale version of the image:
const CImage inImg_gray( inImg, FAST_REF_OR_CONVERT_TO_GRAY );
const IplImage *IPL = inImg_gray.getAs<IplImage>();
TSimpleFeatureList corners;
TFeatureType type_of_this_feature;
switch (N_fast)
{
case 9: fast_corner_detect_9 (IPL,corners, options.FASTOptions.threshold, 0, NULL); type_of_this_feature=featFASTER9; break;
case 10: fast_corner_detect_10(IPL,corners, options.FASTOptions.threshold, 0, NULL); type_of_this_feature=featFASTER10; break;
case 12: fast_corner_detect_12(IPL,corners, options.FASTOptions.threshold, 0, NULL); type_of_this_feature=featFASTER12; break;
default:
THROW_EXCEPTION("Only the 9,10,12 FASTER detectors are implemented.")
break;
};
// *All* the features have been extracted.
const size_t N = corners.size();
// Now:
// 1) Sort them by "response": It's ~100 times faster to sort a list of
// indices "sorted_indices" than sorting directly the actual list of features "corners"
std::vector<size_t> sorted_indices(N);
for (size_t i=0;i<N;i++) sorted_indices[i]=i;
// Use KLT response
if (options.FASTOptions.use_KLT_response ||
nDesiredFeatures!=0 // If the user wants us to limit the number of features, we need to do it according to some quality measure
)
{
const int KLT_half_win = 4;
const int max_x = inImg_gray.getWidth() - 1 - KLT_half_win;
const int max_y = inImg_gray.getHeight() - 1 - KLT_half_win;
for (size_t i=0;i<N;i++)
{
const int x = corners[i].pt.x;
const int y = corners[i].pt.y;
if (x>KLT_half_win && y>KLT_half_win && x<=max_x && y<=max_y)
corners[i].response = inImg_gray.KLT_response(x,y,KLT_half_win);
else corners[i].response = -100;
}
std::sort( sorted_indices.begin(), sorted_indices.end(), KeypointResponseSorter<TSimpleFeatureList>(corners) );
}
else
{
for (size_t i=0;i<N;i++)
corners[i].response = 0;
}
// 2) Filter by "min-distance" (in options.FASTOptions.min_distance)
// 3) Convert to MRPT CFeatureList format.
// Steps 2 & 3 are done together in the while() below.
// The "min-distance" filter is done by means of a 2D binary matrix where each cell is marked when one
// feature falls within it. This is not exactly the same than a pure "min-distance" but is pretty close
// and for large numbers of features is much faster than brute force search of kd-trees.
// (An intermediate approach would be the creation of a mask image updated for each accepted feature, etc.)
const bool do_filter_min_dist = options.FASTOptions.min_distance>1;
// Used half the min-distance since we'll later mark as occupied the ranges [i-1,i+1] for a feature at "i"
const unsigned int occupied_grid_cell_size = options.FASTOptions.min_distance/2.0;
const float occupied_grid_cell_size_inv = 1.0f/occupied_grid_cell_size;
unsigned int grid_lx = !do_filter_min_dist ? 1 : (unsigned int)(1 + inImg.getWidth() * occupied_grid_cell_size_inv);
unsigned int grid_ly = !do_filter_min_dist ? 1 : (unsigned int)(1 + inImg.getHeight() * occupied_grid_cell_size_inv );
mrpt::math::CMatrixBool occupied_sections(grid_lx,grid_ly); // See the comments above for an explanation.
occupied_sections.fillAll(false);
unsigned int nMax = (nDesiredFeatures!=0 && N > nDesiredFeatures) ? nDesiredFeatures : N;
const int offset = (int)this->options.patchSize/2 + 1;
const int size_2 = options.patchSize/2;
const size_t imgH = inImg.getHeight();
const size_t imgW = inImg.getWidth();
unsigned int i = 0;
unsigned int cont = 0;
TFeatureID nextID = init_ID;
if( !options.addNewFeatures )
feats.clear();
while( cont != nMax && i!=N )
{
// Take the next feature fromt the ordered list of good features:
const TSimpleFeature &feat = corners[ sorted_indices[i] ];
i++;
// Patch out of the image??
const int xBorderInf = feat.pt.x - size_2;
const int xBorderSup = feat.pt.x + size_2;
const int yBorderInf = feat.pt.y - size_2;
const int yBorderSup = feat.pt.y + size_2;
if (!( xBorderSup < (int)imgW && xBorderInf > 0 && yBorderSup < (int)imgH && yBorderInf > 0 ))
continue; // nope, skip.
if (do_filter_min_dist)
{
// Check the min-distance:
const size_t section_idx_x = size_t(feat.pt.x * occupied_grid_cell_size_inv);
const size_t section_idx_y = size_t(feat.pt.y * occupied_grid_cell_size_inv);
if (occupied_sections(section_idx_x,section_idx_y))
continue; // Already occupied! skip.
// Mark section as occupied
occupied_sections.set_unsafe(section_idx_x,section_idx_y, true);
if (section_idx_x>0) occupied_sections.set_unsafe(section_idx_x-1,section_idx_y, true);
if (section_idx_y>0) occupied_sections.set_unsafe(section_idx_x,section_idx_y-1, true);
if (section_idx_x<grid_lx-1) occupied_sections.set_unsafe(section_idx_x+1,section_idx_y, true);
if (section_idx_y<grid_ly-1) occupied_sections.set_unsafe(section_idx_x,section_idx_y+1, true);
}
// All tests passed: add new feature:
CFeaturePtr ft = CFeature::Create();
ft->type = type_of_this_feature;
ft->ID = nextID++;
ft->x = feat.pt.x;
ft->y = feat.pt.y;
ft->response = feat.response;
ft->orientation = 0;
ft->scale = 1;
ft->patchSize = options.patchSize; // The size of the feature patch
if( options.patchSize > 0 )
{
inImg.extract_patch(
ft->patch,
round( ft->x ) - offset,
round( ft->y ) - offset,
options.patchSize,
options.patchSize ); // Image patch surronding the feature
}
feats.push_back( ft );
++cont;
}
#endif
MRPT_END
}
inline void trackFeatures_addNewFeats<TSimpleFeatureList>(TSimpleFeatureList &featureList,const TSimpleFeatureList &new_feats, const std::vector<size_t> &sorted_indices, const size_t nNewToCheck,const size_t maxNumFeatures,const float minimum_KLT_response_to_add,const double threshold_sqr_dist_to_add_new,const size_t patchSize,const CImage &cur_gray, TFeatureID &max_feat_ID_at_input)
{
#if 0
// Brute-force version:
const int max_manhatan_dist = std::sqrt(2*threshold_sqr_dist_to_add_new);
for (size_t i=0;i<nNewToCheck && featureList.size()<maxNumFeatures;i++)
{
const TSimpleFeature &feat = new_feats[ sorted_indices[i] ];
if (feat.response<minimum_KLT_response_to_add) break; // continue;
// Check the min-distance:
int manh_dist = std::numeric_limits<int>::max();
for (size_t j=0;j<featureList.size();j++)
{
const TSimpleFeature &existing = featureList[j];
const int d = std::abs(existing.pt.x-feat.pt.x)+std::abs(existing.pt.y-feat.pt.y);
mrpt::utils::keep_min(manh_dist, d);
}
if (manh_dist<max_manhatan_dist)
continue; // Already occupied! skip.
// OK: accept it
featureList.push_back_fast(feat.pt.x,feat.pt.y); // (x,y)
//featureList.mark_kdtree_as_outdated();
// Fill out the rest of data:
TSimpleFeature &newFeat = featureList.back();
newFeat.ID = ++max_feat_ID_at_input;
newFeat.response = feat.response;
newFeat.octave = 0;
newFeat.track_status = status_IDLE; //!< Inactive: right after detection, and before being tried to track
}
#elif 0
// Version with an occupancy grid:
const int grid_cell_log2 = round( std::log(std::sqrt(threshold_sqr_dist_to_add_new)*0.5)/std::log(2.0));
int grid_lx = 1+(cur_gray.getWidth() >> grid_cell_log2);
int grid_ly = 1+(cur_gray.getHeight()>> grid_cell_log2);
mrpt::math::CMatrixBool & occupied_sections = featureList.getOccupiedSectionsMatrix();
occupied_sections.setSize(grid_lx,grid_ly); // See the comments above for an explanation.
occupied_sections.fillAll(false);
for (size_t i=0;i<featureList.size();i++)
{
const TSimpleFeature &feat = featureList[i];
const int section_idx_x = feat.pt.x >> grid_cell_log2;
const int section_idx_y = feat.pt.y >> grid_cell_log2;
if (!section_idx_x || !section_idx_y || section_idx_x>=grid_lx-1 || section_idx_y>=grid_ly-1)
continue; // This may be too radical, but speeds up the logic below...
// Mark sections as occupied
bool *ptr1 = &occupied_sections.get_unsafe(section_idx_x-1,section_idx_y-1);
bool *ptr2 = &occupied_sections.get_unsafe(section_idx_x-1,section_idx_y );
bool *ptr3 = &occupied_sections.get_unsafe(section_idx_x-1,section_idx_y+1);
ptr1[0]=ptr1[1]=ptr1[2]=true;
ptr2[0]=ptr2[1]=ptr2[2]=true;
ptr3[0]=ptr3[1]=ptr3[2]=true;
}
for (size_t i=0;i<nNewToCheck && featureList.size()<maxNumFeatures;i++)
{
const TSimpleFeature &feat = new_feats[ sorted_indices[i] ];
if (feat.response<minimum_KLT_response_to_add) break; // continue;
// Check the min-distance:
const int section_idx_x = feat.pt.x >> grid_cell_log2;
const int section_idx_y = feat.pt.y >> grid_cell_log2;
if (!section_idx_x || !section_idx_y || section_idx_x>=grid_lx-2 || section_idx_y>=grid_ly-2)
continue; // This may be too radical, but speeds up the logic below...
if (occupied_sections(section_idx_x,section_idx_y))
continue; // Already occupied! skip.
// Mark section as occupied
bool *ptr1 = &occupied_sections.get_unsafe(section_idx_x-1,section_idx_y-1);
bool *ptr2 = &occupied_sections.get_unsafe(section_idx_x-1,section_idx_y );
bool *ptr3 = &occupied_sections.get_unsafe(section_idx_x-1,section_idx_y+1);
ptr1[0]=ptr1[1]=ptr1[2]=true;
ptr2[0]=ptr2[1]=ptr2[2]=true;
ptr3[0]=ptr3[1]=ptr3[2]=true;
// OK: accept it
featureList.push_back_fast(feat.pt.x,feat.pt.y); // (x,y)
//featureList.mark_kdtree_as_outdated();
// Fill out the rest of data:
TSimpleFeature &newFeat = featureList.back();
newFeat.ID = ++max_feat_ID_at_input;
newFeat.response = feat.response;
newFeat.octave = 0;
newFeat.track_status = status_IDLE; //!< Inactive: right after detection, and before being tried to track
}
#else
// Version with KD-tree
CFeatureListKDTree<TSimpleFeature> kdtree(featureList.getVector());
for (size_t i=0;i<nNewToCheck && featureList.size()<maxNumFeatures;i++)
{
const TSimpleFeature &feat = new_feats[ sorted_indices[i] ];
if (feat.response<minimum_KLT_response_to_add) break; // continue;
// Check the min-distance:
double min_dist_sqr = std::numeric_limits<double>::max();
if (!featureList.empty())
{
//m_timlog.enter("[CGenericFeatureTracker] add new features.kdtree");
min_dist_sqr = kdtree.kdTreeClosestPoint2DsqrError(feat.pt.x,feat.pt.y );
//m_timlog.leave("[CGenericFeatureTracker] add new features.kdtree");
}
if (min_dist_sqr>threshold_sqr_dist_to_add_new)
{
// OK: accept it
featureList.push_back_fast(feat.pt.x,feat.pt.y); // (x,y)
kdtree.mark_as_outdated();
// Fill out the rest of data:
TSimpleFeature &newFeat = featureList.back();
newFeat.ID = ++max_feat_ID_at_input;
newFeat.response = feat.response;
newFeat.octave = 0;
newFeat.track_status = status_IDLE; //!< Inactive: right after detection, and before being tried to track
}
}
#endif
} // end of trackFeatures_addNewFeats<>
// ------------------------------------------------------
// DoTrackingDemo
// ------------------------------------------------------
int DoTrackingDemo(CCameraSensorPtr cam, bool DO_SAVE_VIDEO)
{
win = mrpt::gui::CDisplayWindow3D::Create("Tracked features",800,600);
mrpt::vision::CVideoFileWriter vidWritter;
bool hasResolution = false;
TCamera cameraParams; // For now, will only hold the image resolution on the arrive of the first frame.
TSimpleFeatureList trackedFeats;
unsigned int step_num = 0;
bool SHOW_FEAT_IDS = true;
bool SHOW_RESPONSES = true;
bool SHOW_FEAT_TRACKS = true;
const double SAVE_VIDEO_FPS = 30; // If DO_SAVE_VIDEO=true, the FPS of the video file
const char* SAVE_VIDEO_CODEC = "XVID"; // "XVID", "PIM1", "MJPG"
bool DO_HIST_EQUALIZE_IN_GRAYSCALE = false;
string VIDEO_OUTPUT_FILE = "./tracking_video.avi";
const double MAX_FPS = 5000; // 5.0; // Hz (to slow down visualization).
CGenericFeatureTrackerAutoPtr tracker;
// "CFeatureTracker_KL" is by far the most robust implementation for now:
tracker = CGenericFeatureTrackerAutoPtr( new CFeatureTracker_KL );
tracker->enableTimeLogger(true); // Do time profiling.
// Set of parameters common to any tracker implementation:
// -------------------------------------------------------------
// To see all the existing params and documentation, see mrpt::vision::CGenericFeatureTracker
tracker->extra_params["remove_lost_features"] = 1; // automatically remove out-of-image and badly tracked features
tracker->extra_params["add_new_features"] = 1; // track, AND ALSO, add new features
tracker->extra_params["add_new_feat_min_separation"] = 32;
tracker->extra_params["minimum_KLT_response_to_add"] = 10;
tracker->extra_params["add_new_feat_max_features"] = 350;
tracker->extra_params["add_new_feat_patch_size"] = 11;
tracker->extra_params["update_patches_every"] = 0; // Don't update patches.
tracker->extra_params["check_KLT_response_every"] = 5; // Re-check the KLT-response to assure features are in good points.
tracker->extra_params["minimum_KLT_response"] = 5;
// Specific params for "CFeatureTracker_KL"
// ------------------------------------------------------
tracker->extra_params["window_width"] = 5;
tracker->extra_params["window_height"] = 5;
//tracker->extra_params["LK_levels"] = 3;
//tracker->extra_params["LK_max_iters"] = 10;
//tracker->extra_params["LK_epsilon"] = 0.1;
//tracker->extra_params["LK_max_tracking_error"] = 150;
// --------------------------------
// The main loop
// --------------------------------
CImage previous_image;
TSequenceFeatureObservations feat_track_history;
bool save_tracked_history = true; // Dump feat_track_history to a file at the end
TCameraPoseID curCamPoseId = 0;
cout << endl << "TO END THE PROGRAM: Close the window.\n";
mrpt::opengl::COpenGLViewportPtr gl_view;
{
mrpt::opengl::COpenGLScenePtr scene = win->get3DSceneAndLock();
gl_view = scene->getViewport("main");
win->unlockAccess3DScene();
}
// Aux data for drawing the recent track of features:
static const size_t FEATS_TRACK_LEN = 10;
std::map<TFeatureID,std::list<TPixelCoord> > feat_tracks;
// infinite loop, until we close the win:
while( win->isOpen() )
{
CObservationPtr obs;
try
{
obs= cam->getNextFrame();
}
catch (CExceptionEOF &)
{ // End of a rawlog file.
break;
}
if (!obs)
{
cerr << "*Warning* getNextFrame() returned NULL!\n";
mrpt::system::sleep(50);
continue;
}
CImage theImg; // The grabbed image:
if (IS_CLASS(obs,CObservationImage))
{
CObservationImagePtr o = CObservationImagePtr(obs);
theImg.copyFastFrom(o->image);
}
else if (IS_CLASS(obs,CObservationStereoImages))
{
CObservationStereoImagesPtr o = CObservationStereoImagesPtr(obs);
theImg.copyFastFrom(o->imageLeft);
}
else if (IS_CLASS(obs,CObservation3DRangeScan))
{
CObservation3DRangeScanPtr o = CObservation3DRangeScanPtr(obs);
if (o->hasIntensityImage)
theImg.copyFastFrom(o->intensityImage);
}
else
{
continue; // Silently ignore non-image observations.
}
// Make sure the image is loaded (for the case it came from a rawlog file)
if (theImg.isExternallyStored())
theImg.loadFromFile( theImg.getExternalStorageFileAbsolutePath());
// Take the resolution upon first valid frame.
if (!hasResolution)
{
hasResolution = true;
// cameraParams.scaleToResolution()...
cameraParams.ncols = theImg.getWidth();
cameraParams.nrows = theImg.getHeight();
}
// Do tracking:
if (step_num>1) // we need "previous_image" to be valid.
{
// This single call makes: detection, tracking, recalculation of KLT_response, etc.
tracker->trackFeatures(previous_image, theImg, trackedFeats);
}
// Save the image for the next step:
previous_image = theImg;
// Save history of feature observations:
tracker->getProfiler().enter("Save history");
for (size_t i=0;i<trackedFeats.size();++i)
{
TSimpleFeature &f = trackedFeats[i];
const TPixelCoordf pxRaw(f.pt.x,f.pt.y);
TPixelCoordf pxUndist;
//mrpt::vision::pinhole::undistort_point(pxRaw,pxUndist, cameraParams);
pxUndist = pxRaw;
feat_track_history.push_back( TFeatureObservation(f.ID,curCamPoseId, pxUndist ) );
}
curCamPoseId++;
tracker->getProfiler().leave("Save history");
// now that we're done with the image, we can directly write onto it
// for the display
// ----------------------------------------------------------------
if (DO_HIST_EQUALIZE_IN_GRAYSCALE && !theImg.isColor())
theImg.equalizeHistInPlace();
// Convert to color so we can draw color marks, etc.
theImg.colorImageInPlace();
double extra_tim_to_wait=0;
{ // FPS:
static CTicTac tictac;
const double T = tictac.Tac();
tictac.Tic();
const double fps = 1.0/(std::max(1e-5,T));
//theImg.filledRectangle(1,1,175,25,TColor(0,0,0));
const int current_adapt_thres = tracker->getDetectorAdaptiveThreshold();
theImg.selectTextFont("6x13B");
theImg.textOut(3,3,format("FPS: %.03f Hz", fps ),TColor(200,200,0) );
theImg.textOut(3,22,format("# feats: %u - Adaptive threshold: %i", (unsigned int)trackedFeats.size(), current_adapt_thres ),TColor(200,200,0) );
theImg.textOut(3,41,
format("# raw feats: %u - Removed: %u",
(unsigned int)tracker->last_execution_extra_info.raw_FAST_feats_detected,
(unsigned int)tracker->last_execution_extra_info.num_deleted_feats ),
TColor(200,200,0) );
extra_tim_to_wait = 1.0/MAX_FPS - 1.0/fps;
}
// Draw feature tracks
if (SHOW_FEAT_TRACKS)
{
// Update new feature coords:
tracker->getProfiler().enter("drawFeatureTracks");
std::set<TFeatureID> observed_IDs;
for (size_t i=0;i<trackedFeats.size();++i)
{
const TSimpleFeature &ft = trackedFeats[i];
std::list<TPixelCoord> & seq = feat_tracks[ft.ID];
observed_IDs.insert(ft.ID);
if (seq.size()>=FEATS_TRACK_LEN) seq.erase(seq.begin());
seq.push_back(ft.pt);
// Draw:
if (seq.size()>1)
{
const std::list<TPixelCoord>::const_iterator it_end = seq.end();
std::list<TPixelCoord>::const_iterator it = seq.begin();
std::list<TPixelCoord>::const_iterator it_prev = it++;
for (;it!=it_end;++it)
{
theImg.line(it_prev->x,it_prev->y,it->x,it->y, TColor(190,190,190) );
it_prev = it;
}
}
}
tracker->getProfiler().leave("drawFeatureTracks");
// Purge old data:
for (std::map<TFeatureID,std::list<TPixelCoord> >::iterator it=feat_tracks.begin();it!=feat_tracks.end(); )
{
if (observed_IDs.find(it->first)==observed_IDs.end())
{
std::map<TFeatureID,std::list<TPixelCoord> >::iterator next_it = it;
next_it++;
feat_tracks.erase(it);
it = next_it;
}
else ++it;
}
}
// Draw Tracked feats:
{
theImg.selectTextFont("5x7");
tracker->getProfiler().enter("drawFeatures");
theImg.drawFeatures(trackedFeats, TColor(0,0,255), SHOW_FEAT_IDS, SHOW_RESPONSES);
tracker->getProfiler().leave("drawFeatures");
}
// Update window:
win->get3DSceneAndLock();
gl_view->setImageView(theImg);
win->unlockAccess3DScene();
win->repaint();
// Save debug output video:
// ----------------------------------
if (DO_SAVE_VIDEO)
{
static bool first = true;
if (first)
{
first=false;
if (vidWritter.open(
VIDEO_OUTPUT_FILE,
SAVE_VIDEO_FPS /* fps */, theImg.getSize(),
SAVE_VIDEO_CODEC,
true /* force color video */ ) )
{
cout << "[track-video] Saving tracking video to: " << VIDEO_OUTPUT_FILE << endl;
}
else
cerr << "ERROR: Trying to create output video: " << VIDEO_OUTPUT_FILE << endl;
}
vidWritter << theImg;
}
if (extra_tim_to_wait>0)
mrpt::system::sleep(1000*extra_tim_to_wait);
step_num++;
} // end infinite loop
// Save tracked feats:
if (save_tracked_history)
{
cout << "Saving tracked features to: tracked_feats.txt..."; cout.flush();
feat_track_history.saveToTextFile("./tracked_feats.txt");
cout << "Done!\n"; cout.flush();
#if 0
// SBA:
cout << "Saving cams.txt & pts.txt files in SBA library format..."; cout.flush();
feat_track_history.removeFewObservedFeatures(3);
feat_track_history.decimateCameraFrames(20);
feat_track_history.compressIDs();
TLandmarkLocationsVec locs;
TFramePosesVec cams;
feat_track_history.saveAsSBAFiles(locs,"pts.txt", cams, "cams.txt");
cout << "Done!\n"; cout.flush();
#endif
}
return 0; // End ok.
}
