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

add landmark type

Browse Source
This commit is contained in:
gfuse
2021-10-12 20:41:15 +02:00
parent bfecc6bda7
commit 094c9f4282
8 changed files with 33 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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/param/vo_fast.yaml
+1 -1
View File
@@ -13,6 +13,6 @@ vio:
ACC_ekf: 0.2 #acc white noise in continuous in EKF
GYO_ekf: 0.5 #gyro white noise in continuous in EKF
VO_ekf_translation: 1.0 #VO noise in EKF
VO_ekf_orientation: 0.1 #VO noise in EKF
VO_ekf_orientation: 0.5 #VO noise in EKF
CMD_ekf_translation: 0.1 #control vector noise in EKF
CMD_ekf_orientation: 0.01 #control vector noise in EKF
GPU_version/vio/src/frame_handler_mono.cpp
+2 -2
View File
@@ -181,7 +181,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
#endif
double depth_mean=0.0, depth_min=0.0;
frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
if(depth_mean>10.0)return RESULT_NO_KEYFRAME;
if(depth_mean>20.0)return RESULT_NO_KEYFRAME;
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
// select keyframe
@@ -215,7 +215,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
// add keyframe to map
map_.addKeyframe(new_frame_);
map_.checkKeyFrames();
//ba_glob_->new_key_frame();
ba_glob_->new_key_frame();
return RESULT_IS_KEYFRAME;
}
GPU_version/vio/src/global_optimizer.cpp
+1
View File
@@ -110,6 +110,7 @@ namespace vio {
for(auto&& it_ftr:(*it_kf)->fts_)
{
if(it_ftr->point==NULL)continue;
if(it_ftr->point->type_ != vio::Point::TYPE_GOOD)continue;
// for each keyframe add edges to all observed mapoints
std::shared_ptr<Point> mp = it_ftr->point;
if(mp->pos_.hasNaN())continue;
GPU_version/vio/src/initialization.cpp
+1
View File
@@ -102,6 +102,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
frame_cur->c2f(px_cur_.at(f.index).x,px_cur_.at(f.index).y), f.item->score,f.item->level,f.item->descriptor);
frame_cur->addFeature(ftr_cur);
new_point->addFrameRef(ftr_cur);
new_point->type_=vio::Point::TYPE_GOOD;
std::shared_ptr<Feature> ftr_ref=std::make_shared<Feature>(frame_ref_, new_point, f.item->px, f.item->f, f.item->score,f.item->level,f.item->descriptor);
frame_ref_->addFeature(ftr_ref);
GPU_version/vio/src/point.cpp
+3 -3
View File
@@ -166,11 +166,11 @@ void Point::optimize(const size_t n_iter)
for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * pos_));
e /= (1<<(*it)->level);
if (e.norm() > 3.0/ (*it)->frame->cam_->errorMultiplier2())
if (e.norm() > 4.0/ (*it)->frame->cam_->errorMultiplier2())
n_failed_reproj_++;
}
if(n_failed_reproj_<0.75*obs_.size())
type_=TYPE_GOOD;
}
} // namespace vio
GPU_version/vio/src/pose_optimizer.cpp
+22 -5
View File
@@ -48,11 +48,20 @@ void optimizeGaussNewton(
std::vector<float> errors;
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
{
if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.) {
if((*it)->point == NULL) {
map.safeDeletePoint((*it)->point);
it = frame->fts_.erase(it);
continue;
}
if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.){
map.safeDeletePoint((*it)->point);
it = frame->fts_.erase(it);
continue;
}
if((*it)->point->type_==vio::Point::TYPE_UNKNOWN){
it++;
continue;
}
//Reprojection error
Vector2d e = vk::project2d((*it)->f)
- vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
@@ -86,7 +95,8 @@ void optimizeGaussNewton(
// compute residual
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.)
if((*it)->point == NULL)continue;
if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0. || (*it)->point->type_==vio::Point::TYPE_UNKNOWN)
continue;
Matrix23d J;
Vector3d xyz_f(Vector3d(frame->se3()*(*it)->point->pos_));
@@ -125,12 +135,17 @@ void optimizeGaussNewton(
size_t n_deleted_refs = 0;
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
{
if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.) {
if((*it)->point == NULL) {
map.safeDeletePoint((*it)->point);
it = frame->fts_.erase(it);
++n_deleted_refs;
continue;
}
if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.){
map.safeDeletePoint((*it)->point);
it = frame->fts_.erase(it);
continue;
}
Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
e /= (1<<(*it)->level);
chi2_vec_final.push_back(e.norm());
@@ -139,9 +154,11 @@ void optimizeGaussNewton(
map.safeDeletePoint((*it)->point);
it = frame->fts_.erase(it);
++n_deleted_refs;
}
else
}else{
(*it)->point->type_=vio::Point::TYPE_CANDIDATE;
it++;
}
}
//point_mut_.unlock();
num_obs -= n_deleted_refs;
GPU_version/vio/src/reprojector.cpp
+2 -1
View File
@@ -152,16 +152,17 @@ namespace vio {
new_point->addFrameRef(frame->fts_.back());
added_keypoints.push_back(match.trainIdx);
frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
frame->fts_.back()->point->type_=vio::Point::TYPE_UNKNOWN;
grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
overlap_kfs.back().second++;
++points_count;
}else{
if(!matcher_.findMatchDirect(*(*it_ref)->point, *frame, px))continue;
(*it_ref)->point->last_frame_overlap_id_ = frame->id_;
frame->addFeature(std::make_shared<Feature>(frame,
(*it_ref)->point,
px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor));
(*it_ref)->point->type_=vio::Point::TYPE_CANDIDATE;
added_keypoints.push_back(match.trainIdx);
grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
overlap_kfs.back().second++;