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>
<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
+2 -2
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.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_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
map_.addKeyframe(new_frame_);
map_.checkKeyFrames();
ba_glob_->new_key_frame();
//ba_glob_->new_key_frame();
return RESULT_IS_KEYFRAME;
}
GPU_version/vio/src/pose_optimizer.cpp
+10 -4
View File
@@ -48,14 +48,18 @@ void optimizeGaussNewton(
std::vector<float> errors;
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.) {
map.safeDeletePoint((*it)->point);
it = frame->fts_.erase(it);
continue;
}
//Reprojection error
Vector2d e = vk::project2d((*it)->f)
- vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
if(std::isnan(e.norm())){
map.safeDeletePoint((*it)->point);
it = frame->fts_.erase(it);
continue;
}
e *= 1.0 / (1<<(*it)->level);
chi2_vec_init.push_back(e.norm()); // just for debug
@@ -82,7 +86,7 @@ void optimizeGaussNewton(
// compute residual
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;
Matrix23d J;
Vector3d xyz_f(Vector3d(frame->se3()*(*it)->point->pos_));
@@ -121,8 +125,10 @@ void optimizeGaussNewton(
size_t n_deleted_refs = 0;
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
{
if((*it)->point == NULL) {
it++;
if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.) {
map.safeDeletePoint((*it)->point);
it = frame->fts_.erase(it);
++n_deleted_refs;
continue;
}
Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));