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

add some calibration

Browse Source
This commit is contained in:
gfuse
2021-10-04 11:35:24 +02:00
parent 346dbb640c
commit b39cb43382
11 changed files with 1862 additions and 393 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+136
View File
@@ -0,0 +1,136 @@
[11:32:26.215] New key frame
[11:32:26.215] BA loop run
[11:32:26.217] init error: 8.472095
[11:32:26.222] end error: 0.002451
[11:32:26.332] New key frame
[11:32:26.332] BA loop run
[11:32:26.334] init error: 12.629072
[11:32:26.340] end error: 2.173331
[11:32:26.468] New key frame
[11:32:26.468] BA loop run
[11:32:26.470] init error: 65.010863
[11:32:26.477] end error: 1.928134
[11:32:26.617] New key frame
[11:32:26.617] BA loop run
[11:32:26.619] init error: 111.982387
[11:32:26.627] end error: 8.215693
[11:32:26.780] New key frame
[11:32:26.780] BA loop run
[11:32:26.782] init error: 133637.531428
[11:32:26.789] end error: 11.632299
[11:32:27.443] New key frame
[11:32:27.443] BA loop run
[11:32:27.447] init error: 1319.422136
[11:32:27.455] end error: 12.848739
[11:32:27.621] New key frame
[11:32:27.621] BA loop run
[11:32:27.625] init error: 182.371153
[11:32:27.641] end error: 13.471113
[11:32:27.930] New key frame
[11:32:27.930] BA loop run
[11:32:27.935] init error: 281.210624
[11:32:27.952] end error: 70.606181
[11:32:28.077] New key frame
[11:32:28.077] BA loop run
[11:32:28.082] init error: 309.023063
[11:32:28.422] end error: 23.878624
[11:32:28.430] New key frame
[11:32:28.430] BA loop run
[11:32:28.440] init error: 399.805659
[11:32:29.963] end error: 21.797671
[11:32:29.976] New key frame
[11:32:29.976] BA loop run
[11:32:29.984] init error: 10406.282878
[11:32:30.266] end error: 27.018722
[11:32:30.497] New key frame
[11:32:30.497] BA loop run
[11:32:30.505] init error: 306.484531
[11:32:30.770] end error: 29.021864
[11:32:30.843] New key frame
[11:32:30.843] BA loop run
[11:32:30.868] init error: 218.967075
[11:32:31.623] end error: 38.436167
[11:32:31.642] New key frame
[11:32:31.642] BA loop run
[11:32:31.668] init error: 242.205017
[11:32:31.915] end error: 39.613393
[11:32:32.126] New key frame
[11:32:32.126] BA loop run
[11:32:32.137] init error: 1068.056513
[11:32:32.471] end error: 32.717972
[11:32:32.628] New key frame
[11:32:32.628] BA loop run
[11:32:32.640] init error: 205.632654
[11:32:33.290] end error: 46.394407
[11:32:33.307] New key frame
[11:32:33.307] BA loop run
[11:32:33.327] init error: 257.984168
[11:32:34.235] end error: 45.118705
[11:32:34.253] New key frame
[11:32:34.253] BA loop run
[11:32:34.269] init error: 378.196667
[11:32:35.135] end error: 72.485312
[11:32:35.152] New key frame
[11:32:35.152] BA loop run
[11:32:35.166] init error: 2940.286773
[11:32:36.051] end error: 70.470916
[11:32:36.064] New key frame
[11:32:36.064] BA loop run
[11:32:36.098] init error: 175.651875
[11:32:36.564] end error: 51.774350
[11:32:36.576] New key frame
[11:32:36.576] BA loop run
[11:32:36.594] init error: 212.690168
[11:32:37.025] end error: 60.040802
[11:32:37.035] New key frame
[11:32:37.035] BA loop run
[11:32:37.065] init error: 224.779653
[11:32:37.748] end error: 50.944995
[11:32:37.762] New key frame
[11:32:37.762] BA loop run
[11:32:37.799] init error: 144.458647
[11:32:38.281] end error: 55.943794
[11:32:38.292] New key frame
[11:32:38.292] BA loop run
[11:32:38.308] init error: 417.348226
[11:32:38.776] end error: 65.937311
[11:32:38.793] New key frame
[11:32:38.793] BA loop run
[11:32:38.826] init error: 321.374078
[11:32:39.508] end error: 117.666568
[11:32:39.522] New key frame
[11:32:39.522] BA loop run
[11:32:39.560] init error: 582.482623
[11:32:40.106] end error: 56.439056
[11:32:40.120] New key frame
[11:32:40.120] BA loop run
[11:32:40.145] init error: 406.636760
[11:32:40.672] end error: 56.901814
[11:32:40.685] New key frame
[11:32:40.685] BA loop run
[11:32:40.728] init error: 855.372149
[11:32:41.341] end error: 65.671640
[11:32:41.357] New key frame
[11:32:41.357] BA loop run
[11:32:41.397] init error: 587.088866
[11:32:41.858] end error: 114.055639
[11:32:41.936] New key frame
[11:32:41.936] BA loop run
[11:32:41.963] init error: 439.136273
[11:32:42.631] end error: 63.348724
[11:32:42.646] New key frame
[11:32:42.646] BA loop run
[11:32:42.681] init error: 661.332414
[11:32:43.163] end error: 88.189274
[11:32:43.327] New key frame
[11:32:43.327] BA loop run
[11:32:43.341] init error: 526.133967
[11:32:44.023] end error: 79.628375
[11:32:44.038] New key frame
[11:32:44.038] BA loop run
[11:32:44.060] init error: 490.175656
[11:32:44.972] end error: 50.982413
[11:32:44.982] New key frame
[11:32:44.982] BA loop run
[11:32:45.005] init error: 610.616898
[11:32:45.434] end error: 57.851118
GPU_version/vio/frame_handler_log.txt
+1704 -251
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/point.h
-23
View File
@@ -46,17 +46,12 @@ public:
static int point_counter_; //!< Counts the number of created points. Used to set the unique id.
int id_; //!< Unique ID of the point.
Vector3d pos_; //!< 3d pos of the point in the world coordinate frame.
Vector3d normal_; //!< Surface normal at point.
Matrix3d normal_information_; //!< Inverse covariance matrix of normal estimation.
bool normal_set_; //!< Flag whether the surface normal was estimated or not.
list<std::shared_ptr<Feature>> obs_; //!< References to keyframes which observe the point.
size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
g2o::VertexSBAPointXYZ* v_pt_=nullptr; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
int last_published_ts_; //!< Timestamp of last publishing.
int last_frame_overlap_id_; //!< Flag for the reprojection: don't reproject a pt twice.
PointType type_; //!< Quality of the point.
int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point.
int n_succeeded_reproj_; //!< Number of succeeded reprojections. Used to assess the quality of the point.
int last_structure_optim_; //!< Timestamp of last point optimization
Point(const Vector3d& pos);
@@ -69,8 +64,6 @@ public:
/// Remove reference to a frame.
bool deleteFrameRef(FramePtr frame);
/// Initialize point normal. The inital estimate will point towards the frame.
void initNormal();
/// Check whether mappoint has reference to a frame.
std::shared_ptr<Feature> findFrameRef(FramePtr frame);
@@ -85,22 +78,6 @@ public:
void optimize(const size_t n_iter);
/// Jacobian of point projection on unit plane (focal length = 1) in frame (f).
inline static void jacobian_xyz2uv(
const Vector3d& p_in_f,
const Matrix3d& R_f_w,
Matrix23d& point_jac)
{
const double z_inv = 1.0/p_in_f[2];
const double z_inv_sq = z_inv*z_inv;
point_jac(0, 0) = z_inv;
point_jac(0, 1) = 0.0;
point_jac(0, 2) = -p_in_f[0] * z_inv_sq;
point_jac(1, 0) = 0.0;
point_jac(1, 1) = z_inv;
point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
point_jac = - point_jac * R_f_w;
}
void jacobian_xyz2uv_(
const Vector3d& p_in_f,
const Matrix3d& R_f_w,
GPU_version/vio/include/vio/pose_optimizer.h
+2 -2
View File
@@ -18,6 +18,7 @@
#define SVO_POSE_OPTIMIZER_H_
#include <vio/global.h>
#include <vio/map.h>
namespace vio {
@@ -35,14 +36,13 @@ class Point;
namespace pose_optimizer {
void optimizeGaussNewton(
const double reproj_thresh,
const size_t n_iter,
const bool verbose,
FramePtr& frame,
double& estimated_scale,
double& error_init,
double& error_final,
size_t& num_obs,
vio::Map& map,
FILE* log);
} // namespace pose_optimizer
GPU_version/vio/include/vio/reprojector.h
+1 -4
View File
@@ -61,8 +61,7 @@ public:
/// Project points from the map into the image. First finds keyframes with
/// overlapping field of view and projects only those map-points.
void reprojectMap2(
void reprojectMap(
FramePtr frame,
FramePtr last_frame,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs,
@@ -97,10 +96,8 @@ private:
Matcher matcher_;
Map& map_;
static bool pointQualityComparator(Candidate& lhs, Candidate& rhs);
void initializeGrid(vk::AbstractCamera* cam);
void resetGrid();
bool reprojectCell(Cell& cell, FramePtr frame, FILE* log_);
};
} // namespace vio
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
@@ -10,7 +10,7 @@ svo:
kfselect_mindist: 0.01
poseoptim_thresh: 8.0
init_min_inliers: 50
ACC_noise: 0.5 #acc white noise in continuous in EKF
GYO_noise: 0.5 #gyro white noise in continuous in EKF
ACC_noise: 0.4 #acc white noise in continuous in EKF
GYO_noise: 0.4 #gyro white noise in continuous in EKF
svo_ekf: 0.1 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 0.1 #control vector noise in EKF
GPU_version/vio/src/frame_handler_mono.cpp
+5 -6
View File
@@ -194,13 +194,12 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_=last_frame_;
return RESULT_FAILURE;
}
reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_, gpu_fast_, log_);
reprojector_.reprojectMap(new_frame_, last_frame_,overlap_kfs_, gpu_fast_, log_);
int n_point=0;
for(auto i:overlap_kfs_)n_point+=i.second;
#if VIO_DEBUG
fprintf(log_,"[%s] After Reprojection Map nPoint:%d ,nCell:%d ,nMatches:%d ,distance between ekf and vo x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
fprintf(log_,"[%s] After Reprojection Map nMatches:%d ,distance between ekf and vo x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
n_point,
reprojector_.n_trials_,reprojector_.n_matches_,
new_frame_->T_f_w_.se2().translation().x()-init_f.second.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-init_f.second.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
@@ -209,8 +208,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
size_t sfba_n_edges_final=0;
double sfba_thresh, sfba_error_init, sfba_error_final;
pose_optimizer::optimizeGaussNewton(
Config::poseOptimThresh(), 70, false,
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,log_);
Config::poseOptimThresh(),
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,map_,log_);
#if VIO_DEBUG
fprintf(log_,"[%s] After pose optimization, distance between ekf and vo x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->T_f_w_.se2().translation().x()-init_f.second.se2().translation().x(),
@@ -218,7 +217,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
#endif
if((init_f.second.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.2 ||
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.02 || sfba_n_edges_final<10){
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.2 || sfba_n_edges_final<10){
new_frame_=last_frame_;
return RESULT_FAILURE;
}
GPU_version/vio/src/point.cpp
-18
View File
@@ -27,28 +27,22 @@ int Point::point_counter_ = 0;
Point::Point(const Vector3d& pos) :
id_(point_counter_++),
pos_(pos),
normal_set_(false),
n_obs_(0),
v_pt_(NULL),
last_published_ts_(0),
last_frame_overlap_id_(-10),
type_(TYPE_UNKNOWN),
n_failed_reproj_(0),
n_succeeded_reproj_(0),
last_structure_optim_(0)
{}
Point::Point(const Vector3d& pos, std::shared_ptr<Feature> ftr) :
id_(point_counter_++),
pos_(pos),
normal_set_(false),
n_obs_(1),
v_pt_(NULL),
last_published_ts_(0),
last_frame_overlap_id_(-10),
type_(TYPE_UNKNOWN),
n_failed_reproj_(0),
n_succeeded_reproj_(0),
last_structure_optim_(0)
{
obs_.push_front(ftr);
@@ -90,17 +84,6 @@ bool Point::deleteFrameRef(FramePtr frame)
return false;
}
void Point::initNormal()
{
assert(!obs_.empty());
const std::shared_ptr<Feature> ftr = obs_.back();
assert(ftr->frame != NULL);
normal_ = ftr->frame->se3().rotation_matrix().transpose()*(-ftr->f);
double x=pow(20/(pos_-Vector3d(ftr->frame->pos()(0),0.0,ftr->frame->pos()(1))).norm(),2);
normal_information_ = DiagonalMatrix<double,3,3>(x, 1.0, 1.0);
normal_set_ = true;
}
bool Point::getCloseViewObs(const Vector2d& framepos, std::shared_ptr<Feature>& ftr,int id) const
{
boost::unique_lock<boost::mutex> lock(point_mut_);
@@ -153,7 +136,6 @@ void Point::optimize(const size_t n_iter)
{
Matrix23d J;
const Vector3d p_in_f((*it)->frame->se3()*pos_);
//Point::jacobian_xyz2uv(p_in_f, (*it)->frame->se3().rotation_matrix(), J);
Point::jacobian_xyz2uv_(p_in_f, (*it)->frame->se3().rotation_matrix(), J, (*it)->frame->cam_->params(), (*it)->frame->T_f_w_);
const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
new_chi2 += e.squaredNorm();
GPU_version/vio/src/pose_optimizer.cpp
+4 -13
View File
@@ -27,14 +27,13 @@ namespace vio {
namespace pose_optimizer {
void optimizeGaussNewton(
const double reproj_thresh,
const size_t n_iter,
const bool verbose,
FramePtr& frame,
double& estimated_scale,
double& error_init,
double& error_final,
size_t& num_obs,
vio::Map& map,
FILE* log)
{
// init
@@ -55,11 +54,6 @@ void optimizeGaussNewton(
Vector2d e = vk::project2d((*it)->f)
- vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
e *= 1.0 / (1<<(*it)->level);
//std::cerr<<"pose optimizer - error : "<<e.norm()<<std::endl;
/* if(e.norm() > 0.01){
it = frame->fts_.erase(it);
continue;
}*/
chi2_vec_init.push_back(e.norm()); // just for debug
errors.push_back(e.norm());
it++;
@@ -69,9 +63,6 @@ void optimizeGaussNewton(
return;
vk::robust_cost::MADScaleEstimator scale_estimator;
estimated_scale = scale_estimator.compute(errors);
//#if VIO_DEBUG
// fprintf(log,"[%s] Init estimate scale:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),estimated_scale);
//#endif
num_obs = errors.size();
double scale = estimated_scale;
@@ -122,7 +113,7 @@ void optimizeGaussNewton(
if(vk::norm_max(dT) <= EPS)
break;
}
point_mut_.lock();
//point_mut_.lock();
size_t n_deleted_refs = 0;
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
{
@@ -135,14 +126,14 @@ void optimizeGaussNewton(
chi2_vec_final.push_back(e.norm());
if(e.norm() > 1.0 / frame->cam_->errorMultiplier2())
{
(*it)->point.reset();
map.safeDeletePoint((*it)->point);
it = frame->fts_.erase(it);
++n_deleted_refs;
}
else
it++;
}
point_mut_.unlock();
//point_mut_.unlock();
num_obs -= n_deleted_refs;
#if VIO_DEBUG
error_init=0.0;
GPU_version/vio/src/reprojector.cpp
+7 -73
View File
@@ -65,7 +65,7 @@ namespace vio {
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO - second reimplementation
void Reprojector::reprojectMap2(
void Reprojector::reprojectMap(
FramePtr frame,
FramePtr last_frame,
std::vector<std::pair<FramePtr, std::size_t> > &overlap_kfs,
@@ -116,31 +116,23 @@ namespace vio {
std::vector<cv::KeyPoint> keypoints_kfs;
std::vector<std::vector<cv::DMatch>> matches;
keypoints_kfs.push_back(cv::KeyPoint((*it_ref)->px.x(), (*it_ref)->px.y(), 7.f,(*it_ref)->score));
cv::Mat ref_des=cv::Mat(1,64,CV_8UC1,(*it_ref)->descriptor);
if ((*it_ref)->px.y() < frame->img().rows/2) {
matcher->knnMatch(ref_des, cur_des, matches, 1, cv::InputArray(maskup));
}
else{
matcher->knnMatch(ref_des, cur_des, matches, 1, cv::InputArray(maskdown));
}
/*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();*/
for(auto&& match:matches.back()){
it_cur=keypoints.begin();
std::advance(it_cur,match.trainIdx);
if(!(*it_cur))continue;
/* const int k = static_cast<int>((*it_cur)->px.y() / grid_.cell_size) *
grid_.grid_n_cols
+ static_cast<int>((*it_cur)->px.x() / grid_.cell_size);
if(k>grid_.cells.size()-1)continue;
assert(grid_.cells.at(k) != nullptr);
if(grid_.cells.at(k)->size()> Config::gridSize()-1)continue;*/
Vector2d px((int) (*it_cur)->px.x(),
(int) (*it_cur)->px.y());
const int k = static_cast<int>((*it_cur)->px.y() / grid_.cell_size) *
grid_.grid_n_cols
+ static_cast<int>((*it_cur)->px.x() / grid_.cell_size);
if(grid_.cells.at(k)->size()> Config::gridSize()-1)continue;
if ((*it_ref)->point == NULL){
SE3 T_ref_cur=it_frame.item.first->se3().inverse()*frame->se3();
// pose with respect to reference frame
@@ -157,8 +149,7 @@ namespace vio {
new_point->addFrameRef(frame->fts_.back());
added_keypoints.push_back(match.trainIdx);
frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
///TODO number of cell is not reflecting the number of point, check K value
//grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
overlap_kfs.back().second++;
}else{
if(!matcher_.findMatchDirect(*(*it_ref)->point, *frame, px))continue;
@@ -168,70 +159,13 @@ namespace vio {
(*it_ref)->point,
px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor));
added_keypoints.push_back(match.trainIdx);
///TODO number of cell is not reflecting the number of point, check K value
//grid_.cells.at(k)->push_back(Candidate((*it_ref)->point, px));
grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
overlap_kfs.back().second++;
}
}
++it_ref;
}
}
/*{
for (auto&& cell : grid_.cells) {
if (reprojectCell(*cell, frame, log_))
++n_matches_;
if (n_matches_ > (size_t) Config::maxFts())
break;
}
}*/
//exit(0);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Reprojector::pointQualityComparator(Candidate &lhs, Candidate &rhs) {
if (lhs.pt->type_ > rhs.pt->type_)
return true;
return false;
}
bool Reprojector::reprojectCell(Cell &cell, FramePtr frame, FILE* log_) {
if(cell.size()<1){
return false;
}
cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
Cell::iterator it=cell.begin();
while(it!=cell.end())
{
++n_trials_;
if( it->pt ==NULL){
it = cell.erase(it);
continue;
}
if(it->pt->type_ == Point::TYPE_DELETED)
{
it = cell.erase(it);
continue;
}
bool res = matcher_.findMatchDirect(*it->pt, *frame, it->px);
if(!res)
{
it->pt->n_failed_reproj_++;
if(it->pt->n_failed_reproj_ > 15)it->pt.reset();
it = cell.erase(it);
continue;
}
it->pt->n_succeeded_reproj_++;
if(it->pt->n_succeeded_reproj_ > 10)
it->pt->type_ = Point::TYPE_GOOD;
cell.erase(it);
// Maximum one point per cell.
return true;
}
return false;
}
}