raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added minor changes to nullspace

Browse Source
This commit is contained in:
Raphael Maenle 2019-04-25 13:44:21 +02:00
parent 6ba26d782d
commit de07296d31
1 changed files with 28 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
src/msckf_vio.cpp
View File

@ -345,6 +345,9 @@ void MsckfVio::imageCallback(
// Augment the state vector. // Augment the state vector.
start_time = ros::Time::now(); start_time = ros::Time::now();
if(PHOTOMETRIC)
PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
else
stateAugmentation(feature_msg->header.stamp.toSec()); stateAugmentation(feature_msg->header.stamp.toSec());
double state_augmentation_time = ( double state_augmentation_time = (
ros::Time::now()-start_time).toSec(); ros::Time::now()-start_time).toSec();
@ -1186,20 +1189,20 @@ void MsckfVio::PhotometricFeatureJacobian(
// get Nullspace // get Nullspace
FullPivLU<MatrixXd> lu(H_yi);
MatrixXd A_right = lu.kernel();
//FullPivLU<MatrixXd> lu2(A_right.transpose());
//MatrixXd A = lu2.kernel().transpose();
/*
JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV); JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
int sv_size = 0;
Eigen::VectorXd singularValues = svd_helper.singularValues();
for(int i = 0; i < singularValues.size(); i++)
if(singularValues[i] < 1e-3)
sv_size++;
int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size(); int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size();
MatrixXd A = svd_helper.matrixU().rightCols( MatrixXd A = svd_helper.matrixU().rightCols(
jacobian_row_size-null_space_size); jacobian_row_size-null_space_size);
*/
//H_x = A.transpose() * H_xi; H_x = A.transpose() * H_xi;
//r = A.transpose() * r_i; r = A.transpose() * r_i;
H_x = H_xi * A_right;
r = r_i * A_right;
cout << "r\n" << r << endl; cout << "r\n" << r << endl;
cout << "Hx\n" << H_x << endl; cout << "Hx\n" << H_x << endl;
@ -1262,17 +1265,6 @@ void MsckfVio::measurementJacobian(
H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc; H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc;
H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg; H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg;
// Modifty the measurement Jacobian to ensure
// observability constrain.
Matrix<double, 4, 6> A = H_x;
Matrix<double, 6, 1> u = Matrix<double, 6, 1>::Zero();
u.block<3, 1>(0, 0) = quaternionToRotation(
cam_state.orientation_null) * IMUState::gravity;
u.block<3, 1>(3, 0) = skewSymmetric(
p_w-cam_state.position_null) * IMUState::gravity;
H_x = A - A*u*(u.transpose()*u).inverse()*u.transpose();
H_f = -H_x.block<4, 3>(0, 3);
// Compute the residual. // Compute the residual.
r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2),
p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
@ -1327,12 +1319,24 @@ void MsckfVio::featureJacobian(
// Project the residual and Jacobians onto the nullspace // Project the residual and Jacobians onto the nullspace
// of H_fj. // of H_fj.
JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV); JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV);
int sv_size = 0;
Eigen::VectorXd singularValues = svd_helper.singularValues();
for(int i = 0; i < singularValues.size(); i++)
if(singularValues[i] < 1e-3)
sv_size++;
int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size();
//cout << "singular values: \n" << svd_helper.singularValues();
cout << "null_space: " << null_space_size << endl;
MatrixXd A = svd_helper.matrixU().rightCols( MatrixXd A = svd_helper.matrixU().rightCols(
jacobian_row_size - 3); jacobian_row_size - 3);
H_x = A.transpose() * H_xj; H_x = A.transpose() * H_xj;
r = A.transpose() * r_j; r = A.transpose() * r_j;
return; return;
} }