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

pipline is working but we have a problem number of observation is too high

Browse Source
This commit is contained in:
gfuse
2021-09-30 19:53:22 +02:00
parent 128083fb1e
commit fb37801384
8 changed files with 722 additions and 3481 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+18 -6
View File
@@ -1,6 +1,18 @@
[17:12:03.647] New key frame
[17:12:03.647] BA loop
[17:12:44.004] New key frame
[17:12:44.004] BA loop
[17:12:44.192] New key frame
[17:12:44.192] BA loop
[19:47:24.922] New key frame
[19:47:24.922] BA loop
[19:47:25.028] New key frame
[19:47:25.028] BA loop
[19:47:25.138] New key frame
[19:47:25.138] BA loop
[19:47:25.234] New key frame
[19:47:25.234] BA loop
[19:47:30.451] New key frame
[19:47:30.451] BA loop
[19:47:32.031] New key frame
[19:47:32.031] BA loop
[19:47:44.267] New key frame
[19:47:44.268] BA loop
[19:48:08.881] New key frame
[19:48:08.881] BA loop
[19:49:01.049] New key frame
[19:49:01.049] BA loop
GPU_version/vio/frame_handler_log.txt
+676 -3434
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/ukf.h
+14 -14
View File
@@ -23,14 +23,14 @@ public:
virtual ~Base(){
};
void predict(double dx,double dy,double dpitch,double time){
double dt=time-t_cmd;
if(t_cmd==0.0){
double dt=0.1;
/*if(t_cmd==0.0){
t_cmd=time;
return;
}
}*/
t_cmd=time;
state_h_(2)=state_(2)+dt*dpitch;
state_h_(1)=state_(1)+dt*2.0*(dy*cos(state_(2))-dx*sin(state_(2)));
state_h_(1)=state_(1)+dt*/*2.0**/(dy*cos(state_(2))-dx*sin(state_(2)));
state_h_(0)=state_(0)+dt*(dx*cos(state_(2))+dy*sin(state_(2)));
state_h_(3)=dpitch;
Eigen::Matrix<double,4,4> R;
@@ -40,28 +40,28 @@ public:
0,0,0,vio::Config::Cmd_Cov();
Eigen::Matrix<double,4,4> G;
G<<1.0,0,dt*(-dx*sin(state_(2))+dy*cos(state_(2))),0.0,
0,1.0,dt*2.0*(-dy*sin(state_(2))-dx*cos(state_(2))),0.0,
0,1.0,dt*/*2.0**/(-dy*sin(state_(2))-dx*cos(state_(2))),0.0,
0,0,1.0,dt,
0,0,0,1.0;
cov_h_=G*cov_*G.transpose()+R;
}
void correct(double ddx, double ddy, double dpitch, double time){
double dt=time-t_imu;
if(t_imu==0.0){
double dt=0.005;
/*if(t_imu==0.0){
t_imu=time;
return;
}
}*/
t_imu=time;
Eigen::Matrix<double,4,4> H;
Eigen::Matrix<double,4,4> Q;
Eigen::Matrix<double,4,1> E;
Eigen::Matrix<double,4,4> k;
E(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)))-state_h_(0);
E(1)=state_(1)+2.0*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*state_(3)-state_h_(2);
E(3)=dpitch-state_h_(3);
H<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),0.0,
0,1.0,-2.0*pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0.0,
0,1.0,-/*2.0**/pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0.0,
0,0,1.0,dt,
0,0,0,1.0;
Q<<vio::Config::ACC_Noise(),0,0,0,
@@ -72,7 +72,7 @@ public:
state_=state_h_+k*E;
cov_=(Eigen::MatrixXd::Identity(4,4)-k*H)*cov_h_;
state_h_(2)=state_(2)+dt*state_(3);
state_h_(1)=state_(1)+2.0*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_(3)=state_(3);
Eigen::Matrix<double,4,4> R;
@@ -82,7 +82,7 @@ public:
0,0,0,vio::Config::GYO_Noise();
Eigen::Matrix<double,4,4> G;
G<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),0,
0,1.0,2.0*pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0,
0,1.0,/*2.0**/pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0,
0,0,1.0,dt,
0,0,0,1.0;
cov_h_=G*cov_*G.transpose()+R;
@@ -122,8 +122,8 @@ public:
delete filter_;
};
void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) {
if(abs(x)<1.0)x=pow(x,3);//picked up from your code
if(abs(y)<1.0)y=pow(y,3);//picked up from your code
/* if(abs(x)<1.0)x=pow(x,3);//picked up from your code
if(abs(y)<1.0)y=pow(y,3);//picked up from your code*/
++imu_syn_count_;
if(imu_syn_[imu_syn_count_ % 7]){
imu_syn_[imu_syn_count_ % 7]=false;
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch>
<!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" / -->
<node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
<!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/param/vo_fast.yaml
+2 -2
View File
@@ -12,5 +12,5 @@ svo:
init_min_inliers: 30
ACC_noise: 0.5 #acc white noise in continuous in EKF
GYO_noise: 0.5 #gyro white noise in continuous in EKF
svo_ekf: 0.05 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 1.0 #control vector noise in EKF
svo_ekf: 0.1 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 0.1 #control vector noise in EKF
GPU_version/vio/src/frame_handler_mono.cpp
+6 -1
View File
@@ -276,12 +276,17 @@ bool FrameHandlerMono::needNewKf()
SE2_5 closest_kfs(0,0,0);
for(auto&& it:overlap_kfs_)
{
if(it.first->id_==last_frame_->id_)continue;
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;
#if VIO_DEBUG
fprintf(log_,"[%s] need key frame pitch dis: %f translation dif:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()),(closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm());
#endif
if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.2 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.1)return true;
return false;
}
void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){
GPU_version/vio/src/point.cpp
+3 -8
View File
@@ -184,16 +184,11 @@ void Point::optimize(const size_t n_iter)
break;
}
/* for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * (*it)->point->pos_));
//e /= (1 << (*it)->level);
// std::cerr<<"error - point.h : "<<e.norm()<<std::endl;
if (e.norm() > 1.0) //{
// we don't need to delete a reference in the point since it was not created yet
if (e.norm() > 1.0)
(*it)->point->n_failed_reproj_++;
// } else
// it++;
}*/
}
}
GPU_version/vio/src/reprojector.cpp
+2 -15
View File
@@ -81,18 +81,10 @@ namespace vio {
std::vector<cv::KeyPoint> keypoints_cur;
list<std::shared_ptr<Feature>>::iterator it_cur=keypoints.begin();
cv::Mat cur_des=cv::Mat(keypoints.size(),64,CV_8UC1);
// std::cerr<<"84"<<std::endl;
//cv::Mat maskup = cv::Mat(keypoints.size(),64,CV_8UC1);
//cv::Mat maskdown = cv::Mat(keypoints.size(),64,CV_8UC1);
cv::Mat maskup = cv::Mat_<uchar>(1, keypoints.size());
cv::Mat maskdown = cv::Mat_<uchar>(1, keypoints.size());
// std::cerr<<"88"<<std::endl;
//// uint8_t one[64] = {255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
//// 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
//// 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
//// 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255};
//// uint8_t zero[64] = {0};
// std::cerr<<"94"<<std::endl;
for (int i=0;i<keypoints.size() && it_cur !=keypoints.end();++i) {
keypoints_cur.push_back(cv::KeyPoint((*it_cur)->px.x(), (*it_cur)->px.y(), 7.f, (*it_cur)->score));
@@ -100,18 +92,13 @@ namespace vio {
if ((*it_cur)->px.y() < frame->img().rows/2) {
maskup.at<uchar>(0, i) = 1;
maskdown.at<uchar>(0, i) = 0;
//memcpy(maskup.data + (i * 64), one, sizeof(uint8_t) * 64);
//memcpy(maskdown.data + (i * 64), zero, sizeof(uint8_t) * 64);
}
else{
maskup.at<uchar>(0, i) = 0;
maskdown.at<uchar>(0, i) = 1;
//memcpy(maskup.data + (i * 64), zero, sizeof(uint8_t) * 64);
//memcpy(maskdown.data + (i * 64), one, sizeof(uint8_t) * 64);
}
++it_cur;
}
// std::cerr<<"114"<<std::endl;
list<pair<FramePtr, double> > close_kfs;
map_.getCloseKeyframes(frame, close_kfs);
if (!last_frame->fts_.empty())