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

segmentation fault close obs solved

Browse Source
This commit is contained in:
gfuse
2021-09-02 01:01:32 +02:00
parent 4d857d790c
commit 68cd6d9b48
6 changed files with 41 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/point.h
+1 -1
View File
@@ -78,7 +78,7 @@ public:
Feature* findFrameRef(Frame* frame);
/// Get Frame with similar viewpoint.
bool getCloseViewObs(const Vector2d& pos, Feature*& obs) const;
bool getCloseViewObs(const Vector2d& pos, Feature*& obs, int id=0) const;
/// Get number of observations.
inline size_t nRefs() const { return obs_.size(); }
GPU_version/vio/src/depth_filter.cpp
+1 -1
View File
@@ -299,7 +299,7 @@ namespace vio {
lock_t lock(seeds_mut_);
for(std::list<Seed>::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
{
if (it->ftr->frame == frame.get())
if (it->ftr->frame->id_ == frame->id_)
seeds.push_back(*it);
}
}
GPU_version/vio/src/map.cpp
+2 -2
View File
@@ -44,7 +44,7 @@ bool Map::safeDeleteFrame(FramePtr frame)
bool found = false;
for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
{
if(*it == frame)
if(it->get()->id_ == frame->id_)
{
std::for_each((*it)->fts_.begin(), (*it)->fts_.end(), [&](Feature* ftr){
removePtFrameRef(it->get(), ftr);
@@ -268,7 +268,7 @@ void MapPointCandidates::removeFrameCandidates(FramePtr frame)
auto it=candidates_.begin();
while(it!=candidates_.end())
{
if(it->second->frame == frame.get())
if(it->second->frame->id_ == frame->id_)
{
deleteCandidate(*it);
it = candidates_.erase(it);
GPU_version/vio/src/matcher.cpp
+3 -1
View File
@@ -140,7 +140,9 @@ bool Matcher::findMatchDirect(
const Frame& cur_frame,
Vector2d& px_cur)
{
if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))return false;
if(pt.last_frame_overlap_id_ !=cur_frame.id_)return true;
if(pt.obs_.size()<2)return false;
if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_,cur_frame.id_))return false;
if(ref_ftr_== nullptr)return false;
if(ref_ftr_->frame== nullptr)return false;
if(ref_ftr_->frame->cam_== nullptr)return false;
GPU_version/vio/src/point.cpp
+6 -2
View File
@@ -59,6 +59,7 @@ Point::~Point()
void Point::addFrameRef(Feature* ftr)
{
obs_.push_front(ftr);
++n_obs_;
}
@@ -95,7 +96,7 @@ void Point::initNormal()
normal_set_ = true;
}
bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const
bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr,int id) const
{
// TODO: get frame with same point of view AND same pyramid level!
ftr= nullptr;
@@ -103,7 +104,10 @@ bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const
double max_cos_angle = 1.0;
if(obs_.empty())return false;
for(auto&& ob:obs_){
if(ob == nullptr)continue;
if(ob->frame->id_==id){
ftr = ob;
return true;
}
Vector2d t=ob->frame->pos();
Vector3d dir(Vector3d(t.x(),1e-9,t.y()) + pos_);
double cos_angle = obs_dir.dot(dir.normalized());
GPU_version/vio/src/reprojector.cpp
+28 -17
View File
@@ -155,23 +155,33 @@ namespace vio {
for (auto &&match:matches) {
if (match.queryIdx != f.index)continue;
if (f.item->point == NULL){
f.item->point = new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()));
// assert(!f.item->point->obs_.empty());
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);
Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
(int) keypoints_cur.at(match.trainIdx).pt.y);
frame->fts_.push_back(new Feature(it_frame.item.first.get(),
new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item),
px,it_frame.item.first->cam_->cam2world(f.item->px),0));
frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
overlap_kfs.back().second++;
break;
}else{
if (f.item->point->last_frame_overlap_id_ == frame->id_)continue;
f.item->point->last_frame_overlap_id_ = frame->id_;
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
+ static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
assert(grid_.cells.at(k) != nullptr);
Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
(int) keypoints_cur.at(match.trainIdx).pt.y);
//f.item->point->addFrameRef(f.item);
grid_.cells.at(k)->push_back(Candidate(f.item->point, px));
overlap_kfs.back().second++;
break;
}
if (f.item->point->last_frame_overlap_id_ == frame->id_)continue;
f.item->point->last_frame_overlap_id_ = frame->id_;
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
+ static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
assert(grid_.cells.at(k) != nullptr);
Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
(int) keypoints_cur.at(match.trainIdx).pt.y);
f.item->point->addFrameRef(new Feature(it_frame.item.first.get(),f.item->px,0));
//f.item->point->addFrameRef(f.item);
grid_.cells.at(k)->push_back(Candidate(f.item->point, px));
overlap_kfs.back().second++;
break;
}
}
}
@@ -193,12 +203,13 @@ namespace vio {
}
bool Reprojector::reprojectCell(Cell &cell, FramePtr frame) {
if(cell.size()<1)return false;
cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
Cell::iterator it=cell.begin();
while(it!=cell.end())
{
++n_trials_;
if(it->pt->type_ == Point::TYPE_DELETED)
if(it->pt->type_ == Point::TYPE_DELETED || it->pt ==NULL)
{
it = cell.erase(it);
continue;