diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp index 54c0812e6dccdcf5f5d7ab11ef9ae35e1602a885..acb0b1d22670d47a3a06ce266c5a53a734d49d48 100644 --- a/src/processor/processor_visual_odometry.cpp +++ b/src/processor/processor_visual_odometry.cpp @@ -98,7 +98,7 @@ void ProcessorVisualOdometry::preProcess() capture_image_incoming_->setTracksPrev(tracks_init); 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; } @@ -162,16 +162,16 @@ void ProcessorVisualOdometry::preProcess() // detect new KeyPoints in last and track them to incoming //////////////////////////////// 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){ - WOLF_TRACE( "Too Few Tracks. Detecting more keypoints..." ); + WOLF_INFO( "Too Few Tracks. Detecting more keypoints..." ); // Detect new KeyPoints std::vector<cv::KeyPoint> kps_last_new; detector_->detect(img_last, kps_last_new); 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 KeyPointsMap mwkps_last_new, mwkps_incoming_new; @@ -179,7 +179,7 @@ void ProcessorVisualOdometry::preProcess() WKeyPoint wkp(cvkp); 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); @@ -187,7 +187,7 @@ void ProcessorVisualOdometry::preProcess() // tracks that are not geometrically consistent are removed from tracks_last_incoming_new cv::Mat 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 // Only keep tracks until it reaches a max nb of tracks @@ -201,7 +201,7 @@ void ProcessorVisualOdometry::preProcess() tracks_last_incoming_filtered[track.first] = track.second; 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 capture_image_last_->addKeyPoints(mwkps_last_new); @@ -214,7 +214,7 @@ void ProcessorVisualOdometry::preProcess() } 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() // otherwise we store the pair as a newly detected track (for processNew) TracksMap tracks_map_li = capture_image_incoming_->getTracksPrev(); 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); // create a feature using the corresponding WKeyPoint contained in incoming (hence the "second") auto feat_inco = FeatureBase::emplace<FeaturePointImage>( @@ -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(); - WOLF_TRACE( "dt_processKnown (ms): " , dt_processKnown ); + WOLF_INFO( "dt_processKnown (ms): " , dt_processKnown ); // return number of successful tracks until incoming return tracks_map_li_matched_.size(); @@ -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(); - WOLF_TRACE( "dt_processNew (ms): " , dt_processNew ); + WOLF_INFO( "dt_processNew (ms): " , dt_processNew ); return counter_new_tracks; }