diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index cb79071..0a9d279 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -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 diff --git a/GPU_version/vio/src/frame_handler_base.cpp b/GPU_version/vio/src/frame_handler_base.cpp index 8bd06e4..bd7a55a 100644 --- a/GPU_version/vio/src/frame_handler_base.cpp +++ b/GPU_version/vio/src/frame_handler_base.cpp @@ -100,7 +100,11 @@ void FrameHandlerBase::optimizeStructure( { deque> 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); diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 85c6b01..365c550 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -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 diff --git a/GPU_version/vio/src/global_optimizer.cpp b/GPU_version/vio/src/global_optimizer.cpp index c4bbc76..c327201 100644 --- a/GPU_version/vio/src/global_optimizer.cpp +++ b/GPU_version/vio/src/global_optimizer.cpp @@ -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 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>(*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(v_point)); e->setVertex(1, dynamic_cast(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; } diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index c09c20f..6ff43f3 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -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 new_point=std::make_shared(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);*/ diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp index 437dc7c..c13803f 100644 --- a/GPU_version/vio/src/sparse_img_align_gpu.cpp +++ b/GPU_version/vio/src/sparse_img_align_gpu.cpp @@ -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);