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

correct frame name in functions

Browse Source
This commit is contained in:
gfuse
2021-08-28 17:29:05 +02:00
parent 670f33401e
commit 029fdc8651
3 changed files with 43 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/global.h
+8 -7
View File
@@ -80,19 +80,20 @@ namespace vio
-sin(pitch),cos(pitch);
T2_=SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
}
SE2_5(double x,double z,double pitch){
SE2_5(double y,double z,double roll){
Eigen::Matrix<double, 2,2> R;
R<<cos(pitch),sin(pitch),
-sin(pitch),cos(pitch);
T2_=SE2(R,Vector2d(x,z));
R<<cos(roll),sin(roll),
-sin(roll),cos(roll);
T2_=SE2(R,Vector2d(y,z));
};
SE2 se2() const{
return T2_;
}
SE2 inverse() const{
double yaw=atan2(T2_.so2().unit_complex().imag(),T2_.so2().unit_complex().real());
return SE2(yaw+M_PI,-1.0*T2_.translation());
double roll=atan2(T2_.so2().unit_complex().imag(),T2_.so2().unit_complex().real());
return SE2(roll+M_PI,-1.0*T2_.translation());
}
// Rotation around x
double pitch()const{
return atan2(T2_.rotation_matrix()(0,1),T2_.rotation_matrix()(0,0));
}
@@ -102,7 +103,7 @@ namespace vio
R<<T2_.rotation_matrix()(0,0),0.0,T2_.rotation_matrix()(0,1),
0.0,1.0,0.0,
T2_.rotation_matrix()(1,0),0.0,T2_.rotation_matrix()(1,1);
return SE3(R,Vector3d(T2_.translation()(0),1e-19,T2_.translation()(1)));
return SE3(R,Vector3d(T2_.translation()(0),1e-19,T2_.translation()(1)));
}
~SE2_5(){}
GPU_version/vio/include/vio/ukf.h
+26 -26
View File
@@ -17,7 +17,7 @@ public:
};
virtual ~Base(){
};
void predict(double dx,double dz,double dpitch,double time){
void predict(double dx,double dy,double dpitch,double time){
double dt=time-t_cmd;
if(t_cmd==0.0){
t_cmd=time;
@@ -25,20 +25,20 @@ public:
}
t_cmd=time;
state_h_(2)=state_(2)+dt*dpitch;
state_h_(1)=state_(1)+dt*(dz*cos(state_(2))-dx*sin(state_(2)));
state_h_(0)=state_(0)+dt*(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,dt*(-dx*sin(state_(2))+dz*cos(state_(2))),
0,1.0,dt*(-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;
@@ -49,28 +49,28 @@ public:
Eigen::Matrix<double,3,3> Q;
Eigen::Matrix<double,3,1> E;
Eigen::Matrix<double,3,3> k;
H<<1.0,0,pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
0,1.0,-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)+pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)))-state_h_(0);
E(1)=state_(1)+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)+pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)));
state_h_(0)=state_(0)+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,pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
0,1.0,-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;
@@ -99,7 +99,7 @@ public:
private:
double t_cmd=0.0;
Eigen::Matrix<double,3,3> cov_h_;
Eigen::Matrix<double, 3, 1> state_h_;//state{x,z,pitch}
Eigen::Matrix<double, 3, 1> state_h_;//state{x,y,pitch( rotation aound z)} in world frame
double t_imu=0.0;
};
@@ -111,29 +111,29 @@ 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(-z,-x,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_(1),-filter_->state_(0),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;
GPU_version/vio/kernel/compute-residual.cl
+9 -19
View File
@@ -15,25 +15,15 @@ float2 world2cam(float3 feature)
float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
{
ref.x*=-1.0;
ref.y*=-1.0;
ref.z = 3.141592653589793238462643383279502884197169399375 +ref.z;
float3 error=ref+cur;
float yaw=0.0;
float pitch=error.z;
float roll=15.0;
float R00=cos(yaw)*cos(pitch);//cos(pitch)
float R01=cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll);//0
float R02=cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll);//sin(pitch)
float R10=sin(yaw)*cos(pitch);//0
float R11=sin(yaw)*sin(pitch)*sin(roll)+cos(yaw)*cos(roll);//1
float R12=sin(yaw)*sin(pitch)*cos(roll)-cos(yaw)*sin(roll);//0
float R20=-sin(pitch);//-sin(pitch)
float R21=cos(pitch)*sin(roll);//0
float R22=cos(pitch)*cos(roll);//cos(pitch)
return (float3)(R00*ref_feature.x+R01*ref_feature.y+R02*ref_feature.z+error.x,
R10*ref_feature.x+R11*ref_feature.y+R12*ref_feature.z,
R20*ref_feature.x+R21*ref_feature.y+R22*ref_feature.z+error.y);
if(ref.z < 0.0)
ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z;
else
ref.z = ref.z - 3.1415926535897932384626433832795028841971693993;
float cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z);
float sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z);
return (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z),
ref_feature.y,
-1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));
}
void jacobian_xyz2uv(float3 xyz_in_f, float* J)