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

solve the crash issue for feature delete

Browse Source
This commit is contained in:
gfuse
2021-07-13 12:05:55 +02:00
parent 99fa000ad5
commit 4cb5cbff09
6 changed files with 34 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/gpu_svo/map.h
+2 -1
View File
@@ -66,7 +66,7 @@ public:
void emptyTrash();
};
static bool syn_=false;
/// Map object which saves all keyframes which are in a map.
class Map : boost::noncopyable
{
@@ -75,6 +75,7 @@ public:
list< Point* > trash_points_; //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also.
MapPointCandidates point_candidates_;
Map();
~Map();
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<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"/ -->
<!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/src/feature_alignment.cpp
+8 -8
View File
@@ -36,9 +36,9 @@ bool align1D(
Vector2d& cur_px_estimate,
double& h_inv)
{
const int halfpatch_size_ = 4;
const int patch_size = 8;
const int patch_area = 64;
const int halfpatch_size_ = 1;
const int patch_size = 2;
const int patch_area = 4;
bool converged=false;
// compute derivative of template and prepare inverse compositional
@@ -69,7 +69,7 @@ bool align1D(
float v = cur_px_estimate.y();
// termination condition
const float min_update_squared = 0.5*0.5;
const float min_update_squared = 0.05;
const int cur_step = cur_img.step.p[0];
float chi2 = 0;
Vector2f update; update.setZero();
@@ -163,9 +163,9 @@ bool align2D(
return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate);
#endif
const int halfpatch_size_ = 4;
const int patch_size_ = 8;
const int patch_area_ = 64;
const int halfpatch_size_ = 1;
const int patch_size_ = 2;
const int patch_area_ = 4;
bool converged=false;
// compute derivative of template and prepare inverse compositional
@@ -202,7 +202,7 @@ bool align2D(
bool sign=true;// +
// termination condition
const float min_update_squared = 0.5;//0.001
const float min_update_squared = 0.05;//0.001
const int cur_step = cur_img.step.p[0];
// float chi2 = 0;
Vector3f update; update.setZero();
GPU_version/vio/src/frame_handler_mono.cpp
+3 -4
View File
@@ -148,7 +148,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
pose_optimizer::optimizeGaussNewton(
Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>2.0 || fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI){
if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>2.0 ||
fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI ||
sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
new_frame_=last_frame_;
return RESULT_NO_KEYFRAME;
}
@@ -160,9 +162,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
<<"\tReprojected points after opmization: "<<sfba_n_edges_final<<"\t distance between two frames: "<<
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
return RESULT_NO_KEYFRAME;
}
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
// select keyframe
GPU_version/vio/src/map.cpp
+9
View File
@@ -40,6 +40,7 @@ void Map::reset()
bool Map::safeDeleteFrame(FramePtr frame)
{
if(syn_==true)return false;
bool found = false;
for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
{
@@ -107,6 +108,7 @@ void Map::getCloseKeyframes(
const FramePtr& frame,
std::list< std::pair<FramePtr,double> >& close_kfs) const
{
syn_=true;
for(auto kf : keyframes_)
{
// check if kf has overlaping field of view with frame, use therefore KeyPoints
@@ -123,6 +125,7 @@ void Map::getCloseKeyframes(
}
}
}
syn_=false;
}
FramePtr Map::getClosestKeyframe(const FramePtr& frame) const
@@ -149,6 +152,7 @@ FramePtr Map::getFurthestKeyframe(const Vector2d& pos) const
{
FramePtr furthest_kf;
double maxdist = 0.0;
syn_=true;
for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
{
double dist = ((*it)->pos()-pos).norm();
@@ -157,18 +161,21 @@ FramePtr Map::getFurthestKeyframe(const Vector2d& pos) const
furthest_kf = *it;
}
}
syn_=false;
return furthest_kf;
}
bool Map::getKeyframeById(const int id, FramePtr& frame) const
{
bool found = false;
syn_=true;
for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
if((*it)->id_ == id) {
found = true;
frame = *it;
break;
}
syn_=false;
return found;
}
@@ -178,6 +185,7 @@ void Map::transform(const Matrix2d& R, const Vector2d& t, const double& s)
R3<<R(0,0),0.0,R(0,1),
0.0,1.0,0.0,
R(1,0),0.0,R(1,1);
syn_=true;
for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
{
SE2_5 tem(SE2(R*(*it)->T_f_w_.inverse().rotation_matrix(),s*R*(*it)->pos() + t));
@@ -192,6 +200,7 @@ void Map::transform(const Matrix2d& R, const Vector2d& t, const double& s)
(*ftr)->point->pos_ = s*R3*(*ftr)->point->pos_ + Vector3d(t(0),0.0,t(1));
}
}
syn_=false;
}
void Map::emptyTrash()
GPU_version/vio/src/matcher.cpp
+11 -11
View File
@@ -161,20 +161,20 @@ bool Matcher::findMatchDirect(
Vector2d px_scaled(px_cur/(1<<search_level_));
bool success = false;
// if(ref_ftr_->type == Feature::EDGELET)
//{
// Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad);
// dir_cur.normalize();
// success = feature_alignment::align1D(
// cur_frame.img_pyr_[search_level_], dir_cur.cast<float>(),
// patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
//}
//else
//{
if(ref_ftr_->type == Feature::EDGELET)
{
Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad);
dir_cur.normalize();
success = feature_alignment::align1D(
cur_frame.img_pyr_[search_level_], dir_cur.cast<float>(),
patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
}
else
{
success = feature_alignment::align2D(
cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
options_.align_max_iter, px_scaled);
// }
}
px_cur = px_scaled * (1<<search_level_);
return success;
}