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

debug depth mines

Browse Source
This commit is contained in:
gfuse
2021-10-13 21:00:05 +02:00
parent 08a0bbe124
commit 7395e10523
7 changed files with 379 additions and 831 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
-28
View File
@@ -1,28 +0,0 @@
[19:53:47.528] New key frame
[19:53:47.528] BA loop run
[19:53:47.529] init error: 16.678827
[19:53:47.530] end error: 0.000156
[19:53:47.626] New key frame
[19:53:47.626] BA loop run
[19:53:47.626] init error: 5.578004
[19:53:47.629] end error: 0.135229
[19:53:47.760] New key frame
[19:53:47.760] BA loop run
[19:53:47.760] init error: 11.645444
[19:53:47.761] end error: 0.000002
[19:53:47.877] New key frame
[19:53:47.877] BA loop run
[19:53:47.877] init error: 23.621901
[19:53:47.878] end error: 0.000000
[19:53:47.967] New key frame
[19:53:47.967] BA loop run
[19:53:47.968] init error: 2.238685
[19:53:47.968] end error: 0.000000
[19:53:48.433] New key frame
[19:53:48.433] BA loop run
[19:53:48.434] init error: 3.916923
[19:53:48.434] end error: 0.000339
[19:53:48.549] New key frame
[19:53:48.549] BA loop run
[19:53:48.549] init error: 28.908982
[19:53:48.550] end error: 0.000015
GPU_version/vio/frame_handler_log.txt
+368 -789
View File
File diff suppressed because it is too large Load Diff
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/src/frame_handler_mono.cpp
+2 -3
View File
@@ -132,7 +132,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
fprintf(log_,"[%s] Init: Selected Second frame. \t The number of features: %d depth mean:%f min:%f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),new_frame_->fts_.size(),depth_mean,depth_min);
#endif
ba_glob_->new_key_frame();
//ba_glob_->new_key_frame();
ROS_INFO("VIO initialized :)");
ROS_INFO("Running ...");
return RESULT_IS_KEYFRAME;
@@ -179,7 +179,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
fprintf(log_,"[%s] Update EKF and 3D points the number of feature in the new frame: %d and number of obs: %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->fts_.size(),new_frame_->nObs());
#endif
//posEdit(new_frame_);
double depth_mean=0.0, depth_min=0.0;
new_frame_->getSceneDepth(map_, depth_mean, depth_min);
#if VIO_DEBUG
@@ -220,7 +219,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
+2 -2
View File
@@ -191,7 +191,7 @@ namespace vio {
}
}
// Remove Measurements with too large reprojection error
/*// Remove Measurements with too large reprojection error
for(list< pair<FramePtr,std::shared_ptr<Feature>> >::iterator it=incorrect_edges.begin();
it!=incorrect_edges.end(); ++it)
map_.removePtFrameRef(it->first, it->second);
@@ -202,7 +202,7 @@ namespace vio {
{
map_.removePtFrameRef(it->frame, it->feature);
}
}
}*/
ba_mux_.unlock();
}
}
GPU_version/vio/src/point.cpp
+5 -5
View File
@@ -136,9 +136,9 @@ void Point::optimize(const size_t n_iter)
for(auto it=obs_.begin(); it!=obs_.end(); ++it)
{
Matrix23d J;
const Vector3d p_in_f((*it)->frame->se3()*pos_);
//jacobian_xyz2uv_(p_in_f, (*it)->frame->se3().rotation_matrix(), J, (*it)->frame->cam_->params(), (*it)->frame->T_f_w_);
jacobian_xyz2uv(p_in_f,(*it)->frame->se3().rotation_matrix(),J);
const Vector3d p_in_f((*it)->frame->w2f(pos_));
jacobian_xyz2uv_(p_in_f, (*it)->frame->se3().rotation_matrix(), J, (*it)->frame->cam_->params(), (*it)->frame->T_f_w_);
//jacobian_xyz2uv(p_in_f,(*it)->frame->se3().rotation_matrix(),J);
const Vector2d e=vk::project2d((*it)->f) - vk::project2d(p_in_f)/(1<<(*it)->level);
new_chi2 += e.norm();
A.noalias() += J.transpose() * J;
@@ -167,9 +167,9 @@ 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_));
Vector2d e = vk::project2d((*it)->f) - vk::project2d((*it)->frame->w2f(pos_));
e /= (1<<(*it)->level);
if (e.squaredNorm() > 2.0*vio::Config::poseOptimThresh()/ (*it)->frame->cam_->errorMultiplier2())
if (e.squaredNorm() > 8.0*vio::Config::poseOptimThresh()/ (*it)->frame->cam_->errorMultiplier2())
n_failed_reproj_++;
}
if(n_failed_reproj_<0.50*obs_.size())
GPU_version/vio/src/pose_optimizer.cpp
+1 -3
View File
@@ -129,7 +129,6 @@ void optimizeGaussNewton(
if(vk::norm_max(dT) <= EPS)
break;
}
//point_mut_.lock();
num_obs=0;
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
{
@@ -143,7 +142,7 @@ void optimizeGaussNewton(
it = frame->fts_.erase(it);
continue;
}
Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->w2f((*it)->point->pos_));
e /= (1<<(*it)->level);
chi2_vec_final.push_back(e.norm());
if(e.norm() > vio::Config::poseOptimThresh() / frame->cam_->errorMultiplier2())
@@ -157,7 +156,6 @@ void optimizeGaussNewton(
}
}
//point_mut_.unlock();
#if VIO_DEBUG
error_init=0.0;
error_final=0.0;