search

KumarRobotics / msckf_vio

Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight
Other
1.71k stars 599 forks source link

About stateAugmentation() #67

Closed struggleforbetter closed 3 years ago

struggleforbetter commented 5 years ago

910037413 Matrix<double, 6, 21> J = Matrix<double, 6, 21>::Zero(); J.block<3, 3>(0, 0) = R_i_c; J.block<3, 3>(0, 15) = Matrix3d::Identity(); J.block<3, 3>(3, 0) = skewSymmetric(R_w_i.transpose()*t_c_i); //J.block<3, 3>(3, 0) = -R_w_i.transpose()*skewSymmetric(t_c_i); J.block<3, 3>(3, 12) = Matrix3d::Identity(); J.block<3, 3>(3, 18) = Matrix3d::Identity();

minxuanjun commented 5 years ago

@struggleforbetter you can test this code J.block<3, 3>(3, 0) = -R_w_i.transpose()*skewSymmetric(t_c_i); J.block<3, 3>(3, 12) = Matrix3d::Identity(); J.block<3, 3>(3, 18) = R_w_i.transpose();

struggleforbetter commented 5 years ago

@struggleforbetter you can test this code *J.block<3, 3>(3, 0) = -R_w_i.transpose()skewSymmetric(t_c_i);** J.block<3, 3>(3, 12) = Matrix3d::Identity(); J.block<3, 3>(3, 18) = R_w_i.transpose();

*J.block<3, 3>(3, 0) = R_w_i.transpose()skewSymmetric(t_c_i);** I think there is no minus