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

latest working version

Browse Source
This commit is contained in:
gfuse
2021-08-31 19:04:34 +02:00
parent 6cafe07db2
commit 034158ef39
9 changed files with 18 additions and 39 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/include/vio/point.h
+1 -1
View File
@@ -54,7 +54,7 @@ public:
size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
g2oPoint* v_pt_; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
int last_published_ts_; //!< Timestamp of last publishing.
int last_projected_kf_id_; //!< Flag for the reprojection: don't reproject a pt twice.
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.
GPU_version/vio/include/vio/ukf.h
+4 -9
View File
@@ -125,20 +125,15 @@ public:
filter_->predict(x,y,pitch,1e-9*time.toNSec());
lock=false;
};
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double y/*in camera frame*/,double z/*in camera frame*/,double roll/*in camera frame*/,size_t match,ros::Time& time) {
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/,size_t match,ros::Time& time) {
while(lock)usleep(5);
lock=true;
roll=roll+M_PI;
if(filter_->predict_up)filter_->correct(z,y,roll,match,1e-9*time.toNSec());
//if(filter_->predict_up)filter_->correct(z,x,pitch,match,1e-9*time.toNSec());
lock=false;
double roll_f;
roll_f=filter_->state_(2)+M_PI;
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),roll_f));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(1),filter_->state_(0),filter_->state_(2)));
};
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
double pitch_f;
pitch_f=filter_->state_(2)+M_PI;
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),pitch_f));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(1),filter_->state_(0),filter_->state_(2)));
}
private:
Base* filter_= nullptr;
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
@@ -220,7 +220,7 @@ namespace vio {
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())
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
+2 -2
View File
@@ -168,8 +168,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
// 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;
//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_, last_frame_,overlap_kfs_);
GPU_version/vio/src/point.cpp
+2 -2
View File
@@ -31,7 +31,7 @@ Point::Point(const Vector3d& pos) :
n_obs_(0),
v_pt_(NULL),
last_published_ts_(0),
last_projected_kf_id_(-1),
last_frame_overlap_id_(-10),
type_(TYPE_UNKNOWN),
n_failed_reproj_(0),
n_succeeded_reproj_(0),
@@ -45,7 +45,7 @@ Point::Point(const Vector3d& pos, Feature* ftr) :
n_obs_(1),
v_pt_(NULL),
last_published_ts_(0),
last_projected_kf_id_(-1),
last_frame_overlap_id_(-10),
type_(TYPE_UNKNOWN),
n_failed_reproj_(0),
n_succeeded_reproj_(0),
GPU_version/vio/src/reprojector.cpp
+5 -21
View File
@@ -92,9 +92,9 @@ void Reprojector::reprojectMap(
{
if((*it_ftr)->point == NULL)
continue;
if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
if((*it_ftr)->point->last_frame_overlap_id_ == frame->id_)
continue;
(*it_ftr)->point->last_projected_kf_id_ = frame->id_;
(*it_ftr)->point->last_frame_overlap_id_= frame->id_;
if(reprojectPoint(frame, (*it_ftr)->point)){
overlap_kfs.back().second++;
}
@@ -240,15 +240,15 @@ void Reprojector::reprojectMap(
extractor->compute(it_frame.item.first->img(), keypoints_kfs, descriptors_ref);
matcher->match(descriptors_ref, descriptors_cur, matches);
for(auto&& f: _for(it_frame.item.first->fts_)){
// if(f.item->point->last_projected_kf_id_ == it_frame.item.first->id_ || f.index > matches.size()-1)continue;
// f.item->point->last_projected_kf_id_ = it_frame.item.first->id_;
if(f.item->point==NULL)f.item->point=new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(),f.item->px.y()));
if(f.item->point->last_frame_overlap_id_ == frame->id_)continue;
f.item->point->last_frame_overlap_id_ = frame->id_;
for(auto&& match:matches){
if(match.queryIdx!=f.index)continue;
assert(keypoints_cur.size()>match.trainIdx);
const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) * grid_.grid_n_cols
+ static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
assert(grid_.cells.at(k)!= nullptr);
if(f.item->point==NULL)continue;
Vector2d px((int)keypoints_cur.at(match.trainIdx).pt.x,(int)keypoints_cur.at(match.trainIdx).pt.y);
grid_.cells.at(k)->push_back(Candidate(f.item->point,px));
overlap_kfs.back().second++;
@@ -256,22 +256,6 @@ void Reprojector::reprojectMap(
}
}
}
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))