Skip to content
Snippets Groups Projects
Commit 027df309 authored by Angel Santamaria-Navarro's avatar Angel Santamaria-Navarro
Browse files

remove the use of features explicitly and use kps and descriptors

parent 884f854b
No related branches found
No related tags found
No related merge requests found
...@@ -2,11 +2,12 @@ ...@@ -2,11 +2,12 @@
ADD_DEFINITIONS(-DPRINT_INFO_VU) ADD_DEFINITIONS(-DPRINT_INFO_VU)
# library source files # library source files
SET(sources SET(sources_main vision_utils.cpp)
vision_utils.cpp SET(sources_sen
sensors/sensor_base.cpp sensors/sensor_base.cpp
sensors/usb_cam/usb_cam.cpp sensors/usb_cam/usb_cam.cpp
sensors/usb_cam/usb_cam_load_yaml.cpp sensors/usb_cam/usb_cam_load_yaml.cpp)
SET(sources_det
detectors/detector_base.cpp detectors/detector_base.cpp
detectors/orb/detector_orb.cpp detectors/orb/detector_orb.cpp
detectors/orb/detector_orb_load_yaml.cpp detectors/orb/detector_orb_load_yaml.cpp
...@@ -31,7 +32,8 @@ SET(sources ...@@ -31,7 +32,8 @@ SET(sources
detectors/akaze/detector_akaze.cpp detectors/akaze/detector_akaze.cpp
detectors/akaze/detector_akaze_load_yaml.cpp detectors/akaze/detector_akaze_load_yaml.cpp
detectors/agast/detector_agast.cpp detectors/agast/detector_agast.cpp
detectors/agast/detector_agast_load_yaml.cpp detectors/agast/detector_agast_load_yaml.cpp)
SET(sources_des
descriptors/descriptor_base.cpp descriptors/descriptor_base.cpp
descriptors/orb/descriptor_orb.cpp descriptors/orb/descriptor_orb.cpp
descriptors/orb/descriptor_orb_load_yaml.cpp descriptors/orb/descriptor_orb_load_yaml.cpp
...@@ -54,7 +56,8 @@ SET(sources ...@@ -54,7 +56,8 @@ SET(sources
descriptors/daisy/descriptor_daisy.cpp descriptors/daisy/descriptor_daisy.cpp
descriptors/daisy/descriptor_daisy_load_yaml.cpp descriptors/daisy/descriptor_daisy_load_yaml.cpp
descriptors/lucid/descriptor_lucid.cpp descriptors/lucid/descriptor_lucid.cpp
descriptors/lucid/descriptor_lucid_load_yaml.cpp descriptors/lucid/descriptor_lucid_load_yaml.cpp)
SET(sources_mat
matchers/matcher_base.cpp matchers/matcher_base.cpp
matchers/bruteforce/matcher_bruteforce.cpp matchers/bruteforce/matcher_bruteforce.cpp
matchers/bruteforce/matcher_bruteforce_load_yaml.cpp matchers/bruteforce/matcher_bruteforce_load_yaml.cpp
...@@ -65,7 +68,8 @@ SET(sources ...@@ -65,7 +68,8 @@ SET(sources
matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2.cpp matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2.cpp
matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2_load_yaml.cpp matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2_load_yaml.cpp
matchers/flannbased/matcher_flannbased.cpp matchers/flannbased/matcher_flannbased.cpp
matchers/flannbased/matcher_flannbased_load_yaml.cpp matchers/flannbased/matcher_flannbased_load_yaml.cpp)
SET(sources_alg
algorithms/algorithm_base.cpp algorithms/algorithm_base.cpp
algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.cpp algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.cpp
algorithms/opticalflowpyrlk/alg_opticalflowpyrlk_load_yaml.cpp algorithms/opticalflowpyrlk/alg_opticalflowpyrlk_load_yaml.cpp
...@@ -75,15 +79,17 @@ SET(sources ...@@ -75,15 +79,17 @@ SET(sources
algorithms/activesearch/alg_activesearch_load_yaml.cpp) algorithms/activesearch/alg_activesearch_load_yaml.cpp)
# application header files # application header files
SET(headers SET(headers_main
vision_utils.h vision_utils.h
factory.h factory.h)
SET(headers_com
common_class/buffer.h common_class/buffer.h
common_class/feature.h common_class/frame.h)
common_class/frame.h SET(headers_sen
sensors/sensor_factory.h sensors/sensor_factory.h
sensors/sensor_base.h sensors/sensor_base.h
sensors/usb_cam/usb_cam.h sensors/usb_cam/usb_cam.h)
SET(headers_det
detectors/detector_factory.h detectors/detector_factory.h
detectors/detector_base.h detectors/detector_base.h
detectors/orb/detector_orb.h detectors/orb/detector_orb.h
...@@ -97,7 +103,8 @@ SET(headers ...@@ -97,7 +103,8 @@ SET(headers
detectors/sbd/detector_sbd.h detectors/sbd/detector_sbd.h
detectors/kaze/detector_kaze.h detectors/kaze/detector_kaze.h
detectors/akaze/detector_akaze.h detectors/akaze/detector_akaze.h
detectors/agast/detector_agast.h detectors/agast/detector_agast.h)
SET(headers_des
descriptors/descriptor_factory.h descriptors/descriptor_factory.h
descriptors/descriptor_base.h descriptors/descriptor_base.h
descriptors/orb/descriptor_orb.h descriptors/orb/descriptor_orb.h
...@@ -110,14 +117,16 @@ SET(headers ...@@ -110,14 +117,16 @@ SET(headers
descriptors/freak/descriptor_freak.h descriptors/freak/descriptor_freak.h
descriptors/brief/descriptor_brief.h descriptors/brief/descriptor_brief.h
descriptors/daisy/descriptor_daisy.h descriptors/daisy/descriptor_daisy.h
descriptors/lucid/descriptor_lucid.h descriptors/lucid/descriptor_lucid.h)
SET(headers_mat
matchers/matcher_factory.h matchers/matcher_factory.h
matchers/matcher_base.h matchers/matcher_base.h
matchers/bruteforce/matcher_bruteforce.h matchers/bruteforce/matcher_bruteforce.h
matchers/bruteforce_l1/matcher_bruteforce_l1.h matchers/bruteforce_l1/matcher_bruteforce_l1.h
matchers/bruteforce_hamming/matcher_bruteforce_hamming.h matchers/bruteforce_hamming/matcher_bruteforce_hamming.h
matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2.h matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2.h
matchers/flannbased/matcher_flannbased.h matchers/flannbased/matcher_flannbased.h)
SET(headers_alg
algorithms/algorithm_factory.h algorithms/algorithm_factory.h
algorithms/algorithm_base.h algorithms/algorithm_base.h
algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.h algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.h
...@@ -179,7 +188,7 @@ MESSAGE(STATUS "Configuring ${PROJECT_NAME} (creating config.h from _internal di ...@@ -179,7 +188,7 @@ MESSAGE(STATUS "Configuring ${PROJECT_NAME} (creating config.h from _internal di
include_directories("${PROJECT_BINARY_DIR}/conf") include_directories("${PROJECT_BINARY_DIR}/conf")
# create the shared library # create the shared library
ADD_LIBRARY(${PROJECT_NAME} SHARED ${sources}) ADD_LIBRARY(${PROJECT_NAME} SHARED ${sources_main} ${sources_sen} ${sources_det} ${sources_des} ${sources_mat} ${sources_alg})
# link necessary libraries # link necessary libraries
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS} ${Boost_LIBRARIES}) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS} ${Boost_LIBRARIES})
...@@ -193,7 +202,13 @@ INSTALL(TARGETS ${PROJECT_NAME} ...@@ -193,7 +202,13 @@ INSTALL(TARGETS ${PROJECT_NAME}
RUNTIME DESTINATION bin RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/${PROJECT_NAME} LIBRARY DESTINATION lib/${PROJECT_NAME}
ARCHIVE DESTINATION lib/${PROJECT_NAME}) ARCHIVE DESTINATION lib/${PROJECT_NAME})
INSTALL(FILES ${headers} DESTINATION include/${PROJECT_NAME}) INSTALL(FILES ${headers_main} DESTINATION include/${PROJECT_NAME})
INSTALL(FILES ${headers_com} DESTINATION include/${PROJECT_NAME}/common_class)
INSTALL(FILES ${headers_sen} DESTINATION include/${PROJECT_NAME}/sensors)
INSTALL(FILES ${headers_det} DESTINATION include/${PROJECT_NAME}/detectors)
INSTALL(FILES ${headers_des} DESTINATION include/${PROJECT_NAME}/descriptors)
INSTALL(FILES ${headers_mat} DESTINATION include/${PROJECT_NAME}/matchers)
INSTALL(FILES ${headers_alg} DESTINATION include/${PROJECT_NAME}/algorithms)
INSTALL(FILES ../cmake_modules/Find${PROJECT_NAME}.cmake DESTINATION ${CMAKE_ROOT}/Modules/) INSTALL(FILES ../cmake_modules/Find${PROJECT_NAME}.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
INSTALL(FILES "${VU_CONFIG_DIR}/config.h" DESTINATION include/${PROJECT_NAME}/_internal) INSTALL(FILES "${VU_CONFIG_DIR}/config.h" DESTINATION include/${PROJECT_NAME}/_internal)
......
...@@ -121,7 +121,7 @@ void AlgorithmACTIVESEARCH::run(Buffer<FramePtr>& _frame_buff, const DetectorBas ...@@ -121,7 +121,7 @@ void AlgorithmACTIVESEARCH::run(Buffer<FramePtr>& _frame_buff, const DetectorBas
{ {
// Initialize vars if necessary // Initialize vars if necessary
if (!initialized_) if (!initialized_)
initAlg(_frame_buff.back()->image); initAlg(_frame_buff.back()->getImage());
switch (_frame_buff.size()) switch (_frame_buff.size())
{ {
...@@ -144,7 +144,7 @@ void AlgorithmACTIVESEARCH::detectNewFeatures(FramePtr& _frame, const DetectorBa ...@@ -144,7 +144,7 @@ void AlgorithmACTIVESEARCH::detectNewFeatures(FramePtr& _frame, const DetectorBa
cv::Rect roi; cv::Rect roi;
if (params_ptr_->draw_results) if (params_ptr_->draw_results)
image_draw_ = _frame->image.clone(); image_draw_ = _frame->getImage().clone();
for (unsigned int n_iterations = 0; max_new_features == 0 || n_iterations < max_new_features; n_iterations++) for (unsigned int n_iterations = 0; max_new_features == 0 || n_iterations < max_new_features; n_iterations++)
{ {
...@@ -156,8 +156,8 @@ void AlgorithmACTIVESEARCH::detectNewFeatures(FramePtr& _frame, const DetectorBa ...@@ -156,8 +156,8 @@ void AlgorithmACTIVESEARCH::detectNewFeatures(FramePtr& _frame, const DetectorBa
// drawRoi(image_draw_, roi, getName(), cv::Scalar(255,255,255)); // drawRoi(image_draw_, roi, getName(), cv::Scalar(255,255,255));
// Detect features in ROI // Detect features in ROI
kps = _det_ptr->detect(_frame->image, roi); kps = _det_ptr->detect(_frame->getImage(), roi);
desc = _des_ptr->getDescriptor(_frame->image, kps); desc = _des_ptr->getDescriptor(_frame->getImage(), kps);
if (kps.size() > 0) if (kps.size() > 0)
{ {
...@@ -174,8 +174,8 @@ void AlgorithmACTIVESEARCH::detectNewFeatures(FramePtr& _frame, const DetectorBa ...@@ -174,8 +174,8 @@ void AlgorithmACTIVESEARCH::detectNewFeatures(FramePtr& _frame, const DetectorBa
if(kps[0].response > params_ptr_->min_response_new_feature) if(kps[0].response > params_ptr_->min_response_new_feature)
{ {
// Keep feature // Keep feature
FeaturePtr feature_ptr = std::make_shared<Feature>(_frame->features.size(),kps[0],desc.row(index)); _frame->addKeyPoint(kps[0]);
_frame->features.push_back(feature_ptr); _frame->addDescriptor(desc.row(index));
// Hit cell // Hit cell
hitCell(kps[0]); hitCell(kps[0]);
...@@ -215,34 +215,37 @@ void AlgorithmACTIVESEARCH::trackFrame(const FramePtr& _frame_old, FramePtr& _tr ...@@ -215,34 +215,37 @@ void AlgorithmACTIVESEARCH::trackFrame(const FramePtr& _frame_old, FramePtr& _tr
clock_t tStart = clock(); clock_t tStart = clock();
if (params_ptr_->draw_results) if (params_ptr_->draw_results)
image_draw_ = _tracked_frame->image.clone(); image_draw_ = _tracked_frame->getImage().clone();
// Renew active ROI // Renew active ROI
renew(); renew();
KeyPointVector target_feature = _frame_old->getKeyPoints();
cv::Mat target_desc = _frame_old->getDescriptors();
// Try to track all previous features // Try to track all previous features
for(auto target_feature : _frame_old->features) for(int ii=0; ii<target_feature.size(); ++ii)
{ {
// Hit cell in the position of the targeted kp // Hit cell in the position of the targeted kp
hitCell(target_feature->kp); hitCell(target_feature[ii]);
// Set ROI // Set ROI
cv::Rect roi = setRoi(target_feature->kp.pt.x,target_feature->kp.pt.y, cell_size_(0), cell_size_(1)); cv::Rect roi = setRoi(target_feature[ii].pt.x,target_feature[ii].pt.y, cell_size_(0), cell_size_(1));
// // draw ROI // // draw ROI
// if (params_ptr_->draw_results) // if (params_ptr_->draw_results)
// drawRoi(image_draw_, roi, getName(), cv::Scalar(0,255,255)); // drawRoi(image_draw_, roi, getName(), cv::Scalar(0,255,255));
// Detect features in ROI // Detect features in ROI
KeyPointVector kps = _det_ptr->detect(_tracked_frame->image, roi); KeyPointVector kps = _det_ptr->detect(_tracked_frame->getImage(), roi);
cv::Mat desc = _des_ptr->getDescriptor(_tracked_frame->image, kps); cv::Mat desc = _des_ptr->getDescriptor(_tracked_frame->getImage(), kps);
// If there are keypoints around the candidate ROI // If there are keypoints around the candidate ROI
if(kps.size() > 0) if(kps.size() > 0)
{ {
// Match // Match
cv::DMatch match_data; cv::DMatch match_data;
double normalized_score = _mat_ptr->match(target_feature->desc, desc, _des_ptr->getSize(), match_data); double normalized_score = _mat_ptr->match(target_desc.row(ii), desc, _des_ptr->getSize(), match_data);
// If good match, keep feature // If good match, keep feature
if (normalized_score > params_ptr_->matcher_min_norm_score) if (normalized_score > params_ptr_->matcher_min_norm_score)
...@@ -250,22 +253,23 @@ void AlgorithmACTIVESEARCH::trackFrame(const FramePtr& _frame_old, FramePtr& _tr ...@@ -250,22 +253,23 @@ void AlgorithmACTIVESEARCH::trackFrame(const FramePtr& _frame_old, FramePtr& _tr
// introduce in list of tracked features // introduce in list of tracked features
cv::KeyPoint tracked_kp = kps[match_data.trainIdx]; cv::KeyPoint tracked_kp = kps[match_data.trainIdx];
cv::Mat tracked_desc = desc(cv::Rect(0,match_data.trainIdx,desc.cols,1)); cv::Mat tracked_desc = desc(cv::Rect(0,match_data.trainIdx,desc.cols,1));
_tracked_frame->features.push_back( std::make_shared<Feature>(_tracked_frame->features.size(), tracked_kp, tracked_desc) ); _tracked_frame->addKeyPoint(tracked_kp);
_tracked_frame->addDescriptor(tracked_desc);
// Draw // Draw
if (params_ptr_->draw_results) if (params_ptr_->draw_results)
{ {
cv::line(image_draw_, tracked_kp.pt, target_feature->kp.pt, cv::Scalar(0, 0, 255), 3); cv::line(image_draw_, tracked_kp.pt, target_feature[ii].pt, cv::Scalar(0, 0, 255), 3);
cv::circle(image_draw_, tracked_kp.pt, 5, cv::Scalar(255.0, 255.0, 0.0), -1, 8, 0); cv::circle(image_draw_, tracked_kp.pt, 5, cv::Scalar(255.0, 255.0, 0.0), -1, 8, 0);
cv::putText(image_draw_, std::to_string(target_feature->id), tracked_kp.pt, cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(0, 0, 0)); cv::putText(image_draw_, std::to_string(ii), tracked_kp.pt, cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(0, 0, 0));
} }
} }
} }
} }
// If not enough tracked features, add new ones // If not enough tracked features, add new ones
if (_tracked_frame->features.size() < params_ptr_->min_feat_track) if (_tracked_frame->getNumFeatures() < params_ptr_->min_feat_track)
detectNewFeatures(_tracked_frame, _det_ptr, _des_ptr, params_ptr_->max_new_features - _tracked_frame->features.size()); detectNewFeatures(_tracked_frame, _det_ptr, _des_ptr, params_ptr_->max_new_features - _tracked_frame->getNumFeatures());
else else
{ {
if (params_ptr_->draw_results) if (params_ptr_->draw_results)
......
...@@ -4,7 +4,6 @@ ...@@ -4,7 +4,6 @@
#include "../algorithm_base.h" #include "../algorithm_base.h"
#include "../algorithm_factory.h" #include "../algorithm_factory.h"
#include "../../common_class/buffer.h" #include "../../common_class/buffer.h"
#include "../../common_class/feature.h"
#include "../../common_class/frame.h" #include "../../common_class/frame.h"
#include "../../detectors/detector_base.h" #include "../../detectors/detector_base.h"
#include "../../descriptors/descriptor_base.h" #include "../../descriptors/descriptor_base.h"
......
...@@ -49,18 +49,19 @@ void AlgorithmOPTFLOWPYRLK::run(Buffer<FramePtr>& _frame_buff, const DetectorBas ...@@ -49,18 +49,19 @@ void AlgorithmOPTFLOWPYRLK::run(Buffer<FramePtr>& _frame_buff, const DetectorBas
void AlgorithmOPTFLOWPYRLK::detectNewFeatures(FramePtr& _frame, const DetectorBasePtr& _det_ptr) void AlgorithmOPTFLOWPYRLK::detectNewFeatures(FramePtr& _frame, const DetectorBasePtr& _det_ptr)
{ {
if (params_ptr_->draw_results) if (params_ptr_->draw_results)
image_draw_ = _frame->image.clone(); image_draw_ = _frame->getImage().clone();
// Convert to gray scale if necessary // Convert to gray scale if necessary
cv::Mat img_gray; cv::Mat img_gray;
if (_frame->image.channels() == 3) if (_frame->getImage().channels() == 3)
cv::cvtColor(_frame->image, img_gray, CV_RGB2GRAY); cv::cvtColor(_frame->getImage(), img_gray, CV_RGB2GRAY);
else else
img_gray = _frame->image.clone(); img_gray = _frame->getImage().clone();
KeyPointVector kps_new; KeyPointVector kps_new;
cv::Mat desc_new;
if (_frame->features.size() > 0) if (_frame->getNumFeatures() > 0)
{ {
// Get features again // Get features again
KeyPointVector kps = _det_ptr->detect(img_gray); KeyPointVector kps = _det_ptr->detect(img_gray);
...@@ -69,18 +70,16 @@ void AlgorithmOPTFLOWPYRLK::detectNewFeatures(FramePtr& _frame, const DetectorBa ...@@ -69,18 +70,16 @@ void AlgorithmOPTFLOWPYRLK::detectNewFeatures(FramePtr& _frame, const DetectorBa
KeyPointVector tracked_kps = _frame->getKeyPoints(); KeyPointVector tracked_kps = _frame->getKeyPoints();
std::vector<int> kps_idxs; std::vector<int> kps_idxs;
KeyPointVector kps_new = vecDifference(tracked_kps, kps, kps_idxs); KeyPointVector kps_new = vecDifference(tracked_kps, kps, kps_idxs);
desc_new = cv::Mat(kps_new.size(), 1, CV_32F); // Not used in this context.
cv::Mat desc_new = cv::Mat(kps_new.size(), 1, CV_32F); // Not used in this context.
std::vector<FeaturePtr> new_feat_vec = setFeatures( kps_new, desc_new, _frame->features.back()->id);
_frame->features.insert( _frame->features.end(), new_feat_vec.begin(), new_feat_vec.end() );
} }
else else
{ {
// Get full frame detections and descriptors // Get full frame detections and descriptors
kps_new = _det_ptr->detect(img_gray); kps_new = _det_ptr->detect(img_gray);
cv::Mat desc_new = cv::Mat(kps_new.size(), 1, CV_32F); // Not used in this context. desc_new = cv::Mat(kps_new.size(), 1, CV_32F); // Not used in this context.
_frame = setFrame(_frame->id, _frame->image, kps_new, desc_new);
} }
_frame->setKeyPoints(kps_new);
_frame->setDescriptors(desc_new);
if (params_ptr_->draw_results) if (params_ptr_->draw_results)
{ {
...@@ -97,12 +96,12 @@ void AlgorithmOPTFLOWPYRLK::trackFrame(const FramePtr& _frame_old, FramePtr& _tr ...@@ -97,12 +96,12 @@ void AlgorithmOPTFLOWPYRLK::trackFrame(const FramePtr& _frame_old, FramePtr& _tr
clock_t tStart = clock(); clock_t tStart = clock();
if (params_ptr_->draw_results) if (params_ptr_->draw_results)
image_draw_ = _tracked_frame->image.clone(); image_draw_ = _tracked_frame->getImage().clone();
// Convert to gray scale if necessary // Convert to gray scale if necessary
cv::Mat img_gray; cv::Mat img_gray;
if (_tracked_frame->image.channels() == 3) if (_tracked_frame->getImage().channels() == 3)
cv::cvtColor(_tracked_frame->image, img_gray, CV_RGB2GRAY); cv::cvtColor(_tracked_frame->getImage(), img_gray, CV_RGB2GRAY);
std::vector<uchar> kpt_is_found; // output status vector (of unsigned chars); each element of the vector is set to 1 if the flow for the corresponding features has been found, otherwise, it is set to 0. std::vector<uchar> kpt_is_found; // output status vector (of unsigned chars); each element of the vector is set to 1 if the flow for the corresponding features has been found, otherwise, it is set to 0.
std::vector<float> err; // output vector of errors std::vector<float> err; // output vector of errors
...@@ -125,15 +124,16 @@ void AlgorithmOPTFLOWPYRLK::trackFrame(const FramePtr& _frame_old, FramePtr& _tr ...@@ -125,15 +124,16 @@ void AlgorithmOPTFLOWPYRLK::trackFrame(const FramePtr& _frame_old, FramePtr& _tr
good_kpts.push_back(kp); good_kpts.push_back(kp);
cv::circle(image_draw_, pts_matched_in_frame[ii], 5, cv::Scalar(255.0, 255.0, 0.0), -1); cv::circle(image_draw_, pts_matched_in_frame[ii], 5, cv::Scalar(255.0, 255.0, 0.0), -1);
cv::line(image_draw_, pts_prev[ii], pts_matched_in_frame[ii], cv::Scalar(0, 0, 255), 3); cv::line(image_draw_, pts_prev[ii], pts_matched_in_frame[ii], cv::Scalar(0, 0, 255), 3);
cv::putText(image_draw_, std::to_string(_tracked_frame->features.size() + ii), pts_matched_in_frame[ii], cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(0, 0, 0)); cv::putText(image_draw_, std::to_string(ii), pts_matched_in_frame[ii], cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(0, 0, 0));
} }
} }
// Update // Update
cv::Mat desc_new = cv::Mat(good_kpts.size(), 1, CV_32F); // Not used in this context. cv::Mat desc_new = cv::Mat(good_kpts.size(), 1, CV_32F); // Not used in this context.
_tracked_frame->features = setFeatures( good_kpts, desc_new ); _tracked_frame->setKeyPoints(good_kpts);
_tracked_frame->setDescriptors(desc_new);
// If not enough tracked features, add new ones // If not enough tracked features, add new ones
if (_tracked_frame->features.size() < params_ptr_->min_feat_track) if (_tracked_frame->getNumFeatures() < params_ptr_->min_feat_track)
detectNewFeatures(_tracked_frame, _det_ptr); detectNewFeatures(_tracked_frame, _det_ptr);
else else
{ {
......
...@@ -5,7 +5,6 @@ ...@@ -5,7 +5,6 @@
#include "../algorithm_factory.h" #include "../algorithm_factory.h"
#include "../../common_class/buffer.h" #include "../../common_class/buffer.h"
#include "../../common_class/frame.h" #include "../../common_class/frame.h"
#include "../../common_class/feature.h"
#include "../../detectors/detector_base.h" #include "../../detectors/detector_base.h"
#include "../../descriptors/descriptor_base.h" #include "../../descriptors/descriptor_base.h"
#include "../../matchers/matcher_base.h" #include "../../matchers/matcher_base.h"
......
...@@ -47,33 +47,32 @@ void AlgorithmTRACKFEATURES::run(Buffer<FramePtr>& _frame_buff, const DetectorBa ...@@ -47,33 +47,32 @@ void AlgorithmTRACKFEATURES::run(Buffer<FramePtr>& _frame_buff, const DetectorBa
void AlgorithmTRACKFEATURES::detectNewFeatures(FramePtr& _frame, const DetectorBasePtr& _det_ptr, const DescriptorBasePtr& _des_ptr) void AlgorithmTRACKFEATURES::detectNewFeatures(FramePtr& _frame, const DetectorBasePtr& _det_ptr, const DescriptorBasePtr& _des_ptr)
{ {
if (params_ptr_->draw_results) if (params_ptr_->draw_results)
image_draw_ = _frame->image.clone(); image_draw_ = _frame->getImage().clone();
KeyPointVector kps_new; KeyPointVector kps_new;
cv::Mat desc; cv::Mat desc_new;
if (_frame->features.size() > 0)
if (_frame->getNumFeatures() > 0)
{ {
// Get features again // Get features again
KeyPointVector kps = _det_ptr->detect(_frame->image); KeyPointVector kps = _det_ptr->detect(_frame->getImage());
desc = _des_ptr->getDescriptor( _frame->image, kps); cv::Mat desc = _des_ptr->getDescriptor( _frame->getImage(), kps);
// Keep only new features // Keep only new features
std::vector<int> idxs_new; std::vector<int> idxs_new;
KeyPointVector tracked_kps = _frame->getKeyPoints(); KeyPointVector tracked_kps = _frame->getKeyPoints();
kps_new = vecDifference(tracked_kps, kps, idxs_new); kps_new = vecDifference(tracked_kps, kps, idxs_new);
cv::Mat desc_new(idxs_new.size(), desc.cols, desc.type());
for (int ii=0;ii<idxs_new.size();++ii) for (int ii=0;ii<idxs_new.size();++ii)
desc_new.row(ii) = desc.row(idxs_new[ii]); desc_new.push_back( desc.row(idxs_new[ii]) );
std::vector<FeaturePtr> new_feat_vec = setFeatures( kps_new, desc_new, _frame->features.back()->id);
_frame->features.insert( _frame->features.end(), new_feat_vec.begin(), new_feat_vec.end() );
} }
else else
{ {
// Get full frame detections and descriptors // Get full frame detections and descriptors
kps_new = _det_ptr->detect(_frame->image); kps_new = _det_ptr->detect(_frame->getImage());
desc = _des_ptr->getDescriptor( _frame->image, kps_new); desc_new = _des_ptr->getDescriptor( _frame->getImage(), kps_new);
_frame = setFrame(_frame->id, _frame->image, kps_new, desc);
} }
_frame->addKeyPoints(kps_new);
_frame->addDescriptors(desc_new);
if (params_ptr_->draw_results) if (params_ptr_->draw_results)
{ {
...@@ -89,27 +88,30 @@ void AlgorithmTRACKFEATURES::trackFrame(const FramePtr& _frame_old, FramePtr& _t ...@@ -89,27 +88,30 @@ void AlgorithmTRACKFEATURES::trackFrame(const FramePtr& _frame_old, FramePtr& _t
clock_t tStart = clock(); clock_t tStart = clock();
if (params_ptr_->draw_results) if (params_ptr_->draw_results)
image_draw_ = _tracked_frame->image.clone(); image_draw_ = _tracked_frame->getImage().clone();
KeyPointVector target_kps = _frame_old->getKeyPoints();
cv::Mat target_desc = _frame_old->getDescriptors();
for(auto target_feature : _frame_old->features) for(int ii = 1; ii < target_kps.size(); ++ii)
{ {
// Set a candidate ROI around the target feature // Set a candidate ROI around the target feature
cv::Rect roi = setRoi(target_feature->kp.pt.x,target_feature->kp.pt.y, params_ptr_->roi_width, params_ptr_->roi_height); cv::Rect roi = setRoi(target_kps[ii].pt.x, target_kps[ii].pt.y, params_ptr_->roi_width, params_ptr_->roi_height);
// // draw ROI // // draw ROI
// if (params_ptr_->draw_results) // if (params_ptr_->draw_results)
// drawRoi(image_draw_, roi, getName(), cv::Scalar(0,255,255)); // drawRoi(image_draw_, roi, getName(), cv::Scalar(0,255,255));
// Detect features in ROI // Detect features in ROI
KeyPointVector kps = _det_ptr->detect(_tracked_frame->image, roi); KeyPointVector kps = _det_ptr->detect(_tracked_frame->getImage(), roi);
cv::Mat desc = _des_ptr->getDescriptor(_tracked_frame->image, kps); cv::Mat desc = _des_ptr->getDescriptor(_tracked_frame->getImage(), kps);
// If there are keypoints aroud the candidate ROI // If there are keypoints aroud the candidate ROI
if(kps.size() > 0) if(kps.size() > 0)
{ {
// Match // Match
cv::DMatch match_data; cv::DMatch match_data;
double normalized_score = _mat_ptr->match(target_feature->desc, desc, _des_ptr->getSize(), match_data); double normalized_score = _mat_ptr->match(target_desc.row(ii), desc, _des_ptr->getSize(), match_data);
// If good match, keep feature // If good match, keep feature
if (normalized_score > params_ptr_->matcher_min_norm_score) if (normalized_score > params_ptr_->matcher_min_norm_score)
...@@ -117,21 +119,21 @@ void AlgorithmTRACKFEATURES::trackFrame(const FramePtr& _frame_old, FramePtr& _t ...@@ -117,21 +119,21 @@ void AlgorithmTRACKFEATURES::trackFrame(const FramePtr& _frame_old, FramePtr& _t
// introduce in list of tracked features // introduce in list of tracked features
cv::KeyPoint tracked_kp = kps[match_data.trainIdx]; cv::KeyPoint tracked_kp = kps[match_data.trainIdx];
cv::Mat tracked_desc = desc(cv::Rect(0,match_data.trainIdx,desc.cols,1)); cv::Mat tracked_desc = desc(cv::Rect(0,match_data.trainIdx,desc.cols,1));
_tracked_frame->features.push_back( std::make_shared<Feature>(_tracked_frame->features.size(), tracked_kp, tracked_desc) ); _tracked_frame->addKeyPoint(tracked_kp);
_tracked_frame->addDescriptor(tracked_desc);
// Draw // Draw
if (params_ptr_->draw_results) if (params_ptr_->draw_results)
{ {
cv::line(image_draw_, tracked_kp.pt, target_feature->kp.pt, cv::Scalar(0, 0, 255), 3); cv::line(image_draw_, tracked_kp.pt, target_kps[ii].pt, cv::Scalar(0, 0, 255), 3);
cv::circle(image_draw_, tracked_kp.pt, 5, cv::Scalar(255.0, 255.0, 0.0), -1, 8, 0); cv::circle(image_draw_, tracked_kp.pt, 5, cv::Scalar(255.0, 255.0, 0.0), -1, 8, 0);
cv::putText(image_draw_, std::to_string(target_feature->id), tracked_kp.pt, cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(0, 0, 0)); cv::putText(image_draw_, std::to_string(ii), tracked_kp.pt, cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(0, 0, 0));
} }
} }
} }
} }
// If not enough tracked features, add new ones // If not enough tracked features, add new ones
if (_tracked_frame->features.size() < params_ptr_->min_feat_track) if (_tracked_frame->getNumFeatures() < params_ptr_->min_feat_track)
detectNewFeatures(_tracked_frame, _det_ptr, _des_ptr); detectNewFeatures(_tracked_frame, _det_ptr, _des_ptr);
else else
{ {
......
...@@ -5,7 +5,6 @@ ...@@ -5,7 +5,6 @@
#include "../algorithm_factory.h" #include "../algorithm_factory.h"
#include "../../common_class/buffer.h" #include "../../common_class/buffer.h"
#include "../../common_class/frame.h" #include "../../common_class/frame.h"
#include "../../common_class/feature.h"
#include "../../detectors/detector_base.h" #include "../../detectors/detector_base.h"
#include "../../descriptors/descriptor_base.h" #include "../../descriptors/descriptor_base.h"
#include "../../matchers/matcher_base.h" #include "../../matchers/matcher_base.h"
......
#ifndef _FEATURE_H_
#define _FEATURE_H_
#include "../vision_utils.h"
// OpenCV
#include <opencv2/core/core.hpp>
namespace vision_utils {
VU_PTR_TYPEDEFS(Feature);
/**
* \brief Main class
*/
class Feature {
public:
/**
* \brief Constructor
*/
Feature(const int& _id, const cv::KeyPoint& _kp, const cv::Mat& _desc) : id(_id), kp(_kp), desc(_desc){};
/**
* \brief Destructor
*/
~Feature(void){};
int id; // Feature id
cv::KeyPoint kp; // Feature Keypoint
cv::Mat desc; // Feature descriptor
};
inline std::vector<FeaturePtr> setFeatures( const KeyPointVector& _kps, const cv::Mat& _desc, const int _idx_ini = 0)
{
std::vector<FeaturePtr> features_vec;
for (int ii = 0; ii < _kps.size(); ++ii)
features_vec.push_back( std::make_shared<Feature>( _idx_ini + ii, _kps[ii], _desc.row(ii) ) );
return features_vec;
}
} /* namespace vision_utils */
#endif /* _FEATURE_H_ */
...@@ -2,7 +2,6 @@ ...@@ -2,7 +2,6 @@
#define _FRAME_H_ #define _FRAME_H_
#include "../vision_utils.h" #include "../vision_utils.h"
#include "feature.h"
// OpenCV // OpenCV
#include <opencv2/core/core.hpp> #include <opencv2/core/core.hpp>
...@@ -16,23 +15,65 @@ VU_PTR_TYPEDEFS(Frame); ...@@ -16,23 +15,65 @@ VU_PTR_TYPEDEFS(Frame);
*/ */
class Frame { class Frame {
protected:
int id_; // id
cv::Mat img_; // Image
KeyPointVector kps_; // KeyPoints
cv::Mat desc_; // Descriptors
public: public:
/** /**
* \brief Constructor * \brief Constructor
*/ */
Frame(const int& _id, const cv::Mat _img, const std::vector<FeaturePtr> _features) : id(_id), image(_img), features(_features){}; Frame(const int& _id, const cv::Mat _img, const KeyPointVector _kps, const cv::Mat _desc) :
id_(_id),
img_(_img),
kps_(_kps),
desc_(_desc){};
/** /**
* \brief Destructor * \brief Destructor
*/ */
~Frame(void){}; ~Frame(void){};
/**
* \brief Return the frame id
*/
int getId(void);
/**
* \brief Set the frame id
*/
void setId(const int& _id);
/**
* \brief Return the image
*/
cv::Mat getImage(void);
/**
* \brief Set the image
*/
void setImage(const cv::Mat& _img);
/** /**
* \brief Return the frame Keypoints * \brief Return the frame Keypoints
*/ */
KeyPointVector getKeyPoints(void); KeyPointVector getKeyPoints(void);
/**
* \brief Set the frame Keypoints
*/
void setKeyPoints(const KeyPointVector& _kps);
/**
* \brief Add a Keypoint
*/
void addKeyPoint(const cv::KeyPoint& _kp);
void addKeyPoints(const KeyPointVector& _kps);
/** /**
* \brief Return the frame points from KeyPoints * \brief Return the frame points from KeyPoints
*/ */
...@@ -43,38 +84,101 @@ public: ...@@ -43,38 +84,101 @@ public:
*/ */
cv::Mat getDescriptors(void); cv::Mat getDescriptors(void);
int id; // id /**
cv::Mat image; // image * \brief Set the frame descriptors
std::vector<FeaturePtr> features; // features found in frame */
void setDescriptors(const cv::Mat& _desc);
/**
* \brief Add a descriptor
*/
void addDescriptor(const cv::Mat& _kp);
void addDescriptors(const cv::Mat& _kp);
/**
* \brief Get number of features
*/
int getNumFeatures(void);
}; };
inline int Frame::getId(void)
{
return id_;
}
inline void Frame::setId(const int& _id)
{
id_ = _id;
}
inline cv::Mat Frame::getImage(void)
{
return img_;
}
inline void Frame::setImage(const cv::Mat& _img)
{
img_ = _img;
}
inline KeyPointVector Frame::getKeyPoints(void) inline KeyPointVector Frame::getKeyPoints(void)
{ {
KeyPointVector kps; return kps_;
for (auto feat : features)
kps.push_back(feat->kp);
return kps;
}; };
inline void Frame::setKeyPoints(const KeyPointVector& _kps)
{
kps_ = _kps;
}
inline void Frame::addKeyPoint(const cv::KeyPoint& _kp)
{
kps_.push_back(_kp);
}
inline void Frame::addKeyPoints(const KeyPointVector& _kps)
{
kps_.insert(kps_.end(), _kps.begin(), _kps.end() );
}
inline PointVector Frame::getPoints(void) inline PointVector Frame::getPoints(void)
{ {
PointVector ps; PointVector pvec;
for (auto feat : features) for (auto kp : kps_)
ps.push_back(feat->kp.pt); pvec.push_back(kp.pt);
return ps; return pvec;
}; };
inline cv::Mat Frame::getDescriptors(void) inline cv::Mat Frame::getDescriptors(void)
{ {
cv::Mat desc; return desc_;
for (auto feat : features) };
desc.push_back(feat->desc);
return desc; inline void Frame::setDescriptors(const cv::Mat& _desc)
{
assert( kps_.size() == _desc.rows && "The Number of descriptors must be equal to number of keypoints");
desc_ = _desc;
}; };
inline void Frame::addDescriptor(const cv::Mat& _desc)
{
desc_.push_back(_desc);
}
inline void Frame::addDescriptors(const cv::Mat& _desc)
{
desc_.push_back(_desc);
}
inline int Frame::getNumFeatures(void)
{
assert( kps_.size() == desc_.rows && "The Number of descriptors must be equal to number of keypoints");
return kps_.size();
}
inline FramePtr setFrame(const int& _id, const cv::Mat _img, const KeyPointVector& _kps = KeyPointVector(), const cv::Mat& _desc = cv::Mat()) inline FramePtr setFrame(const int& _id, const cv::Mat _img, const KeyPointVector& _kps = KeyPointVector(), const cv::Mat& _desc = cv::Mat())
{ {
return std::make_shared<Frame>(_id, _img, setFeatures( _kps, _desc)); return std::make_shared<Frame>(_id, _img, _kps, _desc );
} }
} /* namespace vision_utils */ } /* namespace vision_utils */
......
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