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

local m: working version 3

Browse Source
This commit is contained in:
gfuse
2021-08-30 15:06:44 +02:00
parent 95dd4a19d3
commit 5e8e31d691
7 changed files with 14 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/frame.h
+1 -1
View File
@@ -128,7 +128,7 @@ namespace vio {
//TODO add 15 degree roll orientation
SE2 inv=T_f_w_.inverse();
Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX())
q = AngleAxisd(-0.261799, Vector3d::UnitX())
* AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
SE3 out(q.toRotationMatrix(),Vector3d(inv.translation()(0), 0.0,inv.translation()(1)));
GPU_version/vio/include/vio/global.h
+1 -1
View File
@@ -92,7 +92,7 @@ namespace vio
SE3 se3() const{
//Todo add 15 roll orientation
Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX())
q = AngleAxisd(-0.261799, Vector3d::UnitX())
* AngleAxisd(pitch(), Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
return SE3(q.toRotationMatrix(),Vector3d(T2_.translation()(0), 0.0,T2_.translation()(1)));
GPU_version/vio/kernel/compute-residual.cl
+1 -1
View File
@@ -21,7 +21,7 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
float3 error=ref+cur;
float yaw=0.0;
float pitch=error.z;
float roll=0.26;
float roll=-0.26;
float R00=cos(yaw)*cos(pitch);//cos(pitch)
float R01=cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll);//0
float R02=cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll);//sin(pitch)
GPU_version/vio/src/depth_filter.cpp
+1 -1
View File
@@ -218,7 +218,7 @@ namespace vio {
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());
///TODO add 15 degrees roll orientation
Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX())
q = AngleAxisd(-0.261799, Vector3d::UnitX())
* AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1)));
GPU_version/vio/src/frame_handler_mono.cpp
+5 -5
View File
@@ -94,7 +94,7 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
overlap_kfs_.clear();
// create new frame
new_frame_.reset(new Frame(cam_, img.clone(), timestamp));
new_frame_=boost::make_shared<Frame>(cam_, img.clone(), timestamp);
time_=time;
// process frame
UpdateResult res = RESULT_FAILURE;
@@ -107,7 +107,6 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
// set last frame
if(!new_frame_->fts_.empty() || stage_ != STAGE_DEFAULT_FRAME)last_frame_ = new_frame_;
new_frame_.reset();
// finish processing
finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
if(stage_ == STAGE_RELOCALIZING){
@@ -168,11 +167,12 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_->Cov_ = init_f.first;
// sparse image align
//SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false);
//assert(new_frame_.get()== nullptr);
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_.reprojectMap2(new_frame_, overlap_kfs_);
// reprojector_.reprojectMap(new_frame_, overlap_kfs_);
// reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_);
reprojector_.reprojectMap2(new_frame_, overlap_kfs_);
//assert (false);
std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
GPU_version/vio/src/map.cpp
+1
View File
@@ -112,6 +112,7 @@ void Map::getCloseKeyframes(
for(auto kf : keyframes_)
{
// check if kf has overlaping field of view with frame, use therefore KeyPoints
std::cerr<<kf->key_pts_.size()<<'\t';
for(auto keypoint : kf->key_pts_)
{
if(keypoint == nullptr)
GPU_version/vio/src/reprojector.cpp
+4 -28
View File
@@ -71,18 +71,11 @@ void Reprojector::reprojectMap(
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)
{
resetGrid();
// Identify those Keyframes which share a common field of view.
// SVO_START_TIMER("reproject_kfs");
list< pair<FramePtr,double> > close_kfs;
//assert(frame== nullptr);
map_.getCloseKeyframes(frame, close_kfs);
// Sort KFs with overlap according to their closeness
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::second, _2));
// Reproject all mappoints of the closest N kfs with overlap. We only store
// in which grid cell the points fall.
size_t n = 0;
overlap_kfs.reserve(options_.max_n_kfs);
for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end();
@@ -90,16 +83,11 @@ void Reprojector::reprojectMap(
{
FramePtr ref_frame = it_frame->first;
overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));
// Try to reproject each mappoint that the other KF observes
for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end();
it_ftr!=ite_ftr; ++it_ftr)
{
// check if the feature has a mappoint assigned
if((*it_ftr)->point == NULL)
continue;
// make sure we project a point only once
if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
continue;
(*it_ftr)->point->last_projected_kf_id_ = frame->id_;
@@ -108,11 +96,6 @@ void Reprojector::reprojectMap(
}
}
}
//SVO_STOP_TIMER("reproject_kfs");
// Now project all point candidates
// SVO_START_TIMER("reproject_candidates");
{
boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
auto it=map_.point_candidates_.candidates_.begin();
while(it!=map_.point_candidates_.candidates_.end())
@@ -129,21 +112,14 @@ void Reprojector::reprojectMap(
}
++it;
}
} // unlock the mutex when out of scope
//SVO_STOP_TIMER("reproject_candidates");
// Now we go through each grid cell and select one point to match.
// At the end, we should have at maximum one reprojected point per cell.
//SVO_START_TIMER("feature_align");
for(size_t i=0; i<grid_.cells.size(); ++i)
{
// we prefer good quality points over unkown quality (more likely to match)
// and unknown quality over candidates (position not optimized)
if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
++n_matches_;
if(n_matches_ > (size_t) Config::maxFts())
break;
}
// SVO_STOP_TIMER("feature_align");
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO - First reimplementation
@@ -373,7 +349,7 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
it = cell.erase(it);
continue;
}
/*bool found_match = true;
bool found_match = true;
if(options_.find_match_direct)
found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
if(!found_match)
@@ -385,7 +361,7 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
map_.point_candidates_.deleteCandidatePoint(it->pt);
it = cell.erase(it);
continue;
}*/
}
it->pt->n_succeeded_reproj_++;
if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
it->pt->type_ = Point::TYPE_GOOD;