Skip to content
Snippets Groups Projects
Commit edcb49b5 authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

Continue refactoring

parent fd5efdba
No related branches found
No related tags found
1 merge request!38Draft: Resolve "Improve visual odometry"
......@@ -297,8 +297,13 @@ class ProcessorVisualOdometry : public ProcessorTracker
* 2. opencv: histogram_equalization
* 3. opencv: CLAHE
*/
void equalize_img(cv::Mat &img_incoming, ParamsProcessorVisualOdometry::EqualizationParams equalization)
;
void equalize_img(cv::Mat &img_incoming);
void detect_keypoints_empty_grid(cv::Mat img_incoming, CaptureImagePtr capture_image_incoming);
void filter_last_incoming_tracks_from_ransac_result(const TracksMap& tracks_last_incoming, const KeyPointsMap& mwkps_incoming, const TracksMap& tracks_origin_incoming,
TracksMap& tracks_last_incoming_filtered, KeyPointsMap& mwkps_incoming_filtered);
};
......
......@@ -84,41 +84,15 @@ void ProcessorVisualOdometry::preProcess()
// Get Capture
capture_image_incoming_ = std::static_pointer_cast<CaptureImage>(incoming_ptr_);
cv::Mat img_incoming = capture_image_incoming_->getImage();
equalize_img(img_incoming, params_visual_odometry_->equalization);
equalize_img(img_incoming);
// Time to PREPreprocess the image if necessary: greyscale if BGR, CLAHE etc...
// once preprocessing is done, replace the original image (?)
// if first image, compute keypoints, add to capture incoming and return
if (last_ptr_ == nullptr){
detect_keypoints_empty_grid(img_incoming, capture_image_incoming_);
// detect one FAST keypoint in each cell of the grid
cv::Rect rect_roi;
Eigen::Vector2i cell_index;
std::vector<cv::KeyPoint> kps_roi;
for (int i=1; i < params_visual_odometry_->grid.nbr_cells_h-1; i++){
for (int j=1; j < params_visual_odometry_->grid.nbr_cells_v-1; j++){
cell_index << i,j;
cell_grid_.cell2roi(cell_index, rect_roi);
cv::Mat img_roi(img_incoming, rect_roi); // no data copy -> no overhead
detector_->detect(img_roi, kps_roi);
if (kps_roi.size() > 0){
// retain only the best image in each region of interest
retainBest(kps_roi, 1);
// Keypoints are detected in the local coordinates of the region of interest
// -> translate to the full image corner coordinate system
kps_roi.at(0).pt.x = kps_roi.at(0).pt.x + rect_roi.x;
kps_roi.at(0).pt.y = kps_roi.at(0).pt.y + rect_roi.y;
capture_image_incoming_->addKeyPoint(kps_roi.at(0));
}
}
}
WOLF_DEBUG( "Initially detected " , capture_image_incoming_->getKeyPoints().size(), " keypoints in incoming" );
// Initialize the tracks data structure with a "dummy track" where the keypoint is pointing to itself
......@@ -135,6 +109,7 @@ void ProcessorVisualOdometry::preProcess()
return;
}
////////////////////////////////
// FEATURE TRACKING
// Update capture Incoming data
......@@ -153,8 +128,6 @@ void ProcessorVisualOdometry::preProcess()
KeyPointsMap mwkps_last = capture_image_last_ ->getKeyPoints();
KeyPointsMap mwkps_incoming; // init incoming
WOLF_DEBUG( "Tracking " , mwkps_last.size(), " keypoints in last" );
// TracksMap between last and incoming
......@@ -177,15 +150,8 @@ void ProcessorVisualOdometry::preProcess()
// and remove also from mwkps_incoming all the keypoints that have not been tracked
TracksMap tracks_last_incoming_filtered;
KeyPointsMap mwkps_incoming_filtered;
for (auto & track_origin_incoming : tracks_origin_incoming){
for (auto & track_last_incoming : tracks_last_incoming){
if (track_origin_incoming.second == track_last_incoming.second){
tracks_last_incoming_filtered[track_last_incoming.first] = track_last_incoming.second;
mwkps_incoming_filtered[track_last_incoming.second] = mwkps_incoming.at(track_last_incoming.second);
continue;
}
}
}
filter_last_incoming_tracks_from_ransac_result(tracks_last_incoming, mwkps_incoming, tracks_origin_incoming,
tracks_last_incoming_filtered, mwkps_incoming_filtered);
WOLF_DEBUG( "Tracked " , mwkps_incoming_filtered.size(), " inliers in incoming" );
......@@ -621,6 +587,10 @@ TracksMap ProcessorVisualOdometry::kltTrack(const cv::Mat _img_prev, const cv::M
bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev, const KeyPointsMap _mwkps_curr, TracksMap &_tracks_prev_curr, cv::Mat &_E)
{
// We need at least five tracks
// TODO: this case is NOT handled by the rest of the algorithm currently
if (_tracks_prev_curr.size() < 5) return false;
ParamsProcessorVisualOdometry::RansacParams prms = params_visual_odometry_->ransac;
// We need to build lists of pt2d for openCV function
......@@ -634,13 +604,10 @@ bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev
p2d_curr.push_back(cv::Point2d(ray_curr.x(), ray_curr.y()));
}
// We need at least five tracks
if (p2d_prev.size() < 5) return false;
cv::Mat cvMask;
cv::Mat K = cv::Mat::eye(3,3,CV_32F);
double focal = (sen_cam_->getIntrinsicMatrix()(0,0) +
sen_cam_->getIntrinsicMatrix()(1,1)) / 2;
sen_cam_->getIntrinsicMatrix()(1,1)) / 2;
_E = cv::findEssentialMat(p2d_prev,
p2d_curr,
......@@ -674,9 +641,9 @@ void ProcessorVisualOdometry::retainBest(std::vector<cv::KeyPoint> &_keypoints,
}
void ProcessorVisualOdometry::equalize_img(cv::Mat &img_incoming, ParamsProcessorVisualOdometry::EqualizationParams equalization)
void ProcessorVisualOdometry::equalize_img(cv::Mat &img_incoming)
{
switch (equalization.method)
switch (params_visual_odometry_->equalization.method)
{
case 0:
break;
......@@ -684,7 +651,7 @@ void ProcessorVisualOdometry::equalize_img(cv::Mat &img_incoming, ParamsProcesso
{
// average to central brightness
auto img_avg = (cv::mean(img_incoming)).val[0];
img_incoming += cv::Scalar(round(equalization.average.median - img_avg) );
img_incoming += cv::Scalar(round(params_visual_odometry_->equalization.average.median - img_avg) );
break;
}
case 2:
......@@ -696,14 +663,68 @@ void ProcessorVisualOdometry::equalize_img(cv::Mat &img_incoming, ParamsProcesso
{
// Contrast Limited Adaptive Histogram Equalization CLAHE
// -> more continuous lighting and higher contrast images
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(equalization.clahe.clip_limit,
equalization.clahe.tile_grid_size);
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(params_visual_odometry_->equalization.clahe.clip_limit,
params_visual_odometry_->equalization.clahe.tile_grid_size);
clahe->apply(img_incoming, img_incoming);
break;
}
}
}
void ProcessorVisualOdometry::detect_keypoints_empty_grid(cv::Mat img_incoming, CaptureImagePtr capture_image_incoming)
{
// detect one FAST keypoint in each cell of the grid
cv::Rect rect_roi;
Eigen::Vector2i cell_index;
std::vector<cv::KeyPoint> kps_roi;
for (int i=1; i < params_visual_odometry_->grid.nbr_cells_h-1; i++){
for (int j=1; j < params_visual_odometry_->grid.nbr_cells_v-1; j++){
cell_index << i,j;
cell_grid_.cell2roi(cell_index, rect_roi);
cv::Mat img_roi(img_incoming, rect_roi); // no data copy -> no overhead
detector_->detect(img_roi, kps_roi);
if (kps_roi.size() > 0){
// retain only the best image in each region of interest
retainBest(kps_roi, 1);
// Keypoints are detected in the local coordinates of the region of interest
// -> translate to the full image corner coordinate system
kps_roi.at(0).pt.x = kps_roi.at(0).pt.x + rect_roi.x;
kps_roi.at(0).pt.y = kps_roi.at(0).pt.y + rect_roi.y;
capture_image_incoming->addKeyPoint(kps_roi.at(0));
}
}
}
}
void ProcessorVisualOdometry::filter_last_incoming_tracks_from_ransac_result(const TracksMap& tracks_last_incoming, const KeyPointsMap& mwkps_incoming, const TracksMap& tracks_origin_incoming,
TracksMap& tracks_last_incoming_filtered, KeyPointsMap& mwkps_incoming_filtered)
{
/*
Use origin -> incoming track
O ---------------------> I
to filter last -> incoming track (and tracked keypoint map in incoming)
L -------> I
based of the tracks alive after RANSAC check on O --> I
This ugly double loop is due to the fact that features ids in Incoming are in the "values" of both maps, which requires linear search.
Ideally, this may be solved a Boost.Bimap.
**/
for (auto & track_origin_incoming : tracks_origin_incoming){
for (auto & track_last_incoming : tracks_last_incoming){
if (track_origin_incoming.second == track_last_incoming.second){
tracks_last_incoming_filtered[track_last_incoming.first] = track_last_incoming.second;
mwkps_incoming_filtered[track_last_incoming.second] = mwkps_incoming.at(track_last_incoming.second);
continue;
}
}
}
}
} //namespace wolf
// Register in the FactoryProcessor
......
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