Skip to content
Snippets Groups Projects
Commit dbcd9a1e authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Replace WOLF_TRACE --> WOLF_INFO...

... we will remove later all this debug info
parent c55e6d7c
No related branches found
No related tags found
2 merge requests!36After cmake and const refactor,!28Resolve "Building a new visual odometry system"
...@@ -98,7 +98,7 @@ void ProcessorVisualOdometry::preProcess() ...@@ -98,7 +98,7 @@ void ProcessorVisualOdometry::preProcess()
capture_image_incoming_->setTracksPrev(tracks_init); capture_image_incoming_->setTracksPrev(tracks_init);
auto dt_preprocess = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t1).count(); auto dt_preprocess = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t1).count();
WOLF_TRACE( "dt_preprocess (ms): " , dt_preprocess ); WOLF_INFO( "dt_preprocess (ms): " , dt_preprocess );
return; return;
} }
...@@ -162,16 +162,16 @@ void ProcessorVisualOdometry::preProcess() ...@@ -162,16 +162,16 @@ void ProcessorVisualOdometry::preProcess()
// detect new KeyPoints in last and track them to incoming // detect new KeyPoints in last and track them to incoming
//////////////////////////////// ////////////////////////////////
size_t n_tracks_origin = tracks_origin_incoming.size(); size_t n_tracks_origin = tracks_origin_incoming.size();
WOLF_TRACE("# of tracks: ", n_tracks_origin, "; min # of tracks: ", params_visual_odometry_->min_features_for_keyframe); WOLF_INFO("# of tracks: ", n_tracks_origin, "; min # of tracks: ", params_visual_odometry_->min_features_for_keyframe);
if (n_tracks_origin < params_visual_odometry_->min_features_for_keyframe){ if (n_tracks_origin < params_visual_odometry_->min_features_for_keyframe){
WOLF_TRACE( "Too Few Tracks. Detecting more keypoints..." ); WOLF_INFO( "Too Few Tracks. Detecting more keypoints..." );
// Detect new KeyPoints // Detect new KeyPoints
std::vector<cv::KeyPoint> kps_last_new; std::vector<cv::KeyPoint> kps_last_new;
detector_->detect(img_last, kps_last_new); detector_->detect(img_last, kps_last_new);
cv::KeyPointsFilter::retainBest(kps_last_new, params_visual_odometry_->max_new_features); cv::KeyPointsFilter::retainBest(kps_last_new, params_visual_odometry_->max_new_features);
WOLF_TRACE("Detected ", kps_last_new.size(), " raw keypoints"); WOLF_INFO("Detected ", kps_last_new.size(), " raw keypoints");
// Create a map of wolf KeyPoints to track only the new ones // Create a map of wolf KeyPoints to track only the new ones
KeyPointsMap mwkps_last_new, mwkps_incoming_new; KeyPointsMap mwkps_last_new, mwkps_incoming_new;
...@@ -179,7 +179,7 @@ void ProcessorVisualOdometry::preProcess() ...@@ -179,7 +179,7 @@ void ProcessorVisualOdometry::preProcess()
WKeyPoint wkp(cvkp); WKeyPoint wkp(cvkp);
mwkps_last_new[wkp.getId()] = wkp; mwkps_last_new[wkp.getId()] = wkp;
} }
WOLF_TRACE("Found ", mwkps_last_new.size(), " new keypoints in last"); WOLF_INFO("Found ", mwkps_last_new.size(), " new keypoints in last");
TracksMap tracks_last_incoming_new = kltTrack(img_last, img_incoming, mwkps_last_new, mwkps_incoming_new); TracksMap tracks_last_incoming_new = kltTrack(img_last, img_incoming, mwkps_last_new, mwkps_incoming_new);
...@@ -187,7 +187,7 @@ void ProcessorVisualOdometry::preProcess() ...@@ -187,7 +187,7 @@ void ProcessorVisualOdometry::preProcess()
// tracks that are not geometrically consistent are removed from tracks_last_incoming_new // tracks that are not geometrically consistent are removed from tracks_last_incoming_new
cv::Mat E; cv::Mat E;
filterWithEssential(mwkps_last_new, mwkps_incoming_new, tracks_last_incoming_new, E); filterWithEssential(mwkps_last_new, mwkps_incoming_new, tracks_last_incoming_new, E);
WOLF_TRACE("Tracked ", mwkps_incoming_new.size(), " new keypoints to incoming"); WOLF_INFO("Tracked ", mwkps_incoming_new.size(), " new keypoints to incoming");
// Concatenation of old tracks and new tracks // Concatenation of old tracks and new tracks
// Only keep tracks until it reaches a max nb of tracks // Only keep tracks until it reaches a max nb of tracks
...@@ -201,7 +201,7 @@ void ProcessorVisualOdometry::preProcess() ...@@ -201,7 +201,7 @@ void ProcessorVisualOdometry::preProcess()
tracks_last_incoming_filtered[track.first] = track.second; tracks_last_incoming_filtered[track.first] = track.second;
count_new_tracks++; count_new_tracks++;
} }
WOLF_TRACE("New total : ", n_tracks_origin, " + ", mwkps_incoming_new.size(), " = ", tracks_last_incoming_filtered.size(), " tracks"); WOLF_INFO("New total : ", n_tracks_origin, " + ", mwkps_incoming_new.size(), " = ", tracks_last_incoming_filtered.size(), " tracks");
// Update captures // Update captures
capture_image_last_->addKeyPoints(mwkps_last_new); capture_image_last_->addKeyPoints(mwkps_last_new);
...@@ -214,7 +214,7 @@ void ProcessorVisualOdometry::preProcess() ...@@ -214,7 +214,7 @@ void ProcessorVisualOdometry::preProcess()
} }
auto dt_preprocess = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t1).count(); auto dt_preprocess = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t1).count();
WOLF_TRACE( "dt_preprocess (ms): " , dt_preprocess ); WOLF_INFO( "dt_preprocess (ms): " , dt_preprocess );
} }
...@@ -237,7 +237,7 @@ unsigned int ProcessorVisualOdometry::processKnown() ...@@ -237,7 +237,7 @@ unsigned int ProcessorVisualOdometry::processKnown()
// otherwise we store the pair as a newly detected track (for processNew) // otherwise we store the pair as a newly detected track (for processNew)
TracksMap tracks_map_li = capture_image_incoming_->getTracksPrev(); TracksMap tracks_map_li = capture_image_incoming_->getTracksPrev();
if (tracks_map_li.count(id_feat_last)){ if (tracks_map_li.count(id_feat_last)){
// WOLF_TRACE("A corresponding track has been found for id_feat_last ", id_feat_last ); // WOLF_INFO("A corresponding track has been found for id_feat_last ", id_feat_last );
auto kp_track_li = tracks_map_li.find(id_feat_last); auto kp_track_li = tracks_map_li.find(id_feat_last);
// create a feature using the corresponding WKeyPoint contained in incoming (hence the "second") // create a feature using the corresponding WKeyPoint contained in incoming (hence the "second")
auto feat_inco = FeatureBase::emplace<FeaturePointImage>( auto feat_inco = FeatureBase::emplace<FeaturePointImage>(
...@@ -253,7 +253,7 @@ unsigned int ProcessorVisualOdometry::processKnown() ...@@ -253,7 +253,7 @@ unsigned int ProcessorVisualOdometry::processKnown()
} }
} }
auto dt_processKnown = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t1).count(); auto dt_processKnown = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t1).count();
WOLF_TRACE( "dt_processKnown (ms): " , dt_processKnown ); WOLF_INFO( "dt_processKnown (ms): " , dt_processKnown );
// return number of successful tracks until incoming // return number of successful tracks until incoming
return tracks_map_li_matched_.size(); return tracks_map_li_matched_.size();
...@@ -293,7 +293,7 @@ unsigned int ProcessorVisualOdometry::processNew(const int& _max_features) ...@@ -293,7 +293,7 @@ unsigned int ProcessorVisualOdometry::processNew(const int& _max_features)
} }
auto dt_processNew = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t1).count(); auto dt_processNew = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t1).count();
WOLF_TRACE( "dt_processNew (ms): " , dt_processNew ); WOLF_INFO( "dt_processNew (ms): " , dt_processNew );
return counter_new_tracks; return counter_new_tracks;
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment