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

review with original version

Browse Source
This commit is contained in:
gfuse
2021-08-30 19:00:17 +02:00
parent 66129efe92
commit fb6de808cd
6 changed files with 67 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/kernel/compute-residual.cl
-27
View File
@@ -34,33 +34,6 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
return (float3)(R00*ref_feature.x+R01*ref_feature.y+R02*ref_feature.z+error.x,
R10*ref_feature.x+R11*ref_feature.y+R12*ref_feature.z,
R20*ref_feature.x+R21*ref_feature.y+R22*ref_feature.z+error.y);
/*
if(ref.z < 0.0)
ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z;
else
ref.z = ref.z - 3.1415926535897932384626433832795028841971693993;
float cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z);
float sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z);
return (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z),
ref_feature.y,
-1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));
// alfa = 0, betha = 15 degree - 0.261799 radian
//return (float3)(((float)(cos(0.261799)*ref_feature.x)+(float)(sin(0.261799)*sin(cur.z)*ref_feature.y)+(float)(sin(0.261799)*cos(cur.z)*ref_feature.z)),
// ((float)(cos(cur.z)*ref_feature.y)-(float)(sin(cur.z)*ref_feature.z)),
// ((float)(-1*sin(0.261799)*ref_feature.x)+(float)(cos(0.261799)*sin(cur.z)*ref_feature.y)+(float)(cos(0.261799)*cos(cur.z)*ref_feature.z)));
//
// betha = 0, alfa = 15 degree - 0.261799 radian
//return (float3)(((float)(cos(0.261799)*ref_feature.x)+(float)(-1*sin(0.261799)*cos(cur.z)*ref_feature.y)+(float)(sin(0.261799)*sin(cur.z)*ref_feature.z)),
// ((float)(sin(0.261799)*ref_feature.x)+(float)(cos(0.261799)*cos(cur.z)*ref_feature.y)-(float)(cos(0.261799)*sin(cur.z)*ref_feature.z)),
// ((float)(sin(cur.z)*ref_feature.y)+(float)(cos(cur.z)*ref_feature.z)));
}
return (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z),
ref_feature.y,
-1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));*/
}
void jacobian_xyz2uv(float3 xyz_in_f, float* J)
GPU_version/vio/src/depth_filter.cpp
+3 -1
View File
@@ -190,6 +190,8 @@ namespace vio {
}
}
updateSeeds(frame);
if(frame->isKeyframe())
initializeSeeds(frame);
}
}
@@ -215,7 +217,7 @@ namespace vio {
continue;
}
// check if point is visible in the current image
SE2 T(it->ftr->frame->T_f_w_.pitch()-frame->T_f_w_.pitch(),it->ftr->frame->T_f_w_.se2().translation()- frame->T_f_w_.se2().translation());
SE2 T(it->ftr->frame->T_f_w_.pitch()+frame->T_f_w_.pitch()+M_PI,it->ftr->frame->T_f_w_.se2().translation()- frame->T_f_w_.se2().translation());
///TODO add 15 degrees roll orientation
Quaterniond q;
q = AngleAxisd(-0.261799, Vector3d::UnitX())
GPU_version/vio/src/feature_alignment.cpp
+6 -6
View File
@@ -36,9 +36,9 @@ bool align1D(
Vector2d& cur_px_estimate,
double& h_inv)
{
const int halfpatch_size_ = 1;
const int patch_size = 2;
const int patch_area = 4;
const int halfpatch_size_ = 4;
const int patch_size = 8;
const int patch_area = 64;
bool converged=false;
// compute derivative of template and prepare inverse compositional
@@ -163,9 +163,9 @@ bool align2D(
return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate);
#endif
const int halfpatch_size_ = 1;
const int patch_size_ = 2;
const int patch_area_ = 4;
const int halfpatch_size_ = 4;
const int patch_size_ = 8;
const int patch_area_ = 64;
bool converged=false;
// compute derivative of template and prepare inverse compositional
GPU_version/vio/src/frame.cpp
+1 -1
View File
@@ -138,7 +138,7 @@ void Frame::removeKeyPoint(Feature* ftr)
bool Frame::isVisible(const Vector3d& xyz_w) const
{
Vector3d xyz_f = Vector3d(this->getSE3Inv()*xyz_w);
Vector3d xyz_f = Vector3d(this->se3()*xyz_w);
if(xyz_f.z() < 0.0)
return false; // point is behind the camera
Vector2d px = f2c(xyz_f);
GPU_version/vio/src/frame_handler_mono.cpp
+1 -1
View File
@@ -171,7 +171,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE;
//reprojector_.reprojectMap(new_frame_, overlap_kfs_);
// reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_);
//reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_);
reprojector_.reprojectMap2(new_frame_, overlap_kfs_);
//assert (false);
GPU_version/vio/src/reprojector.cpp
+56 -83
View File
@@ -70,55 +70,63 @@ void Reprojector::reprojectMap(
FramePtr frame,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)
{
resetGrid();
list< pair<FramePtr,double> > close_kfs;
map_.getCloseKeyframes(frame, close_kfs);
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::second, _2));
size_t n = 0;
overlap_kfs.reserve(options_.max_n_kfs);
for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end();
it_frame!=ite_frame && n<options_.max_n_kfs; ++it_frame, ++n)
{
FramePtr ref_frame = it_frame->first;
overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));
for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end();
it_ftr!=ite_ftr; ++it_ftr)
{
if((*it_ftr)->point == NULL)
continue;
if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
continue;
(*it_ftr)->point->last_projected_kf_id_ = frame->id_;
if(reprojectPoint(frame, (*it_ftr)->point)){
overlap_kfs.back().second++;
}
}
}
boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
auto it=map_.point_candidates_.candidates_.begin();
while(it!=map_.point_candidates_.candidates_.end())
{
if(!reprojectPoint(frame, it->first))
{
it->first->n_failed_reproj_ += 3;
if(it->first->n_failed_reproj_ > 30)
resetGrid();
list< pair<FramePtr,double> > close_kfs;
map_.getCloseKeyframes(frame, close_kfs);
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::second, _2));
size_t n = 0;
overlap_kfs.reserve(options_.max_n_kfs);
for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end();
it_frame!=ite_frame && n<options_.max_n_kfs; ++it_frame, ++n)
{
map_.point_candidates_.deleteCandidate(*it);
it = map_.point_candidates_.candidates_.erase(it);
continue;
FramePtr ref_frame = it_frame->first;
overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));
for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end();
it_ftr!=ite_ftr; ++it_ftr)
{
if((*it_ftr)->point == NULL)
continue;
if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
continue;
(*it_ftr)->point->last_projected_kf_id_ = frame->id_;
if(reprojectPoint(frame, (*it_ftr)->point)){
overlap_kfs.back().second++;
}
}
}
{
boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
auto it=map_.point_candidates_.candidates_.begin();
while(it!=map_.point_candidates_.candidates_.end())
{
if(!reprojectPoint(frame, it->first))
{
it->first->n_failed_reproj_ += 3;
if(it->first->n_failed_reproj_ > 30)
{
map_.point_candidates_.deleteCandidate(*it);
it = map_.point_candidates_.candidates_.erase(it);
continue;
}
}
++it;
}
}
for(size_t i=0; i<grid_.cells.size(); ++i)
{
if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
++n_matches_;
if(n_matches_ > (size_t) Config::maxFts())
break;
}
}
++it;
}
for(size_t i=0; i<grid_.cells.size(); ++i)
{
if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
++n_matches_;
if(n_matches_ > (size_t) Config::maxFts())
break;
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO - First reimplementation
void Reprojector::reprojectMap1(
@@ -214,11 +222,6 @@ void Reprojector::reprojectMap(
extractor->compute(frame->img(), keypoints_cur, descriptors_cur);
list< pair<FramePtr,double> > close_kfs;
map_.getCloseKeyframes(frame, close_kfs);
<<<<<<< HEAD
// Sort KFs with overlap according to their closeness
=======
>>>>>>> internal/dev2
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::second, _2));
size_t n = 0;
@@ -228,20 +231,14 @@ void Reprojector::reprojectMap(
{
std::vector<cv::KeyPoint> keypoints_kfs;
FramePtr ref_frame = it_frame->first;
<<<<<<< HEAD
// Try to reproject each mappoint that the other KF observes
=======
overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));
>>>>>>> internal/dev2
for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end();
it_ftr!=ite_ftr; ++it_ftr)
{
if((*it_ftr)->point == NULL)
continue;
<<<<<<< HEAD
// make sure we project a point only once
=======
>>>>>>> internal/dev2
if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
continue;
(*it_ftr)->point->last_projected_kf_id_ = frame->id_;
@@ -250,26 +247,7 @@ void Reprojector::reprojectMap(
kp->pt.y = (*it_ftr)->px.y();
keypoints_kfs.push_back(*kp);
}
<<<<<<< HEAD
keypoints_ref.push_back(keypoints_kfs);
}
//SVO_STOP_TIMER("reproject_kfs");
cv::Mat descriptors_ref, descriptors_cur;
std::vector<cv::KeyPoint> keypoints_cur;
cv::FAST(frame->img(),keypoints_cur,50);
std::vector<cv::DMatch> matches;
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 4);
cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
int ii = 0;
extractor->compute(frame->img(), keypoints_cur, descriptors_cur);
for(auto it_frame = close_kfs.begin(), ite_frame = close_kfs.end();
it_frame != ite_frame; ++it_frame, ++ii)
{
extractor->compute(it_frame->first->img(), keypoints_ref[ii], descriptors_ref);
=======
extractor->compute(it_frame->first->img(), keypoints_kfs, descriptors_ref);
>>>>>>> internal/dev2
matcher->match(descriptors_ref, descriptors_cur, matches);
//std::cerr<<keypoints_cur.size()<<"\t"<<descriptors_cur.size<<"\t"<<keypoints_kfs.size()<<"\t"<<descriptors_ref.size<<"\t"<<matches.size()<<std::endl;
for (int i; i < matches.size(); i++){
@@ -281,11 +259,6 @@ void Reprojector::reprojectMap(
if (reprojectPoint1(frame, (*fts2)->point, keypoints_cur[i].pt.x, keypoints_cur[i].pt.y)) {
overlap_kfs.back().second++;
}
<<<<<<< HEAD
//keypoints_cur[i].class_id = 0;
=======
>>>>>>> internal/dev2
}
}
boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);