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

apply all chenges in dev 2

Browse Source
This commit is contained in:
gfuse
2021-09-21 15:47:44 +02:00
parent 93ee49d5a2
commit d6e035fb90
6 changed files with 5706 additions and 1726 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+4870 -870
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+816 -834
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/frame.h
+1 -1
View File
@@ -212,7 +212,7 @@ namespace vio {
void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr); void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr);
/// Get the average depth of the features in the image. /// Get the average depth of the features in the image.
bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min); bool getSceneDepth(const FramePtr frame, double& depth_mean, double& depth_min);
} // namespace frame_utils } // namespace frame_utils
} // namespace vio } // namespace vio
GPU_version/vio/include/vio/ukf.h
+8 -8
View File
@@ -30,7 +30,7 @@ public:
} }
t_cmd=time; t_cmd=time;
state_h_(2)=state_(2)+dt*dpitch; state_h_(2)=state_(2)+dt*dpitch;
state_h_(1)=state_(1)+dt*(dy*cos(state_(2))-dx*sin(state_(2))); state_h_(1)=state_(1)+2.0*dt*(dy*cos(state_(2))-dx*sin(state_(2)));
state_h_(0)=state_(0)+dt*(dx*cos(state_(2))+dy*sin(state_(2))); state_h_(0)=state_(0)+dt*(dx*cos(state_(2))+dy*sin(state_(2)));
Eigen::Matrix<double,3,3> R; Eigen::Matrix<double,3,3> R;
R<<vio::Config::Cmd_Cov(),0,0, R<<vio::Config::Cmd_Cov(),0,0,
@@ -38,7 +38,7 @@ public:
0,0,vio::Config::Cmd_Cov(); 0,0,vio::Config::Cmd_Cov();
Eigen::Matrix<double,3,3> G; Eigen::Matrix<double,3,3> G;
G<<1.0,0,dt*(-dx*sin(state_(2))+dy*cos(state_(2))), G<<1.0,0,dt*(-dx*sin(state_(2))+dy*cos(state_(2))),
0,1.0,dt*(-dy*sin(state_(2))-dx*cos(state_(2))), 0,1.0,2.0*dt*(-dy*sin(state_(2))-dx*cos(state_(2))),
0,0,1.0; 0,0,1.0;
cov_h_=G*cov_*G.transpose()+R; cov_h_=G*cov_*G.transpose()+R;
} }
@@ -54,19 +54,19 @@ public:
Eigen::Matrix<double,3,1> E; Eigen::Matrix<double,3,1> E;
Eigen::Matrix<double,3,3> k; Eigen::Matrix<double,3,3> k;
H<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))), H<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),
0,1.0,-pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))), 0,1.0,-2.0*pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),
0,0,1.0; 0,0,1.0;
Q<<vio::Config::ACC_Noise(),0,0, Q<<vio::Config::ACC_Noise(),0,0,
0,vio::Config::ACC_Noise(),0, 0,vio::Config::ACC_Noise(),0,
0,0,vio::Config::GYO_Noise(); 0,0,vio::Config::GYO_Noise();
E(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)))-state_h_(0); E(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)))-state_h_(0);
E(1)=state_(1)+pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1); E(1)=state_(1)+2.0*pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
E(2)=state_(2)+dt*dpitch-state_h_(2); E(2)=state_(2)+dt*dpitch-state_h_(2);
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse(); k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+k*E; state_=state_h_+k*E;
cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_; cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
state_h_(2)=state_(2)+dt*dpitch; state_h_(2)=state_(2)+dt*dpitch;
state_h_(1)=state_(1)+pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))); state_h_(1)=state_(1)+2.0*pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)));
state_h_(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))); state_h_(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)));
Eigen::Matrix<double,3,3> R; Eigen::Matrix<double,3,3> R;
R<<vio::Config::ACC_Noise(),0,0, R<<vio::Config::ACC_Noise(),0,0,
@@ -74,7 +74,7 @@ public:
0,0,vio::Config::GYO_Noise(); 0,0,vio::Config::GYO_Noise();
Eigen::Matrix<double,3,3> G; Eigen::Matrix<double,3,3> G;
G<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))), G<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),
0,1.0,-pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))), 0,1.0,-2.0*pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),
0,0,1.0; 0,0,1.0;
cov_h_=G*cov_*G.transpose()+R; cov_h_=G*cov_*G.transpose()+R;
@@ -114,8 +114,8 @@ public:
}; };
void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) { void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) {
if(!start_)return; if(!start_)return;
/*x=pow(x,3);//picked up from your code if(abs(x)<1.0)x=pow(x,3);//picked up from your code
y=pow(y,3);//picked up from your code*/ if(abs(y)<1.0)y=pow(y,3);//picked up from your code
boost::unique_lock<boost::mutex> lock(ekf_mut_); boost::unique_lock<boost::mutex> lock(ekf_mut_);
filter_->correct(x,y,theta,1e-9*time.toNSec()); filter_->correct(x,y,theta,1e-9*time.toNSec());
}; };
GPU_version/vio/src/frame.cpp
+4 -7
View File
@@ -171,22 +171,19 @@ void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr)
} }
} }
bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min) bool getSceneDepth(const FramePtr frame, double& depth_mean, double& depth_min)
{ {
vector<double> depth_vec; vector<double> depth_vec;
//depth_vec.reserve(frame.fts_.size()); //depth_vec.reserve(frame.fts_.size());
depth_min = std::numeric_limits<double>::max(); depth_min = std::numeric_limits<double>::max();
for(auto it=frame.fts_.begin(), ite=frame.fts_.end(); it!=ite; ++it) for(auto it=frame->fts_.begin(), ite=frame->fts_.end(); it!=ite; ++it)
{ {
if((*it)->point != NULL) if((*it)->point != NULL)
{ {
double z = frame.w2f((*it)->point->pos_).z(); double z = frame->w2f((*it)->point->pos_).z();
if(z>0 && z<20.0){
depth_vec.push_back(z); depth_vec.push_back(z);
depth_min = fmin(z, depth_min); depth_min = fmin(z, depth_min);
}else{
(*it)->point.reset();
}
} }
} }
if(depth_vec.empty()) if(depth_vec.empty())
GPU_version/vio/src/frame_handler_mono.cpp
+7 -6
View File
@@ -161,16 +161,17 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
} }
new_frame_->setKeyframe(); new_frame_->setKeyframe();
double depth_mean, depth_min; double depth_mean, depth_min;
frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min); depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
/* frame_utils::getSceneDepth(*klt_homography_init_->frame_ref_, depth_mean, depth_min); frame_utils::getSceneDepth(klt_homography_init_->frame_ref_, depth_mean, depth_min);
depth_filter_->addKeyframe(klt_homography_init_->frame_ref_, depth_mean, 0.5*depth_min);*/ depth_filter_->addKeyframe(klt_homography_init_->frame_ref_, depth_mean, 0.5*depth_min);
// add frame to map // add frame to map
map_.addKeyframe(new_frame_); map_.addKeyframe(new_frame_);
stage_ = STAGE_DEFAULT_FRAME; stage_ = STAGE_DEFAULT_FRAME;
klt_homography_init_->reset(); klt_homography_init_->reset();
#if VIO_DEBUG #if VIO_DEBUG
fprintf(log_,"[%s] Init: Selected Second frame. \t The number of features: %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),new_frame_->fts_.size()); 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 #endif
return RESULT_IS_KEYFRAME; return RESULT_IS_KEYFRAME;
} }
@@ -225,7 +226,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(), new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch())); fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()));
#endif #endif
if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.005 || if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.05 ||
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch())>M_PI_2 || sfba_n_edges_final < Config::qualityMinFts()){ fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch())>M_PI_2 || sfba_n_edges_final < Config::qualityMinFts()){
auto init_f= ukfPtr_.get_location(); auto init_f= ukfPtr_.get_location();
new_frame_->T_f_w_=init_f.second; new_frame_->T_f_w_=init_f.second;
@@ -257,7 +258,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
} }
double depth_mean, depth_min; double depth_mean, depth_min;
frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
if(!needNewKf(depth_mean))//edited if(!needNewKf(depth_mean))//edited
{ {