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

debug some issues

Browse Source
This commit is contained in:
gfuse
2021-10-12 17:02:39 +02:00
parent b88547671d
commit 65ade550be
6 changed files with 25 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/param/vo_fast.yaml
+3 -3
View File
@@ -1,6 +1,6 @@
vio:
grid_size: 8 #Feature grid size of a cell in [px].
max_n_kfs: 20 #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. Minimum number of keyframes is 3.
max_n_kfs: 200 #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. Minimum number of keyframes is 3.
loba_num_iter: 10 #Number of iterations in the local bundle adjustment.
quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad.
quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad.
@@ -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: 1.5 #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
CMD_ekf_orientation: 0.1 #control vector noise in EKF
GPU_version/vio/src/frame_handler_base.cpp
+5 -1
View File
@@ -100,7 +100,11 @@ void FrameHandlerBase::optimizeStructure(
{
deque<std::shared_ptr<Point>> pts;
for(auto&& it:frame->fts_){
if(it->point!=NULL)pts.push_back(it->point);
if(it->point!=NULL && !it->point->pos_.hasNaN() && it->point->pos_.norm() !=0.){
pts.push_back(it->point);
}else{
map_.safeDeletePoint(it->point);
}
}
//max_n_pts = min(max_n_pts, pts.size());
//nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator);
GPU_version/vio/src/frame_handler_mono.cpp
+5 -1
View File
@@ -199,7 +199,11 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
#endif
// new keyframe selected
for(auto&& it:new_frame_->fts_){
if(it->point != NULL)it->point->addFrameRef(it);
if(it->point != NULL && !it->point->pos_.hasNaN() && it->point->pos_.norm() !=0.){
it->point->addFrameRef(it);
}else{
map_.safeDeletePoint(it->point);
}
}
// if limited number of keyframes, remove the one furthest apart
GPU_version/vio/src/global_optimizer.cpp
+8 -7
View File
@@ -87,9 +87,9 @@ namespace vio {
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
double* pam=map_.keyframes_.back()->cam_->params();
// setup camera
g2o::CameraParameters* cam_params = new g2o::CameraParameters(1.0, Vector2d(0.,0.), 0.);
g2o::CameraParameters* cam_params = new g2o::CameraParameters(0.5*(pam[0]+pam[1]), Vector2d(pam[2],pam[3]), 0.);
cam_params->setId(0);
if (!optimizer.addParameter(cam_params)) {
assert(false);
@@ -113,6 +113,7 @@ namespace vio {
// for each keyframe add edges to all observed mapoints
std::shared_ptr<Point> mp = it_ftr->point;
if(mp->pos_.hasNaN())continue;
if(mp->pos_.norm()==0.)continue;
g2o::VertexSBAPointXYZ* v_mp = mp->v_pt_;
if(v_mp == NULL)
{
@@ -124,12 +125,11 @@ namespace vio {
// Due to merging of mappoints it is possible that references in kfs suddenly
// have a very large reprojection error which may result in distorted results.
Vector2d error = vk::project2d(it_ftr->f) - vk::project2d((*it_kf)->w2f(mp->pos_));
/* Vector2d error = vk::project2d(it_ftr->f) - vk::project2d((*it_kf)->w2f(mp->pos_));
if(error.squaredNorm() > pow(2,Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()))
incorrect_edges.push_back(pair<FramePtr,std::shared_ptr<Feature>>(*it_kf, it_ftr));
else
{
g2o::Edge_XYZ_VSC e1;
{*/
g2o::EdgeProjectXYZ2UV* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f),
true,
Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth());
@@ -139,7 +139,7 @@ namespace vio {
edge.feature=it_ftr;
edges.push_back(edge);
optimizer.addEdge(e);
}
/*}*/
}
}
if(optimizer.vertices().size()<1)continue;
@@ -242,10 +242,11 @@ namespace vio {
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
e->setMeasurement(f_up);
e->information() = Eigen::Matrix2d::Identity();
e->information() = weight*Eigen::Matrix2d::Identity();
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
rk->setDelta(huber_width);
e->setRobustKernel(rk);
e->setParameterId(0, 0); //old: e->setId(v_point->id());
return e;
}
GPU_version/vio/src/reprojector.cpp
+2 -2
View File
@@ -140,7 +140,7 @@ namespace vio {
// pose with respect to reference frame
Vector3d pos=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(),
(*it_ref)->f,frame->c2f(px));
if(pos.norm()==0.0)continue;
if(pos.norm()==0.)continue;
// point in world frame
std::shared_ptr<Point> new_point=std::make_shared<Point>(it_frame.item.first->se3()*pos,(*it_ref));
@@ -170,7 +170,7 @@ namespace vio {
}
++it_ref;
}
if(points_count>300)return;
//if(points_count>300)return;
}
/* std::cerr<<"here\n";
exit(0);*/
GPU_version/vio/src/sparse_img_align_gpu.cpp
+2
View File
@@ -43,6 +43,8 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
feature_counter_ = 0; // is used to compute the index of the cached jacobian
for(auto it=ref_frame->fts_.begin();it!=ref_frame->fts_.end();++it){
if((*it)->point == nullptr)continue;
if((*it)->point->pos_.hasNaN())continue;
if((*it)->point->pos_.norm()==0.)continue;
Vector3d xyz_ref=(*it)->f*(ref_frame->se3().inverse()*(*it)->point->pos_).norm();
features[feature_counter_].x=xyz_ref(0);
features[feature_counter_].y=xyz_ref(1);