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

bandle adjustment works well

Browse Source
This commit is contained in:
gfuse
2021-10-01 17:02:02 +02:00
parent af0cf99be3
commit f64ef21d03
14 changed files with 716 additions and 905 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+60
View File
@@ -0,0 +1,60 @@
[16:55:41.271] New key frame
[16:55:41.271] BA loop run
[16:55:41.272] init error: 575.642695
[16:55:41.273] end error: 0.013166
[16:55:41.405] New key frame
[16:55:41.405] BA loop run
[16:55:41.405] init error: 189.835833
[16:55:41.407] end error: 0.468588
[16:55:41.507] New key frame
[16:55:41.507] BA loop run
[16:55:41.508] init error: 911.856919
[16:55:41.510] end error: 1.789062
[16:55:41.611] New key frame
[16:55:41.611] BA loop run
[16:55:41.612] init error: 18717.033656
[16:55:41.613] end error: 4.936020
[16:55:44.998] New key frame
[16:55:44.998] BA loop run
[16:55:44.998] init error: 39.791183
[16:55:45.001] end error: 10.492954
[16:55:45.116] New key frame
[16:55:45.116] BA loop run
[16:55:45.117] init error: 786.965335
[16:55:45.122] end error: 1.421966
[16:55:45.268] New key frame
[16:55:45.268] BA loop run
[16:55:45.270] init error: 1782.595577
[16:55:45.277] end error: 2.222177
[16:55:45.468] New key frame
[16:55:45.469] BA loop run
[16:55:45.473] init error: 3538.063991
[16:55:45.484] end error: 15.716388
[16:55:45.749] New key frame
[16:55:45.749] BA loop run
[16:55:45.753] init error: 16354.561639
[16:55:45.761] end error: 8.216372
[16:55:46.186] New key frame
[16:55:46.186] BA loop run
[16:55:46.206] init error: 414249.527414
[16:55:46.289] end error: 138.234439
[16:55:48.072] New key frame
[16:55:48.072] BA loop run
[16:55:48.108] init error: 627388.776280
[16:55:48.264] end error: 1448.316701
[16:55:48.945] New key frame
[16:55:48.945] BA loop run
[16:55:49.017] init error: 146955.415274
[16:55:49.918] end error: 941.453528
[16:56:07.244] New key frame
[16:56:07.244] BA loop run
[16:56:07.330] init error: 125101697.443424
[16:56:08.112] end error: 7023.248058
[16:56:29.389] New key frame
[16:56:29.389] BA loop run
[16:56:29.529] init error: 258236615.290955
[16:56:31.054] end error: 24797.607025
[16:56:52.584] New key frame
[16:56:52.584] BA loop run
[16:56:52.866] init error: 8329221.907695
[16:57:03.183] end error: 3862.867416
GPU_version/vio/frame_handler_log.txt
+627 -825
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/frame_handler_mono.h
-1
View File
@@ -34,7 +34,6 @@ class FrameHandlerMono : public FrameHandlerBase
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,3,1>& init);
virtual ~FrameHandlerMono();
GPU_version/vio/include/vio/global_optimizer.h
-11
View File
@@ -61,7 +61,6 @@ public:
boost::unique_lock< boost::mutex > lk( mtx_);
new_keyframe_=true;
cond_.notify_one();
std::cerr<<"new_key_frame\n";
#if VIO_DEBUG
fprintf(log_,"[%s] New key frame \n",
vio::time_in_HH_MM_SS_MMM().c_str());
@@ -69,8 +68,6 @@ public:
}
protected:
bool new_keyframe_=false;
std::shared_ptr<g2o::OptimizationAlgorithmLevenberg> Levenberg_;
std::shared_ptr<g2o::SparseOptimizer> optimizer_;
boost::condition_variable cond_;
boost::mutex mtx_;
boost::thread* thread_;
@@ -86,14 +83,6 @@ protected:
std::shared_ptr<Feature> feature;
};
/// Initialize g2o with solver type, optimization strategy and camera model.
void setupG2o();
/// Run the optimization on the provided graph.
void runSparseBAOptimizer(
unsigned int num_iter,
double& init_error,
double& final_error);
/// Create a g2o vertice from a keyframe object.
g2o::VertexSE3Expmap* createG2oFrameSE3(
FramePtr kf,
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
@@ -1,7 +1,7 @@
svo:
grid_size: 30 #Feature grid size of a cell in [px].
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.
loba_num_iter: 0 #Number of iterations in the local bundle adjustment.
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.
init_min_tracked: 100
GPU_version/vio/src/config.cpp
+1 -1
View File
@@ -26,7 +26,7 @@ Config::Config() :
use_imu(vk::getParam<bool>("svo/use_imu", false)),
core_n_kfs(vk::getParam<int>("svo/core_n_kfs", 3)),
map_scale(vk::getParam<double>("svo/map_scale", 1.0)),
grid_size(vk::getParam<int>("svo/grid_size", 30)),
grid_size(vk::getParam<int>("svo/grid_size", 10)),
init_min_disparity(vk::getParam<double>("svo/init_min_disparity", 50.0)),
init_min_tracked(vk::getParam<int>("svo/init_min_tracked", 50)),
init_min_inliers(vk::getParam<int>("svo/init_min_inliers", 40)),
GPU_version/vio/src/frame.cpp
+1 -1
View File
@@ -178,7 +178,7 @@ bool getSceneDepth(const FramePtr frame, double& depth_mean, double& depth_min)
{
if((*it)->point != NULL)
{
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().inverse()*(*it)->point->pos_));
if((e /= (1<<(*it)->level)).norm()>0.5)continue;
double z = frame->w2f((*it)->point->pos_).z();
depth_vec.push_back(z);
GPU_version/vio/src/frame_handler_mono.cpp
-2
View File
@@ -166,7 +166,6 @@ 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
//std::cerr<<"run BA\n";
ba_glob_->new_key_frame();
ROS_INFO("VIO initialized :)");
ROS_INFO("Running ...");
@@ -257,7 +256,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
}
// add keyframe to map
map_.addKeyframe(new_frame_);
std::cerr<<"BA run\n";
ba_glob_->new_key_frame();
return RESULT_IS_KEYFRAME;
}
GPU_version/vio/src/global_optimizer.cpp
+17 -38
View File
@@ -73,7 +73,7 @@ namespace vio {
while(map_.keyframes_.empty() || new_keyframe_ == false)
cond_.wait(lk);
#if VIO_DEBUG
fprintf(log_,"[%s] BA loop \n",
fprintf(log_,"[%s] BA loop run \n",
vio::time_in_HH_MM_SS_MMM().c_str());
#endif
new_keyframe_=false;
@@ -115,7 +115,7 @@ namespace vio {
if(v_mp == NULL)
{
// mappoint-vertex doesn't exist yet. create a new one:
v_mp = createG2oPoint(mp->pos_, v_id++, false);
v_mp = createG2oPoint(mp->pos_, v_id++, true);
mp->v_pt_ = v_mp;
optimizer.addVertex(v_mp);
}
@@ -141,10 +141,19 @@ namespace vio {
}
// Optimization
double init_error=0.0, final_error=0.0;
if(Config::lobaNumIter() > 0)
runSparseBAOptimizer( Config::lobaNumIter(), init_error, final_error);
point_mut_.lock();
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
#if VIO_DEBUG
fprintf(log_,"[%s] init error: %f \n",
vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
#endif
optimizer.optimize(vio::Config::lobaNumIter());
#if VIO_DEBUG
fprintf(log_,"[%s] end error: %f \n",
vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
#endif
// Update Keyframe and MapPoint Positions
for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
it_kf != map_.keyframes_.end(); ++it_kf)
@@ -163,6 +172,7 @@ namespace vio {
mp->v_pt_ = NULL;
}
}
point_mut_.unlock();
// Remove Measurements with too large reprojection error
for(list< pair<FramePtr,std::shared_ptr<Feature>> >::iterator it=incorrect_edges.begin();
@@ -179,36 +189,6 @@ namespace vio {
}
}
void BA_Glob::setupG2o()
{
// TODO: What's happening with all this HEAP stuff? Memory Leak?
/* optimizer_=std::make_shared<g2o::SparseOptimizer>();
optimizer_->setVerbose(false);
auto linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));
optimizer_->setAlgorithm(solver);
// setup camera
std::shared_ptr<g2o::CameraParameters> cam_params = std::make_shared<g2o::CameraParameters>(1.0, Vector2d(0.,0.), 0.);
cam_params->setId(0);
if (!optimizer_->addParameter(cam_params.get())) {
assert(false);*/
// }
}
void
BA_Glob::runSparseBAOptimizer(unsigned int num_iter,
double& init_error, double& final_error)
{
optimizer_->initializeOptimization();
optimizer_->computeActiveErrors();
init_error = optimizer_->activeChi2();
optimizer_->optimize(num_iter);
final_error = optimizer_->activeChi2();
}
g2o::VertexSE3Expmap*
BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed)
{
@@ -226,7 +206,7 @@ namespace vio {
{
g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
v->setId(id);
v->setFixed(fixed);
v->setMarginalized(fixed);
v->setEstimate(pos);
return v;
@@ -242,8 +222,7 @@ namespace vio {
g2o::EdgeProjectXYZ2UV* e= new g2o::EdgeProjectXYZ2UV;
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
e->setMeasurement(Vector2d(static_cast<int>(g2o::Sampler::uniformRand(0, 640)),
static_cast<int>(g2o::Sampler::uniformRand(0, 480))));
e->setMeasurement(f_up);
e->information() = Eigen::Matrix2d::Identity();
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
rk->setDelta(huber_width);
GPU_version/vio/src/math_utils.cpp
+1 -1
View File
@@ -12,7 +12,7 @@ namespace vk {
using namespace Eigen;
/*
* R and T are the rotation and translation of the current frames with respect to the refrence frame
* R and T are the rotation and translation of the current frames with respect to the refrence frame, T from ref to current
* */
Vector3d
GPU_version/vio/src/point.cpp
+2 -1
View File
@@ -184,7 +184,8 @@ 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_));
if (e.norm() > 0.3)
e /= (1<<(*it)->level);
if (e.norm() > 3.0/ (*it)->frame->cam_->errorMultiplier2())
n_failed_reproj_++;
}
GPU_version/vio/src/pose_optimizer.cpp
+1 -12
View File
@@ -124,30 +124,19 @@ void optimizeGaussNewton(
}
size_t n_deleted_refs = 0;
//for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
{
//std::cerr<<"123 - pose optimizer - frame->fts_.size() : "<<frame->fts_.size()<<std::endl;
//std::cerr<<"124 - pose optimizer - (*it)->point : "<<(*it)->point<<std::endl;
if((*it)->point == NULL) {
it++;
//std::cerr<<"126 - pose optimizer - (*it)->point == NULL : "<<std::endl;
continue;
}
//std::cerr<<"129 - pose optimizer "<<(*it)->f.x()<<" "<<(*it)->f.y()<<" "<<(*it)->f.z()<<"\t"<<(*it)->point->pos_.x()<<" "<<(*it)->point->pos_.y()<<" "<<(*it)->point->pos_.z()<<std::endl;
Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
//std::cerr<<"131 - pose optimizer"<<std::endl;
e /= (1<<(*it)->level);
//std::cerr<<"133 - pose optimizer"<<std::endl;
chi2_vec_final.push_back(e.norm());
//std::cerr<<"135 - pose optimizer"<<std::endl;
if(e.norm() > 2.0 / frame->cam_->errorMultiplier2())
if(e.norm() > 1.0 / frame->cam_->errorMultiplier2())
{
// we don't need to delete a reference in the point since it was not created yet
(*it)->point.reset();
//std::cerr<<"befor erase 140 pose optimizer"<<std::endl;
it = frame->fts_.erase(it);
//std::cerr<<"after erase 142 pose optimizer"<<std::endl;
++n_deleted_refs;
}
else
GPU_version/vio/src/reprojector.cpp
+3 -9
View File
@@ -75,9 +75,9 @@ namespace vio {
resetGrid();
Features keypoints;
cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING2,false);
feature_detection::FastDetector detector(
std::unique_ptr<feature_detection::FastDetector> detector=std::make_unique<feature_detection::FastDetector>(
frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast_,Config::nPyrLevels());
detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), keypoints);
detector->detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), keypoints);
std::vector<cv::KeyPoint> keypoints_cur;
list<std::shared_ptr<Feature>>::iterator it_cur=keypoints.begin();
cv::Mat cur_des=cv::Mat(keypoints.size(),64,CV_8UC1);
@@ -125,7 +125,6 @@ namespace vio {
else{
matcher->knnMatch(ref_des, cur_des, matches, 1, cv::InputArray(maskdown));
}
//matcher->knnMatch(ref_des, cur_des, matches, 1);
for(auto&& match:matches.back()){
it_cur=keypoints.begin();
@@ -169,14 +168,9 @@ namespace vio {
overlap_kfs.back().second++;
}
}
/* cv::Mat imgMatch,key_point_ref,key_point_cur;
cv::drawMatches( it_frame.item.first->img_pyr_[0], keypoints_kfs, frame->img_pyr_[0], keypoints_cur,matches, imgMatch);
cv::imshow("img",imgMatch);
cv::waitKey();*/
++it_ref;
}
}
//exit(0);
{
for (auto&& cell : grid_.cells) {
if (reprojectCell(*cell, frame, log_))
@@ -216,7 +210,7 @@ namespace vio {
if(!res)
{
it->pt->n_failed_reproj_++;
if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)it->pt.reset();
if(it->pt->n_failed_reproj_ > 15)it->pt.reset();
it = cell.erase(it);
continue;
}