diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp index 8fb0ff1bbf3c6b7911f1853bf863c30c31ecf7b4..54c0812e6dccdcf5f5d7ab11ef9ae35e1602a885 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(); - std::cout << "dt_preprocess (ms): " << dt_preprocess << std::endl; + WOLF_TRACE( "dt_preprocess (ms): " , dt_preprocess ); return; } @@ -165,13 +165,13 @@ void ProcessorVisualOdometry::preProcess() WOLF_TRACE("# 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" ); + WOLF_TRACE( "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(), " new raw keypoints"); + WOLF_TRACE("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"); + WOLF_TRACE("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"); + WOLF_TRACE("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 : ", tracks_last_incoming_filtered.size(), " tracks"); + WOLF_TRACE("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_INFO( "dt_preprocess (ms): " , dt_preprocess ); + WOLF_TRACE( "dt_preprocess (ms): " , dt_preprocess ); } @@ -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(); - std::cout << "dt_processKnown (ms): " << dt_processKnown << std::endl; + WOLF_TRACE( "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(); - std::cout << "dt_processNew (ms): " << dt_processNew << std::endl; + WOLF_TRACE( "dt_processNew (ms): " , dt_processNew ); return counter_new_tracks; }