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

Merge branch 'dev' of https://bitbucket.org/majidgeravand/p_33_vio

Browse Source 
 Conflicts:
	GPU_version/vio/include/vio/frame.h
	GPU_version/vio/include/vio/global.h
	GPU_version/vio/kernel/compute-residual.cl
	GPU_version/vio/src/depth_filter.cpp
	GPU_version/vio/src/frame_handler_mono.cpp
This commit is contained in:
VahdPourAmin
2021-08-30 08:21:46 +02:00
parent 290c3ca057 3e3cf69d98
commit 04591fe1e8
10 changed files with 58 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/depth_filter.h
+1 -1
View File
@@ -150,7 +150,7 @@ protected:
double new_keyframe_min_depth_; //!< Minimum depth in the new keyframe. Used for range in new seeds.
double new_keyframe_mean_depth_; //!< Maximum depth in the new keyframe. Used for range in new seeds.
Matcher matcher_;
uint seeds_cont_=0;
//uint seeds_cont_=0;
/// Initialize new seeds from a frame.
void initializeSeeds(FramePtr frame);
GPU_version/vio/include/vio/matcher.h
+1 -1
View File
@@ -87,7 +87,7 @@ public:
max_epi_search_steps(1000),
subpix_refinement(true),
epi_search_edgelet_filtering(true),
epi_search_edgelet_max_angle(0.7)
epi_search_edgelet_max_angle(0.3)
{}
} options_;
GPU_version/vio/include/vio/ukf.h
+39 -38
View File
@@ -17,59 +17,62 @@ public:
};
virtual ~Base(){
};
void predict(double dx,double dz,double dpitch,double time){
if(t_==0.0){
t_=time;
void predict(double dx,double dy,double dpitch,double time){
double dt=time-t_cmd;
if(t_cmd==0.0){
t_cmd=time;
return;
}
double dt=time-t_;
t_=time;
t_cmd=time;
state_h_(2)=state_(2)+dt*dpitch;
state_h_(1)=state_(1)+dz*cos(state_(2))-dx*sin(state_(2));
state_h_(0)=state_(0)+dx*cos(state_(2))+dz*sin(state_(2));
state_h_(1)=state_(1)+dt*(dy*cos(state_(2))-dx*sin(state_(2)));
state_h_(0)=state_(0)+dt*(dx*cos(state_(2))+dy*sin(state_(2)));
Eigen::Matrix<double,3,3> R;
R<<vio::Config::Cmd_Cov(),0,0,
0,vio::Config::Cmd_Cov(),0,
0,0,vio::Config::Cmd_Cov();
Eigen::Matrix<double,3,3> G;
G<<1.0,0,-dx*sin(state_(2))+dz*cos(state_(2)),
0,1.0,-dz*sin(state_(2))-dx*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,0,1.0;
cov_h_=G*cov_*G.transpose()+R;
predict_up=true;
}
void correct(double ddx, double ddz, double dpitch, double time){
void correct(double ddx, double ddy, double dpitch, double time){
double dt=time-t_imu;
if(t_imu==0.0){
t_imu=time;
return;
}
t_imu=time;
Eigen::Matrix<double,3,3> H;
Eigen::Matrix<double,3,3> Q;
Eigen::Matrix<double,3,1> E;
Eigen::Matrix<double,3,3> k;
double dt=time-t_;
t_=time;
H<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*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,0,1.0;
Q<<vio::Config::ACC_Noise(),0,0,
0,vio::Config::ACC_Noise(),0,
0,0,vio::Config::GYO_Noise();
E(0)=state_(0)+0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)))-state_h_(0);
E(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
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(2)=state_(2)+dt*dpitch-state_h_(2);
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+k*E;
cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
state_h_(2)=state_(2)+dt*dpitch;
state_h_(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)));
state_h_(0)=state_(0)+0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)));
state_h_(1)=state_(1)+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)));
Eigen::Matrix<double,3,3> R;
R<<vio::Config::ACC_Noise(),0,0,
0,vio::Config::ACC_Noise(),0,
0,0,vio::Config::GYO_Noise();
Eigen::Matrix<double,3,3> G;
G<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*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,0,1.0;
cov_h_=G*cov_*G.transpose()+R;
yaw_t_1_=state_(2);
}
void correct(double x, double z,double pitch,size_t match,double time){
@@ -77,7 +80,6 @@ public:
Eigen::Matrix<double,3,3> Q;
Eigen::Matrix<double,3,1> E;
Eigen::Matrix<double,3,3> k;
t_=time;
++match;
H<<1.0,0,0,
0,1.0,0,
@@ -89,17 +91,16 @@ public:
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+(k*E);
cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
yaw_t_1_=state_(2);
predict_up=false;
}
Eigen::Matrix<double,3,3> cov_;
Eigen::Matrix<double, 3, 1> state_;
bool predict_up=false;
private:
double t_=0.0;
double t_cmd=0.0;
Eigen::Matrix<double,3,3> cov_h_;
Eigen::Matrix<double, 3, 1> state_h_;//state{x,z,pitch}
double yaw_t_1_=0.0;
Eigen::Matrix<double, 3, 1> state_h_;//state{x,y,pitch( rotation aound z)} in world frame
double t_imu=0.0;
};
@@ -110,34 +111,34 @@ public:
virtual ~UKF() {
delete filter_;
};
void UpdateIMU(double x,double z,double pitch,const ros::Time& time) {
void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double pitch/*in imu frame*/,const ros::Time& time) {
while(lock)usleep(5);
lock=true;
if(abs(x)<1.0)x=pow(x,3);//picked up from your code
if(abs(z)<1.0)z=pow(z,3);//picked up from your code
if(filter_->predict_up)filter_->correct(x,z,pitch,1e-9*time.toNSec());
if(abs(y)<1.0)y=pow(y,3);//picked up from your code
if(filter_->predict_up)filter_->correct(x,y,pitch,1e-9*time.toNSec());
lock=false;
};
void UpdateCmd(double x,double z,double pitch,const ros::Time& time) {
void UpdateCmd(double x/*in imu frame*/,double y/*in imu frame*/,double pitch/*in imu frame*/,const ros::Time& time) {
while(lock)usleep(5);
lock=true;
filter_->predict(x,z,pitch,1e-9*time.toNSec());
filter_->predict(x,y,pitch,1e-9*time.toNSec());
lock=false;
};
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) {
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) {
while(lock)usleep(5);
lock=true;
pitch=pitch+M_PI;
if(filter_->predict_up)filter_->correct(-x,-z,pitch,match,1e-9*time.toNSec());
roll=roll+M_PI;
if(filter_->predict_up)filter_->correct(-z,-y,roll,match,1e-9*time.toNSec());
lock=false;
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_(0),-filter_->state_(1),pitch_f));
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));
};
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_(0),-filter_->state_(1),pitch_f));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),pitch_f));
}
private:
Base* filter_= nullptr;
GPU_version/vio/out.txt
BIN
View File
Binary file not shown.
GPU_version/vio/param/px30.yaml
+5 -5
View File
@@ -2,8 +2,8 @@ vio:
cam_model: ATAN
cam_width: 640
cam_height: 480
cam_fx: 0.463632
cam_fy: 0.823661
cam_cx: 0.497465
cam_cy: 0.498443
cam_d0: 1.01687
cam_fx: 0.490202
cam_fy: 0.866527
cam_cx: 0.485102
cam_cy: 0.508577
cam_d0: 1.03345
GPU_version/vio/param/vo_fast.yaml
+3 -3
View File
@@ -5,12 +5,12 @@ svo:
quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad.
quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad.
init_min_tracked: 100
init_min_disparity: 60
init_min_disparity: 80
structureoptim_max_pts: 50
kfselect_mindist: 0.01
poseoptim_thresh: 4.0
init_min_inliers: 30
init_min_inliers: 40
ACC_noise: 0.5 #acc white noise in continuous in EKF
GYO_noise: 0.001 #gyro white noise in continuous in EKF
svo_ekf: 1.5 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 1.0 #control vector noise in EKF
Cmd_Cov: 0.1 #control vector noise in EKF
GPU_version/vio/src/matcher.cpp
+1 -1
View File
@@ -249,7 +249,7 @@ bool Matcher::findEpipolarMatchDirect(
return false;
}
size_t n_steps = epi_length_/0.7; // one step per pixel
size_t n_steps = epi_length_/0.3; // one step per pixel
Vector2d step = epi_dir_/n_steps;
if(n_steps > options_.max_epi_search_steps)
GPU_version/vio/src/sparse_img_align_gpu.cpp
+2
View File
@@ -129,6 +129,8 @@ int SparseImgAlignGpu::solve()
x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2]));
if((bool) std::isnan((double) x_[0]) || (bool) std::isnan((double) x_[1]) ||(bool) std::isnan((double) x_[2]))
return 0;
double norm=x_.norm();
if(norm<=0 ||norm > 1.0)x_=Eigen::Vector3d(0.1,0.1,0.1);
return 1;
}
void SparseImgAlignGpu::update()
GPU_version/vio/src/visualizer.cpp
+3 -3
View File
@@ -55,8 +55,8 @@ void Visualizer::publishMinimal(
Quaterniond q(AngleAxisd(odom.second.pitch(), Vector3d::UnitZ()));
geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped);
msg_pose_with_cov->header = header_msg;
msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(1);
msg_pose_with_cov->pose.pose.position.y = odom.second.se2().translation()(0);
msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(0);
msg_pose_with_cov->pose.pose.position.y = odom.second.se2().translation()(1);
msg_pose_with_cov->pose.pose.position.z = 0.0;
msg_pose_with_cov->pose.pose.orientation.x = q.x();
msg_pose_with_cov->pose.pose.orientation.y = q.y();
@@ -71,7 +71,7 @@ void Visualizer::publishMinimal(
pub_pose_with_cov_.publish(msg_pose_with_cov);
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){
publishPointMarker(
pub_points_, Eigen::Vector3d(odom.second.se2().translation()(1),odom.second.se2().translation()(0),0.0), "trajectory",
pub_points_, Eigen::Vector3d(odom.second.se2().translation()(0),odom.second.se2().translation()(1),0.0), "trajectory",
ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5));
}
}
GPU_version/vio/src/vo_node.cpp
+3 -3
View File
@@ -154,7 +154,7 @@ void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
};
imu_in[0] = 0.5*imu_[0]+0.5*imu->linear_acceleration.x;
imu_in[1] = 0.5*imu_[1]+0.5*imu->linear_acceleration.y;
imu_in[2] = 0.5*imu_[2]+0.5*imu->angular_velocity.z;
imu_in[2] = imu->angular_velocity.z;
memcpy(imu_, imu_in, static_cast<std::size_t>(3*sizeof(double)));
vo_->UpdateIMU(imu_in,imu->header.stamp);
imu_time_=imu->header.stamp;
@@ -184,8 +184,8 @@ bool VioNode::getOdom(vio::getOdom::Request &req, vio::getOdom::Response &res) {
res.header.frame_id = "/world";
res.header.seq = trace_id_;
auto odom=vo_->ukfPtr_.get_location();
res.x=odom.second.se2().translation()(1);
res.y=odom.second.se2().translation()(0);
res.x=odom.second.se2().translation()(0);
res.y=odom.second.se2().translation()(1);
res.yaw=odom.second.pitch();
res.cov={odom.first(0,0),odom.first(0,1),odom.first(0,2),
odom.first(1,0),odom.first(1,1),odom.first(1,2),