diff --git a/src/algorithms/trackfeatures/alg_trackfeatures.cpp b/src/algorithms/trackfeatures/alg_trackfeatures.cpp index add44d85cc6250eaa71224daabb283821a9c5234..37078abb7c641869f03e5686e600b86aa7f3362f 100644 --- a/src/algorithms/trackfeatures/alg_trackfeatures.cpp +++ b/src/algorithms/trackfeatures/alg_trackfeatures.cpp @@ -4,109 +4,109 @@ namespace vision_utils { AlgorithmTRACKFEATURES::AlgorithmTRACKFEATURES(void) { - initialized_ = false; + initialized_ = false; } AlgorithmTRACKFEATURES::~AlgorithmTRACKFEATURES(void) { if (params_ptr_->draw_results) - cv::destroyAllWindows(); + cv::destroyAllWindows(); } void AlgorithmTRACKFEATURES::initAlg(void) { - if (params_ptr_->draw_results) - { - cv::startWindowThread(); - cv::namedWindow(getName(), cv::WINDOW_NORMAL); - } + if (params_ptr_->draw_results) + { + cv::startWindowThread(); + cv::namedWindow(getName(), cv::WINDOW_NORMAL); + } initialized_ = true; } void AlgorithmTRACKFEATURES::run(Buffer<FramePtr>& _frame_buff, const DetectorBasePtr& _det_ptr, const DescriptorBasePtr& _des_ptr, const MatcherBasePtr& _mat_ptr) { - // Initialize vars if necessary - if (!initialized_) - initAlg(); - - switch (_frame_buff.size()) - { - case 0: - VU_ERROR("Trying to track an empty buffer. Please, at least insert one image in the buffer."); - break; - case 1: - detectNewFeatures(_frame_buff.back(), _det_ptr, _des_ptr); - break; - default: - trackFrame(_frame_buff[_frame_buff.size()-2], _frame_buff.back(), _det_ptr, _des_ptr, _mat_ptr); - break; - } + // Initialize vars if necessary + if (!initialized_) + initAlg(); + + switch (_frame_buff.size()) + { + case 0: + VU_ERROR("Trying to track an empty buffer. Please, at least insert one image in the buffer."); + break; + case 1: + detectNewFeatures(_frame_buff.back(), _det_ptr, _des_ptr); + break; + default: + trackFrame(_frame_buff[_frame_buff.size()-2], _frame_buff.back(), _det_ptr, _des_ptr, _mat_ptr); + break; + } } void AlgorithmTRACKFEATURES::detectNewFeatures(FramePtr& _frame, const DetectorBasePtr& _det_ptr, const DescriptorBasePtr& _des_ptr) { if (params_ptr_->draw_results) - image_draw_ = _frame->getImage().clone(); - - KeyPointVector kps_new; - cv::Mat desc_new; - - if (_frame->getNumFeatures() > 0) - { - // Get features again - KeyPointVector kps = _det_ptr->detect(_frame->getImage()); - cv::Mat desc = _des_ptr->getDescriptor( _frame->getImage(), kps); - - // Keep only new features - std::vector<int> idxs_new; - KeyPointVector tracked_kps = _frame->getKeyPoints(); - kps_new = differenceKPVec(tracked_kps, kps, idxs_new); - for (int ii=0;ii<idxs_new.size();++ii) - desc_new.push_back( desc.row(idxs_new[ii]) ); - } - else - { - // Get full frame detections and descriptors - kps_new = _det_ptr->detect(_frame->getImage()); - desc_new = _des_ptr->getDescriptor( _frame->getImage(), kps_new); - } - _frame->addKeyPoints(kps_new); - _frame->addDescriptors(desc_new); + image_draw_ = _frame->getImage().clone(); + + KeyPointVector kps_new; + cv::Mat desc_new; + + if (_frame->getNumFeatures() > 0) + { + // Get features again + KeyPointVector kps = _det_ptr->detect(_frame->getImage()); + cv::Mat desc = _des_ptr->getDescriptor( _frame->getImage(), kps); + + // Keep only new features + std::vector<int> idxs_new; + KeyPointVector tracked_kps = _frame->getKeyPoints(); + kps_new = differenceKPVec(tracked_kps, kps, idxs_new); + for (int ii=0;ii<idxs_new.size();++ii) + desc_new.push_back( desc.row(idxs_new[ii]) ); + } + else + { + // Get full frame detections and descriptors + kps_new = _det_ptr->detect(_frame->getImage()); + desc_new = _des_ptr->getDescriptor( _frame->getImage(), kps_new); + } + _frame->addKeyPoints(kps_new); + _frame->addDescriptors(desc_new); if (params_ptr_->draw_results) { - drawKeyPoints(image_draw_, kps_new, 5, cv::Scalar(255,0,100), -1); - cv::imshow(getName(), image_draw_); - cv::waitKey(1); + drawKeyPoints(image_draw_, kps_new, 5, cv::Scalar(255,0,100), -1); + cv::imshow(getName(), image_draw_); + cv::waitKey(1); } } void AlgorithmTRACKFEATURES::trackFrame(const FramePtr& _frame_old, FramePtr& _tracked_frame, const DetectorBasePtr& _det_ptr, const DescriptorBasePtr& _des_ptr, const MatcherBasePtr& _mat_ptr) { - // Start timer - clock_t tStart = clock(); + // Start timer + clock_t tStart = clock(); if (params_ptr_->draw_results) - image_draw_ = _tracked_frame->getImage().clone(); + image_draw_ = _tracked_frame->getImage().clone(); KeyPointVector target_kps = _frame_old->getKeyPoints(); cv::Mat target_desc = _frame_old->getDescriptors(); - for(int ii = 1; ii < target_kps.size(); ++ii) + for(int ii = 1; ii < target_kps.size(); ++ii) { // Set a candidate ROI around the target feature - cv::Rect roi = setRoi(target_kps[ii].pt.x, target_kps[ii].pt.y, params_ptr_->target_roi_height, params_ptr_->target_roi_width); + cv::Rect roi = setRoi(target_kps[ii].pt.x, target_kps[ii].pt.y, params_ptr_->target_roi_height, params_ptr_->target_roi_width); // // draw ROI -// if (params_ptr_->draw_results) -// drawRoi(image_draw_, roi, getName(), cv::Scalar(0,255,255)); +// if (params_ptr_->draw_results) +// drawRoi(image_draw_, roi, getName(), cv::Scalar(0,255,255)); - // Detect features in ROI - KeyPointVector kps = _det_ptr->detect(_tracked_frame->getImage(), roi); - cv::Mat desc = _des_ptr->getDescriptor(_tracked_frame->getImage(), kps); + // Detect features in ROI + KeyPointVector kps = _det_ptr->detect(_tracked_frame->getImage(), roi); + 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) { // Match @@ -116,36 +116,36 @@ void AlgorithmTRACKFEATURES::trackFrame(const FramePtr& _frame_old, FramePtr& _t // If good match, keep feature if (normalized_score > _mat_ptr->getParams()->min_norm_score) { - // introduce in list of tracked features - cv::KeyPoint tracked_kp = kps[match_data.trainIdx]; + // introduce in list of tracked features + cv::KeyPoint tracked_kp = kps[match_data.trainIdx]; cv::Mat tracked_desc = desc(cv::Rect(0,match_data.trainIdx,desc.cols,1)); _tracked_frame->addKeyPoint(tracked_kp); _tracked_frame->addDescriptor(tracked_desc); // Draw if (params_ptr_->draw_results) { - 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::putText(image_draw_, std::to_string(ii), tracked_kp.pt, cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(0, 0, 0)); + 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::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 (_tracked_frame->getNumFeatures() < params_ptr_->min_feat_track) - detectNewFeatures(_tracked_frame, _det_ptr, _des_ptr); - else - { - if (params_ptr_->draw_results) - { - cv::imshow(getName(), image_draw_); - cv::waitKey(1); - } - } - - // Get computation time - comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC; + // If not enough tracked features, add new ones + if (_tracked_frame->getNumFeatures() < params_ptr_->min_feat_track) + detectNewFeatures(_tracked_frame, _det_ptr, _des_ptr); + else + { + if (params_ptr_->draw_results) + { + cv::imshow(getName(), image_draw_); + cv::waitKey(1); + } + } + + // Get computation time + comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC; } } /* namespace vision_utils */