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

add dpich in ekf space

Browse Source
This commit is contained in:
gfuse
2021-09-21 21:46:48 +02:00
parent 78d70c81b0
commit 26eefcd9bc
4 changed files with 68146 additions and 4164 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+64161 -3341
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+3917 -772
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/ukf.h
+64 -47
View File
@@ -13,10 +13,10 @@
class Base{ class Base{
public: public:
Base(const Eigen::Matrix<double, 3, 1> &init) { Base(const Eigen::Matrix<double, 3, 1> &init) {
state_=init; state_=Eigen::Vector4d(init.x(),init.y(),init.z(),0.0);
cov_.setIdentity(); cov_.setIdentity();
cov_*=0.0001; cov_*=0.0001;
state_h_=init; state_h_=Eigen::Vector4d(init.x(),init.y(),init.z(),0.0);
cov_h_.setIdentity(); cov_h_.setIdentity();
cov_h_*=0.0001; cov_h_*=0.0001;
}; };
@@ -30,16 +30,19 @@ 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)+2.0*dt*(dy*cos(state_(2))-dx*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))); state_h_(0)=state_(0)+dt*(dx*cos(state_(2))+dy*sin(state_(2)));
Eigen::Matrix<double,3,3> R; state_h_(3)=dpitch;
R<<vio::Config::Cmd_Cov(),0,0, Eigen::Matrix<double,4,4> R;
0,vio::Config::Cmd_Cov(),0, R<<vio::Config::Cmd_Cov(),0,0,0,
0,0,vio::Config::Cmd_Cov(); 0,vio::Config::Cmd_Cov(),0,0,
Eigen::Matrix<double,3,3> G; 0,0,vio::Config::Cmd_Cov(),0,
G<<1.0,0,dt*(-dx*sin(state_(2))+dy*cos(state_(2))), 0,0,0,vio::Config::Cmd_Cov();
0,1.0,2.0*dt*(-dy*sin(state_(2))-dx*cos(state_(2))), Eigen::Matrix<double,4,4> G;
0,0,1.0; G<<1.0,0,dt*(-dx*sin(state_(2))+dy*cos(state_(2))),0.0,
0,1.0,dt*(-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; cov_h_=G*cov_*G.transpose()+R;
} }
void correct(double ddx, double ddy, double dpitch, double time){ void correct(double ddx, double ddy, double dpitch, double time){
@@ -49,58 +52,64 @@ public:
return; return;
} }
t_imu=time; t_imu=time;
Eigen::Matrix<double,3,3> H; Eigen::Matrix<double,4,4> H;
Eigen::Matrix<double,3,3> Q; Eigen::Matrix<double,4,4> Q;
Eigen::Matrix<double,3,1> E; Eigen::Matrix<double,4,1> E;
Eigen::Matrix<double,3,3> k; Eigen::Matrix<double,4,4> k;
H<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),
0,1.0,-2.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)+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)+2.0*pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1); 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); 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,0,1.0,dt,
0,0,0,1.0;
Q<<vio::Config::ACC_Noise(),0,0,0,
0,vio::Config::ACC_Noise(),0,0,
0,0,vio::Config::GYO_Noise(),0,
0,0,0,vio::Config::GYO_Noise();
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(4,4)-k*H)*cov_h_;
state_h_(2)=state_(2)+dt*dpitch; 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)+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; state_h_(3)=state_(3);
R<<vio::Config::ACC_Noise(),0,0, Eigen::Matrix<double,4,4> R;
0,vio::Config::ACC_Noise(),0, R<<vio::Config::ACC_Noise(),0,0,0,
0,0,vio::Config::GYO_Noise(); 0,vio::Config::ACC_Noise(),0,0,
Eigen::Matrix<double,3,3> G; 0,0,vio::Config::GYO_Noise(),0,
G<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))), 0,0,0,vio::Config::GYO_Noise();
0,1.0,-2.0*pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))), Eigen::Matrix<double,4,4> G;
0,0,1.0; G<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),0,
0,1.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; cov_h_=G*cov_*G.transpose()+R;
} }
void correct(double x, double z,double pitch){ void correct(double x, double z,double pitch){
Eigen::Matrix<double,3,3> H; Eigen::Matrix<double,3,4> H;
Eigen::Matrix<double,3,3> Q; Eigen::Matrix<double,3,3> Q;
Eigen::Matrix<double,3,1> E; Eigen::Matrix<double,3,1> E;
Eigen::Matrix<double,3,3> k; Eigen::Matrix<double,4,3> k;
H<<1.0,0,0, H<<1.0,0,0,0,
0,1.0,0, 0,1.0,0,0,
0,0,1.0; 0,0,1.0,0;
Q<<vio::Config::Svo_Ekf(),0,0, Q<<vio::Config::Svo_Ekf(),0,0,
0,vio::Config::Svo_Ekf(),0, 0,vio::Config::Svo_Ekf(),0,
0,0,vio::Config::Svo_Ekf(); 0,0,vio::Config::Svo_Ekf();
E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(2); E<<x-state_h_(0),z-state_h_(1),pitch-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(4,4)-k*H)*cov_h_;
} }
Eigen::Matrix<double,3,3> cov_; Eigen::Matrix<double,4,4> cov_;
Eigen::Matrix<double, 3, 1> state_; Eigen::Matrix<double, 4, 1> state_;
private: private:
double t_cmd=0.0; double t_cmd=0.0;
Eigen::Matrix<double,3,3> cov_h_; Eigen::Matrix<double,4,4> cov_h_;
Eigen::Matrix<double, 3, 1> state_h_;//state{x,y,pitch( rotation aound z)} in world frame Eigen::Matrix<double, 4, 1> state_h_;//state{x,y,pitch( rotation around z),dyaw} in world frame
double t_imu=0.0; double t_imu=0.0;
}; };
@@ -125,16 +134,24 @@ public:
boost::unique_lock<boost::mutex> lock(ekf_mut_); boost::unique_lock<boost::mutex> lock(ekf_mut_);
filter_->predict(x,y,theta,1e-9*time.toNSec()); filter_->predict(x,y,theta,1e-9*time.toNSec());
}; };
//Camera frame z back, x right, y down -> right hands (pitch counts from x)correct //Camera frame z forward, x right, y down -> right hands (pitch counts from x)correct
//Notice T_F_W is the position of the first frame with respect to the new frame while they are looking at one feature //Notice T_F_W is the position of the first frame with respect to the new frame while they are looking at one feature
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*/) { 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*/) {
boost::unique_lock<boost::mutex> lock(ekf_mut_); boost::unique_lock<boost::mutex> lock(ekf_mut_);
filter_->correct(x,z,-1.0*pitch); filter_->correct(x,z,-1.0*pitch);
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2)))); Eigen::Matrix<double,3,3> cov;
cov<<filter_->cov_(0,0),filter_->cov_(0,1),filter_->cov_(0,2),
filter_->cov_(1,0),filter_->cov_(1,1),filter_->cov_(1,2),
filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2);
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2))));
}; };
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){ std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
boost::unique_lock<boost::mutex> lock(ekf_mut_); boost::unique_lock<boost::mutex> lock(ekf_mut_);
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2)))); Eigen::Matrix<double,3,3> cov;
cov<<filter_->cov_(0,0),filter_->cov_(0,1),filter_->cov_(0,2),
filter_->cov_(1,0),filter_->cov_(1,1),filter_->cov_(1,2),
filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2);
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2))));
} }
bool start_=false; bool start_=false;
private: private:
GPU_version/vio/src/visualizer.cpp
+4 -4
View File
@@ -55,11 +55,11 @@ void Visualizer::publishMinimal(
//Rviz frame z up, x right, y forward -> right hands (yaw counts from y) //Rviz frame z up, x right, y forward -> right hands (yaw counts from y)
//Camera frame z front, x right, y down -> right hands (pitch counts from z) //Camera frame z front, x right, y down -> right hands (pitch counts from z)
//IMU frame y front, x right, z up -> left hands (theta counts from y) //IMU frame y front, x right, z up -> left hands (theta counts from y)
Quaterniond q(AngleAxisd(-odom.second.pitch()+M_PI_2, Vector3d::UnitZ())); Quaterniond q(AngleAxisd(-1.0*(odom.second.pitch()-M_PI_2), Vector3d::UnitZ()));
geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped); geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped);
msg_pose_with_cov->header = header_msg; msg_pose_with_cov->header = header_msg;
msg_pose_with_cov->pose.pose.position.x = -1.0*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 = -1.0*odom.second.se2().translation()(1); 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.position.z = 0.0;
msg_pose_with_cov->pose.pose.orientation.x = q.x(); msg_pose_with_cov->pose.pose.orientation.x = q.x();
msg_pose_with_cov->pose.pose.orientation.y = q.y(); msg_pose_with_cov->pose.pose.orientation.y = q.y();
@@ -74,7 +74,7 @@ void Visualizer::publishMinimal(
pub_pose_with_cov_.publish(msg_pose_with_cov); pub_pose_with_cov_.publish(msg_pose_with_cov);
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){ if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){
publishPointMarker( publishPointMarker(
pub_points_, Eigen::Vector3d(-1.0*odom.second.se2().translation()(0),-1.0*odom.second.se2().translation()(1),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)); ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5));
} }
} }