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 18:29:07 +02:00
parent 65ade550be
commit bfecc6bda7
4 changed files with 15 additions and 9 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> <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 --> <!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" /> <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/param/vo_fast.yaml
+2 -2
View File
@@ -13,6 +13,6 @@ vio:
ACC_ekf: 0.2 #acc white noise in continuous in EKF ACC_ekf: 0.2 #acc white noise in continuous in EKF
GYO_ekf: 0.5 #gyro 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_translation: 1.0 #VO noise in EKF
VO_ekf_orientation: 0.5 #VO noise in EKF VO_ekf_orientation: 0.1 #VO noise in EKF
CMD_ekf_translation: 0.1 #control vector noise in EKF CMD_ekf_translation: 0.1 #control vector noise in EKF
CMD_ekf_orientation: 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
+1 -1
View File
@@ -215,7 +215,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
// add keyframe to map // add keyframe to map
map_.addKeyframe(new_frame_); map_.addKeyframe(new_frame_);
map_.checkKeyFrames(); map_.checkKeyFrames();
ba_glob_->new_key_frame(); //ba_glob_->new_key_frame();
return RESULT_IS_KEYFRAME; return RESULT_IS_KEYFRAME;
} }
GPU_version/vio/src/pose_optimizer.cpp
+11 -5
View File
@@ -48,14 +48,18 @@ void optimizeGaussNewton(
std::vector<float> errors; std::vector<float> errors;
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/) for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
{ {
if((*it)->point == NULL) if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.) {
continue; map.safeDeletePoint((*it)->point);
it = frame->fts_.erase(it);
continue;
}
//Reprojection error //Reprojection error
Vector2d e = vk::project2d((*it)->f) Vector2d e = vk::project2d((*it)->f)
- vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_)); - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
if(std::isnan(e.norm())){ if(std::isnan(e.norm())){
map.safeDeletePoint((*it)->point); map.safeDeletePoint((*it)->point);
it = frame->fts_.erase(it); it = frame->fts_.erase(it);
continue;
} }
e *= 1.0 / (1<<(*it)->level); e *= 1.0 / (1<<(*it)->level);
chi2_vec_init.push_back(e.norm()); // just for debug chi2_vec_init.push_back(e.norm()); // just for debug
@@ -82,7 +86,7 @@ void optimizeGaussNewton(
// compute residual // compute residual
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{ {
if((*it)->point == NULL) if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.)
continue; continue;
Matrix23d J; Matrix23d J;
Vector3d xyz_f(Vector3d(frame->se3()*(*it)->point->pos_)); Vector3d xyz_f(Vector3d(frame->se3()*(*it)->point->pos_));
@@ -121,8 +125,10 @@ void optimizeGaussNewton(
size_t n_deleted_refs = 0; size_t n_deleted_refs = 0;
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/) for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
{ {
if((*it)->point == NULL) { if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.) {
it++; map.safeDeletePoint((*it)->point);
it = frame->fts_.erase(it);
++n_deleted_refs;
continue; continue;
} }
Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_)); Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));