public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

robastest verion till 31 Agu

Browse Source
This commit is contained in:
gfuse
2021-08-31 22:09:08 +02:00
parent 034158ef39
commit a6de3e3168
7 changed files with 155 additions and 291 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/abstract_camera.h
+2 -2
View File
@@ -9,6 +9,7 @@
#define ABSTRACT_CAMERA_H_ #define ABSTRACT_CAMERA_H_
#include <Eigen/Core> #include <Eigen/Core>
#include <iostream>
namespace vk namespace vk
{ {
@@ -66,8 +67,7 @@ public:
inline bool isInFrame(const Vector2i &obs, int boundary, int level) const inline bool isInFrame(const Vector2i &obs, int boundary, int level) const
{ {
if(obs[0] >= boundary && obs[0] < width()/(1<<level)-boundary if(obs[0] >= boundary && obs.x() < width()/(1<<level)-boundary && obs[1] >= boundary && obs.y() <height()/(1<<level)-boundary)
&& obs[1] >= boundary && obs[1] <height()/(1<<level)-boundary)
return true; return true;
return false; return false;
} }
GPU_version/vio/include/vio/reprojector.h
-6
View File
@@ -65,11 +65,6 @@ public:
FramePtr frame, FramePtr frame,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs); std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs);
void reprojectMap1(
FramePtr frame,
FramePtr framel,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs);
void reprojectMap2( void reprojectMap2(
FramePtr frame, FramePtr frame,
FramePtr last_frame, FramePtr last_frame,
@@ -107,7 +102,6 @@ private:
void resetGrid(); void resetGrid();
bool reprojectCell(Cell& cell, FramePtr frame); bool reprojectCell(Cell& cell, FramePtr frame);
bool reprojectPoint(FramePtr frame, Point* point); bool reprojectPoint(FramePtr frame, Point* point);
bool reprojectPoint1(FramePtr frame, Point* point, cv::KeyPoint* px_cv);
}; };
} // namespace vio } // namespace vio
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch> <launch>
<node pkg="vio" type="vio" name="vio" clear_params="true" output="screen"/> <node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" launch-prefix="xterm -e gdb --args" />
<!-- Camera topic to subscribe to --> <!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" /> <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/src/frame_handler_mono.cpp
+2 -2
View File
@@ -168,8 +168,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
// sparse image align // sparse image align
//SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false); //SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false);
//assert(new_frame_.get()== nullptr); //assert(new_frame_.get()== nullptr);
//SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_); SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
//if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE; if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE;
//reprojector_.reprojectMap(new_frame_, overlap_kfs_); //reprojector_.reprojectMap(new_frame_, overlap_kfs_);
// reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_); // reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_);
reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_); reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_);
GPU_version/vio/src/matcher.cpp
+4 -6
View File
@@ -140,23 +140,21 @@ bool Matcher::findMatchDirect(
const Frame& cur_frame, const Frame& cur_frame,
Vector2d& px_cur) Vector2d& px_cur)
{ {
if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_)) ref_ftr_= nullptr;
return false; if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))return false;
if(!ref_ftr_->frame->cam_->isInFrame( if(!ref_ftr_->frame->cam_->isInFrame(ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level))
ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level))
return false; return false;
if(ref_ftr_->frame->img_pyr_.empty())return false; if(ref_ftr_->frame->img_pyr_.empty())return false;
if(ref_ftr_->frame->img_pyr_[ref_ftr_->level].empty())return false; if(ref_ftr_->frame->img_pyr_[ref_ftr_->level].empty())return false;
// warp affine // warp affine
warp::getWarpMatrixAffine( warp::getWarpMatrixAffine(
*ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f, *ref_ftr_->frame->cam_, *(cur_frame.cam_), ref_ftr_->px, ref_ftr_->f,
(Vector3d(ref_ftr_->frame->pos()(0),0.0,ref_ftr_->frame->pos()(1)) - pt.pos_).norm(), (Vector3d(ref_ftr_->frame->pos()(0),0.0,ref_ftr_->frame->pos()(1)) - pt.pos_).norm(),
cur_frame.se3() * ref_ftr_->frame->getSE3Inv(), ref_ftr_->level, A_cur_ref_); cur_frame.se3() * ref_ftr_->frame->getSE3Inv(), ref_ftr_->level, A_cur_ref_);
search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1); search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
if(!warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, if(!warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_))return false; ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_))return false;
createPatchFromPatchWithBorder(); createPatchFromPatchWithBorder();
// px_cur should be set // px_cur should be set
Vector2d px_scaled(px_cur/(1<<search_level_)); Vector2d px_scaled(px_cur/(1<<search_level_));
GPU_version/vio/src/point.cpp
+7 -9
View File
@@ -98,20 +98,18 @@ void Point::initNormal()
bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const
{ {
// TODO: get frame with same point of view AND same pyramid level! // TODO: get frame with same point of view AND same pyramid level!
Vector3d obs_dir(Vector3d(framepos(0),0.0,framepos(1)) - pos_); obs_dir.normalize(); Vector3d obs_dir(Vector3d(framepos(0),1e-9,framepos(1)) + pos_); obs_dir.normalize();
auto max_it=obs_.begin(); double max_cos_angle = 1.0;
double max_cos_angle = 1; if(obs_.empty())return false;
for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) for(auto&& ob:obs_){
{ Vector3d dir(Vector3d(ob->frame->pos()(0),1e-9,ob->frame->pos()(1)) + pos_);
Vector3d dir(Vector3d((*it)->frame->pos()(0),0.0,(*it)->frame->pos()(1)) - pos_); dir.normalize(); double cos_angle = obs_dir.dot(dir.normalized());
double cos_angle = obs_dir.dot(dir);
if(cos_angle < max_cos_angle) if(cos_angle < max_cos_angle)
{ {
max_cos_angle = cos_angle; max_cos_angle = cos_angle;
max_it = it; ftr = ob;
} }
} }
ftr = *max_it;
return true; return true;
} }
GPU_version/vio/src/reprojector.cpp
+35 -161
View File
@@ -36,18 +36,15 @@
namespace vio { namespace vio {
Reprojector::Reprojector(vk::AbstractCamera *cam, Map &map) : Reprojector::Reprojector(vk::AbstractCamera *cam, Map &map) :
map_(map) map_(map) {
{
initializeGrid(cam); initializeGrid(cam);
} }
Reprojector::~Reprojector() Reprojector::~Reprojector() {
{
std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell *c) { delete c; }); std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell *c) { delete c; });
} }
void Reprojector::initializeGrid(vk::AbstractCamera* cam) void Reprojector::initializeGrid(vk::AbstractCamera *cam) {
{
grid_.cell_size = Config::gridSize(); grid_.cell_size = Config::gridSize();
grid_.grid_n_cols = ceil(static_cast<double>(cam->width()) / grid_.cell_size); grid_.grid_n_cols = ceil(static_cast<double>(cam->width()) / grid_.cell_size);
grid_.grid_n_rows = ceil(static_cast<double>(cam->height()) / grid_.cell_size); grid_.grid_n_rows = ceil(static_cast<double>(cam->height()) / grid_.cell_size);
@@ -59,18 +56,17 @@ void Reprojector::initializeGrid(vk::AbstractCamera* cam)
random_shuffle(grid_.cell_order.begin(), grid_.cell_order.end()); // maybe we should do it at every iteration! random_shuffle(grid_.cell_order.begin(), grid_.cell_order.end()); // maybe we should do it at every iteration!
} }
void Reprojector::resetGrid() void Reprojector::resetGrid() {
{
n_matches_ = 0; n_matches_ = 0;
n_trials_ = 0; n_trials_ = 0;
std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell *c) { c->clear(); }); std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell *c) { c->clear(); });
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO - Original implementation // TODO - Original implementation
void Reprojector::reprojectMap( void Reprojector::reprojectMap(
FramePtr frame, FramePtr frame,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs) std::vector<std::pair<FramePtr, std::size_t> > &overlap_kfs) {
{
resetGrid(); resetGrid();
list<pair<FramePtr, double> > close_kfs; list<pair<FramePtr, double> > close_kfs;
@@ -82,14 +78,12 @@ void Reprojector::reprojectMap(
size_t n = 0; size_t n = 0;
overlap_kfs.reserve(options_.max_n_kfs); overlap_kfs.reserve(options_.max_n_kfs);
for (auto it_frame = close_kfs.begin(), ite_frame = close_kfs.end(); for (auto it_frame = close_kfs.begin(), ite_frame = close_kfs.end();
it_frame!=ite_frame && n<options_.max_n_kfs; ++it_frame, ++n) it_frame != ite_frame && n < options_.max_n_kfs; ++it_frame, ++n) {
{
FramePtr ref_frame = it_frame->first; FramePtr ref_frame = it_frame->first;
overlap_kfs.push_back(pair<FramePtr, size_t>(ref_frame, 0)); overlap_kfs.push_back(pair<FramePtr, size_t>(ref_frame, 0));
for (auto it_ftr = ref_frame->fts_.begin(), ite_ftr = ref_frame->fts_.end(); for (auto it_ftr = ref_frame->fts_.begin(), ite_ftr = ref_frame->fts_.end();
it_ftr!=ite_ftr; ++it_ftr) it_ftr != ite_ftr; ++it_ftr) {
{
if ((*it_ftr)->point == NULL) if ((*it_ftr)->point == NULL)
continue; continue;
if ((*it_ftr)->point->last_frame_overlap_id_ == frame->id_) if ((*it_ftr)->point->last_frame_overlap_id_ == frame->id_)
@@ -104,13 +98,10 @@ void Reprojector::reprojectMap(
{ {
boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_); boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
auto it = map_.point_candidates_.candidates_.begin(); auto it = map_.point_candidates_.candidates_.begin();
while(it!=map_.point_candidates_.candidates_.end()) while (it != map_.point_candidates_.candidates_.end()) {
{ if (!reprojectPoint(frame, it->first)) {
if(!reprojectPoint(frame, it->first))
{
it->first->n_failed_reproj_ += 3; it->first->n_failed_reproj_ += 3;
if(it->first->n_failed_reproj_ > 30) if (it->first->n_failed_reproj_ > 30) {
{
map_.point_candidates_.deleteCandidate(*it); map_.point_candidates_.deleteCandidate(*it);
it = map_.point_candidates_.candidates_.erase(it); it = map_.point_candidates_.candidates_.erase(it);
continue; continue;
@@ -119,8 +110,7 @@ void Reprojector::reprojectMap(
++it; ++it;
} }
} }
for(size_t i=0; i<grid_.cells.size(); ++i) for (size_t i = 0; i < grid_.cells.size(); ++i) {
{
if (reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame)) if (reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
++n_matches_; ++n_matches_;
if (n_matches_ > (size_t) Config::maxFts()) if (n_matches_ > (size_t) Config::maxFts())
@@ -128,92 +118,12 @@ void Reprojector::reprojectMap(
} }
} }
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO - First reimplementation
void Reprojector::reprojectMap1(
FramePtr frame,
FramePtr framel,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)
{
resetGrid();
std::vector<cv::KeyPoint> keypoints_ref;
cv::KeyPoint * kp = new cv::KeyPoint;
for(auto it_ftr=framel->fts_.begin(), ite_ftr=framel->fts_.end();
it_ftr!=ite_ftr; ++it_ftr)
{
kp->pt.x = (*it_ftr)->px.x();
kp->pt.y = (*it_ftr)->px.y();
keypoints_ref.push_back(*kp);
}
overlap_kfs.reserve(1);
overlap_kfs.push_back(pair<FramePtr,size_t>(frame,0));
cv::Mat descriptors_ref, descriptors_cur;
std::vector<cv::KeyPoint> keypoints_cur;
cv::FAST(frame->img(),keypoints_cur,50);
std::vector<cv::DMatch> matches;
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 4);
extractor->compute(framel->img(), keypoints_ref, descriptors_ref);
extractor->compute(frame->img(), keypoints_cur, descriptors_cur);
cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
matcher->match(descriptors_ref,descriptors_cur,matches);
//std::cerr<<"matches size : "<<matches.size()<<std::endl;
for(int i=0; i < matches.size(); i++)
{
auto fts = framel->fts_.begin();
auto fts2 = fts;
for(int f = 0; f < matches[i].queryIdx; f++)
fts2++;
if(reprojectPoint1(frame, (*fts2)->point, &keypoints_cur[i])){
overlap_kfs.back().second++;
}
keypoints_cur[i].class_id = 0;
}
for(int i=0; i < keypoints_cur.size(); i++)
{
if(keypoints_cur[i].class_id == 0)
continue;
if(reprojectPoint1(frame, nullptr, &keypoints_cur[i]))
overlap_kfs.back().second++;
}
{
boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
auto it=map_.point_candidates_.candidates_.begin();
while(it!=map_.point_candidates_.candidates_.end())
{
if(!reprojectPoint(frame, it->first))
{
it->first->n_failed_reproj_ += 3;
if(it->first->n_failed_reproj_ > 30)
{
map_.point_candidates_.deleteCandidate(*it);
it = map_.point_candidates_.candidates_.erase(it);
continue;
}
}
++it;
}
}
for(size_t i=0; i<grid_.cells.size(); ++i)
{
if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
++n_matches_;
if(n_matches_ > (size_t) Config::maxFts())
break;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO - second reimplementation // TODO - second reimplementation
void Reprojector::reprojectMap2( void Reprojector::reprojectMap2(
FramePtr frame, FramePtr frame,
FramePtr last_frame, FramePtr last_frame,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs) std::vector<std::pair<FramePtr, std::size_t> > &overlap_kfs) {
{
resetGrid(); resetGrid();
cv::Mat descriptors_cur; cv::Mat descriptors_cur;
std::vector<cv::KeyPoint> keypoints_cur; std::vector<cv::KeyPoint> keypoints_cur;
@@ -223,7 +133,9 @@ void Reprojector::reprojectMap(
extractor->compute(frame->img(), keypoints_cur, descriptors_cur); extractor->compute(frame->img(), keypoints_cur, descriptors_cur);
list<pair<FramePtr, double> > close_kfs; list<pair<FramePtr, double> > close_kfs;
map_.getCloseKeyframes(frame, close_kfs); map_.getCloseKeyframes(frame, close_kfs);
if(!last_frame->fts_.empty())close_kfs.push_back(pair<FramePtr,double>(last_frame, (frame->T_f_w_.se2().translation()-last_frame->T_f_w_.se2().translation()).norm())); if (!last_frame->fts_.empty())
close_kfs.push_back(pair<FramePtr, double>(last_frame, (frame->T_f_w_.se2().translation() -
last_frame->T_f_w_.se2().translation()).norm()));
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) < close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::second, _2)); boost::bind(&std::pair<FramePtr, double>::second, _2));
overlap_kfs.reserve(options_.max_n_kfs); overlap_kfs.reserve(options_.max_n_kfs);
@@ -233,62 +145,59 @@ void Reprojector::reprojectMap(
std::vector<cv::DMatch> matches; std::vector<cv::DMatch> matches;
cv::Mat descriptors_ref; cv::Mat descriptors_ref;
overlap_kfs.push_back(pair<FramePtr, size_t>(it_frame.item.first, 0)); overlap_kfs.push_back(pair<FramePtr, size_t>(it_frame.item.first, 0));
for(auto&& it_ftr:it_frame.item.first->fts_) for (auto &&it_ftr:it_frame.item.first->fts_) {
{
keypoints_kfs.push_back(cv::KeyPoint(it_ftr->px.x(), it_ftr->px.y(), 1)); keypoints_kfs.push_back(cv::KeyPoint(it_ftr->px.x(), it_ftr->px.y(), 1));
} }
extractor->compute(it_frame.item.first->img(), keypoints_kfs, descriptors_ref); extractor->compute(it_frame.item.first->img(), keypoints_kfs, descriptors_ref);
matcher->match(descriptors_ref, descriptors_cur, matches); matcher->match(descriptors_ref, descriptors_cur, matches);
for (auto &&f: _for(it_frame.item.first->fts_)) { for (auto &&f: _for(it_frame.item.first->fts_)) {
if(f.item->point==NULL)f.item->point=new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(),f.item->px.y())); if (f.item->point == NULL)
f.item->point = new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item);
if (f.item->point->last_frame_overlap_id_ == frame->id_)continue; if (f.item->point->last_frame_overlap_id_ == frame->id_)continue;
f.item->point->last_frame_overlap_id_ = frame->id_; f.item->point->last_frame_overlap_id_ = frame->id_;
for (auto &&match:matches) { for (auto &&match:matches) {
if (match.queryIdx != f.index)continue; if (match.queryIdx != f.index)continue;
assert(keypoints_cur.size() > match.trainIdx); assert(keypoints_cur.size() > match.trainIdx);
const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) * grid_.grid_n_cols const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) *
grid_.grid_n_cols
+ static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size); + static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
assert(grid_.cells.at(k) != nullptr); assert(grid_.cells.at(k) != nullptr);
Vector2d px((int)keypoints_cur.at(match.trainIdx).pt.x,(int)keypoints_cur.at(match.trainIdx).pt.y); Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
(int) keypoints_cur.at(match.trainIdx).pt.y);
grid_.cells.at(k)->push_back(Candidate(f.item->point, px)); grid_.cells.at(k)->push_back(Candidate(f.item->point, px));
overlap_kfs.back().second++; overlap_kfs.back().second++;
break; break;
} }
} }
} }
for(size_t i=0; i<grid_.cells.size(); ++i) for (size_t i = 0; i < grid_.cells.size(); ++i) {
{
if (reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame)) if (reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
++n_matches_; ++n_matches_;
if (n_matches_ > (size_t) Config::maxFts()) if (n_matches_ > (size_t) Config::maxFts())
break; break;
} }
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Reprojector::pointQualityComparator(Candidate& lhs, Candidate& rhs) bool Reprojector::pointQualityComparator(Candidate &lhs, Candidate &rhs) {
{
if (lhs.pt->type_ > rhs.pt->type_) if (lhs.pt->type_ > rhs.pt->type_)
return true; return true;
return false; return false;
} }
bool Reprojector::reprojectCell(Cell& cell, FramePtr frame) bool Reprojector::reprojectCell(Cell &cell, FramePtr frame) {
{
cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2)); cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
Cell::iterator it = cell.begin(); Cell::iterator it = cell.begin();
while(it!=cell.end()) while (it != cell.end()) {
{
++n_trials_; ++n_trials_;
if(it->pt->type_ == Point::TYPE_DELETED) if (it->pt->type_ == Point::TYPE_DELETED) {
{
it = cell.erase(it); it = cell.erase(it);
continue; continue;
}/* }
bool found_match = true; bool found_match = true;
if (options_.find_match_direct) if (options_.find_match_direct)
found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px); found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
if(!found_match) if (!found_match) {
{
it->pt->n_failed_reproj_++; it->pt->n_failed_reproj_++;
if (it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15) if (it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
map_.safeDeletePoint(it->pt); map_.safeDeletePoint(it->pt);
@@ -296,7 +205,7 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
map_.point_candidates_.deleteCandidatePoint(it->pt); map_.point_candidates_.deleteCandidatePoint(it->pt);
it = cell.erase(it); it = cell.erase(it);
continue; continue;
}*/ }
it->pt->n_succeeded_reproj_++; it->pt->n_succeeded_reproj_++;
if (it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10) if (it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
it->pt->type_ = Point::TYPE_GOOD; it->pt->type_ = Point::TYPE_GOOD;
@@ -308,8 +217,7 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
// round is only done if this frame is selected as keyframe. // round is only done if this frame is selected as keyframe.
new_feature->point = it->pt; new_feature->point = it->pt;
if(matcher_.ref_ftr_->type == Feature::EDGELET) if (matcher_.ref_ftr_->type == Feature::EDGELET) {
{
new_feature->type = Feature::EDGELET; new_feature->type = Feature::EDGELET;
new_feature->grad = matcher_.A_cur_ref_ * matcher_.ref_ftr_->grad; new_feature->grad = matcher_.A_cur_ref_ * matcher_.ref_ftr_->grad;
new_feature->grad.normalize(); new_feature->grad.normalize();
@@ -324,10 +232,10 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
} }
return false; return false;
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO original implementation // TODO original implementation
bool Reprojector::reprojectPoint(FramePtr frame, Point* point) bool Reprojector::reprojectPoint(FramePtr frame, Point *point) {
{
Vector2d px(frame->w2c(point->pos_)); Vector2d px(frame->w2c(point->pos_));
if (frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher if (frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
{ {
@@ -338,38 +246,4 @@ bool Reprojector::reprojectPoint(FramePtr frame, Point* point)
} }
return false; return false;
} }
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO first reimplementation
bool Reprojector::reprojectPoint1(FramePtr frame, Point* point, cv::KeyPoint* px_cv)
{
if(point != nullptr) {
Vector2d px((int)px_cv->pt.x, (int)px_cv->pt.y);
if (frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
{
const int k = static_cast<int>(px[1] / grid_.cell_size) * grid_.grid_n_cols
+ static_cast<int>(px[0] / grid_.cell_size);
if(k<grid_.cells.size()-1){
assert(grid_.cells.at(k)!= nullptr);
assert(point!=NULL);
grid_.cells.at(k)->push_back(Candidate(point, px));
return true;
} }
}
return false;
}
return false;/*else{
Vector2d px(x, y);
if (frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
{
const int k = static_cast<int>(px[1] / grid_.cell_size) * grid_.grid_n_cols
+ static_cast<int>(px[0] / grid_.cell_size);
frame->fts_.push_back(Fea)
grid_.cells.at(k)->push_back(Candidate(vio::Point(Vector3d(frame->cam_->cam2world(x, y))), px));
return true;
}
return false;
}*/
}
} // namespace vio
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////