297 lines
11 KiB
C++
297 lines
11 KiB
C++
// This file is part of SVO - Semi-direct Visual Odometry.
|
|
//
|
|
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
|
|
// (Robotics and Perception Group, University of Zurich, Switzerland).
|
|
//
|
|
// SVO is free software: you can redistribute it and/or modify it under the
|
|
// terms of the GNU General Public License as published by the Free Software
|
|
// Foundation, either version 3 of the License, or any later version.
|
|
//
|
|
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
|
|
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
|
|
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
#include <vio/config.h>
|
|
#include <vio/frame_handler_mono.h>
|
|
#include <vio/map.h>
|
|
#include <vio/frame.h>
|
|
#include <vio/feature.h>
|
|
#include <vio/point.h>
|
|
#include <vio/pose_optimizer.h>
|
|
#include <vio/sparse_img_align.h>
|
|
#include <vio/global_optimizer.h>
|
|
#include <vio/for_it.hpp>
|
|
#include <assert.h>
|
|
#if VIO_DEBUG
|
|
#include <sys/stat.h>
|
|
#endif
|
|
|
|
namespace vio {
|
|
FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,3,1>& init) :
|
|
FrameHandlerBase(),
|
|
cam_(cam),
|
|
reprojector_(cam_, map_),
|
|
ba_glob_(NULL),
|
|
ukfPtr_(init),
|
|
time_(ros::Time::now())
|
|
{
|
|
gpu_fast_= new opencl(cam_);
|
|
gpu_fast_->make_kernel("fast_gray");
|
|
cv::Mat img=cv::Mat(cv::Size(700, 500),CV_8UC1);
|
|
std::vector<bool> errors;
|
|
errors.push_back(gpu_fast_->write_buf(0,0,img));
|
|
cl_int2 corners_[350000]={0};
|
|
errors.push_back(gpu_fast_->write_buf(0,1,350000,corners_));
|
|
cl_int icorner[1]={0};
|
|
errors.push_back(gpu_fast_->write_buf(0,2,1,icorner));
|
|
gpu_fast_->make_kernel("compute_residual");
|
|
errors.push_back(gpu_fast_->write_buf(1,0,img));
|
|
errors.push_back(gpu_fast_->write_buf(1,1,img));
|
|
cl_float3 pose[1]={0.0};
|
|
errors.push_back(gpu_fast_->write_buf(1,2,3,pose));
|
|
errors.push_back(gpu_fast_->write_buf(1,3,3,pose));
|
|
cl_float3 f[300]={0};
|
|
errors.push_back(gpu_fast_->write_buf(1,4,300,f));
|
|
cl_float2 px[300]={0};
|
|
errors.push_back(gpu_fast_->write_buf(1,5,300,px));
|
|
cl_float e[300]={0};
|
|
errors.push_back(gpu_fast_->write_buf(1,7,300,e));
|
|
cl_float H[9*300]={0};
|
|
errors.push_back(gpu_fast_->write_buf(1,8,9*300,H));
|
|
cl_float3 J[300];
|
|
errors.push_back(gpu_fast_->write_buf(1,9,300,J));
|
|
cl_float chi2[300]={0.01};
|
|
errors.push_back(gpu_fast_->write_buf(1,10,300,chi2));
|
|
for(auto e:errors){
|
|
if(e!=true){
|
|
std::cerr<<"Failed to write into GPU goodbye :) "<<'\n';
|
|
exit(0);
|
|
}
|
|
}
|
|
initialize();
|
|
#if VIO_DEBUG
|
|
log_ =fopen((std::string(PROJECT_DIR)+"/frame_handler_log.txt").c_str(),"w+");
|
|
assert(log_);
|
|
chmod((std::string(PROJECT_DIR)+"/frame_handler_log.txt").c_str(), ACCESSPERMS);
|
|
klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_,&ukfPtr_,log_);
|
|
#else
|
|
klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_&ukfPtr_);
|
|
#endif
|
|
}
|
|
|
|
void FrameHandlerMono::initialize()
|
|
{
|
|
feature_detection::DetectorPtr feature_detector(
|
|
new feature_detection::FastDetector(
|
|
cam_->width(), cam_->height(), Config::gridSize(),gpu_fast_, Config::nPyrLevels()));
|
|
ba_glob_ = new BA_Glob(map_);
|
|
ba_glob_->startThread();
|
|
}
|
|
|
|
FrameHandlerMono::~FrameHandlerMono()
|
|
{
|
|
delete ba_glob_;
|
|
}
|
|
|
|
void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const ros::Time& time)
|
|
{
|
|
|
|
if(!startFrameProcessingCommon(timestamp)){
|
|
return;
|
|
}
|
|
// some cleanup from last iteration, can't do before because of visualization
|
|
overlap_kfs_.clear();
|
|
// create new frame
|
|
new_frame_=std::make_shared<Frame>(cam_, img.clone(), timestamp);
|
|
time_=time;
|
|
// process frame
|
|
UpdateResult res = RESULT_FAILURE;
|
|
if(stage_ == STAGE_DEFAULT_FRAME)
|
|
res = processFrame();
|
|
else if(stage_ == STAGE_SECOND_FRAME)
|
|
res = processSecondFrame();
|
|
else if(stage_ == STAGE_FIRST_FRAME)
|
|
res = processFirstFrame();
|
|
last_frame_ = new_frame_;
|
|
// finish processing
|
|
finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
|
|
#if VIO_DEBUG
|
|
fprintf(log_,"[%s] frame process finished the id is: %d the obs is:%d \n",vio::time_in_HH_MM_SS_MMM().c_str(),
|
|
last_frame_->id_,last_frame_->nObs());
|
|
#endif
|
|
|
|
}
|
|
|
|
FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
|
|
{
|
|
if(klt_homography_init_->addFirstFrame(new_frame_) == initialization::FAILURE){
|
|
return RESULT_NO_KEYFRAME;
|
|
}
|
|
new_frame_->setKeyframe();
|
|
map_.reset();
|
|
map_.addKeyframe(new_frame_);
|
|
stage_ = STAGE_SECOND_FRAME;
|
|
#if VIO_DEBUG
|
|
fprintf(log_,"[%s] Init: Selected first frame. \n",vio::time_in_HH_MM_SS_MMM().c_str());
|
|
#endif
|
|
return RESULT_IS_KEYFRAME;
|
|
}
|
|
|
|
FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
|
|
{
|
|
initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_);
|
|
#if VIO_DEBUG
|
|
fprintf(log_,"[%s] Init: distance between the first and current frame is 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()-last_frame_->T_f_w_.se2().translation().x(),
|
|
new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
|
|
new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch());
|
|
#endif
|
|
if(res == initialization::NO_KEYFRAME){
|
|
return RESULT_NO_KEYFRAME;
|
|
}else if(res == initialization::FAILURE){
|
|
stage_ = STAGE_FIRST_FRAME;
|
|
return RESULT_FAILURE;
|
|
}
|
|
new_frame_->setKeyframe();
|
|
double depth_mean, depth_min;
|
|
frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
|
|
// add frame to map
|
|
map_.addKeyframe(new_frame_);
|
|
stage_ = STAGE_DEFAULT_FRAME;
|
|
klt_homography_init_->reset();
|
|
#if VIO_DEBUG
|
|
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
|
|
ROS_INFO("VIO initialized :)");
|
|
ROS_INFO("Running ...");
|
|
ba_glob_->new_key_frame();
|
|
return RESULT_IS_KEYFRAME;
|
|
}
|
|
|
|
|
|
FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
|
|
{
|
|
auto init_f= ukfPtr_.get_location();
|
|
new_frame_->T_f_w_=init_f.second;
|
|
new_frame_->Cov_ = init_f.first;
|
|
// sparse image align
|
|
SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
|
|
size_t err=img_align.run(last_frame_, new_frame_, log_);
|
|
if(err==-1){
|
|
#if VIO_DEBUG
|
|
fprintf(log_,"[%s] Image Alignment failed -1\n",vio::time_in_HH_MM_SS_MMM().c_str());
|
|
#endif
|
|
new_frame_=last_frame_;
|
|
return RESULT_FAILURE;
|
|
}else if(err==0){
|
|
#if VIO_DEBUG
|
|
fprintf(log_,"[%s] Image Alignment failed 0\n",vio::time_in_HH_MM_SS_MMM().c_str());
|
|
#endif
|
|
new_frame_=last_frame_;
|
|
return RESULT_FAILURE;
|
|
}
|
|
reprojector_.reprojectMap2(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(),
|
|
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()));
|
|
#endif
|
|
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_);
|
|
#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(),
|
|
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()));
|
|
#endif
|
|
if((init_f.second.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
|
|
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.25*M_PI_2 || sfba_n_edges_final<10){
|
|
new_frame_=last_frame_;
|
|
return RESULT_FAILURE;
|
|
}
|
|
auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),
|
|
new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch());
|
|
new_frame_->T_f_w_ =result.second;
|
|
new_frame_->Cov_ = result.first;
|
|
#if VIO_DEBUG
|
|
fprintf(log_,"[%s] Update EKF and 3D points the number of feature in the new frame: %d and number of obs: %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
|
|
new_frame_->fts_.size(),new_frame_->nObs());
|
|
#endif
|
|
|
|
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
|
|
// select keyframe
|
|
if(!needNewKf())//edited
|
|
{
|
|
return RESULT_NO_KEYFRAME;
|
|
}
|
|
|
|
double depth_mean=0.0, depth_min=0.0;
|
|
frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
|
|
new_frame_->setKeyframe();
|
|
#if VIO_DEBUG
|
|
fprintf(log_,"[%s] Choose frame as a key frame Scene Depth mean:%f ,depth min:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
|
|
depth_mean,
|
|
depth_min);
|
|
#endif
|
|
// new keyframe selected
|
|
for(auto&& it:new_frame_->fts_){
|
|
if(it->point != NULL)it->point->addFrameRef(it);
|
|
}
|
|
|
|
// if limited number of keyframes, remove the one furthest apart
|
|
if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs())
|
|
{
|
|
FramePtr furthest_frame = map_.getFurthestKeyframe(new_frame_->pos());
|
|
map_.safeDeleteFrame(furthest_frame);
|
|
}
|
|
// add keyframe to map
|
|
map_.addKeyframe(new_frame_);
|
|
ba_glob_->new_key_frame();
|
|
return RESULT_IS_KEYFRAME;
|
|
}
|
|
|
|
|
|
void FrameHandlerMono::resetAll()
|
|
{
|
|
resetCommon();
|
|
last_frame_.reset();
|
|
new_frame_.reset();
|
|
overlap_kfs_.clear();
|
|
}
|
|
bool FrameHandlerMono::needNewKf()
|
|
{
|
|
size_t com_obs=0;
|
|
SE2_5 closest_kfs(0,0,0);
|
|
for(auto&& it:overlap_kfs_)
|
|
{
|
|
if(it.second>com_obs){
|
|
com_obs=it.second;
|
|
closest_kfs=SE2_5(it.first->T_f_w_.se2());
|
|
}
|
|
}
|
|
if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.43 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.15)return true;
|
|
return false;
|
|
}
|
|
void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){
|
|
if(value== nullptr)return;
|
|
ukfPtr_.UpdateIMU(value[0],value[1],value[2],time);
|
|
}
|
|
void FrameHandlerMono::UpdateCmd(double* value,const ros::Time& time){
|
|
if(value== nullptr)return;
|
|
ukfPtr_.UpdateCmd(value[0],value[1],value[2],time);
|
|
}
|
|
|
|
} // namespace vio
|