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

constructed jacobian based on residual: r = z(measurement pix. pos) - h(rho, x_l, x_a) - based on the projection of the feature point into space based on the anchor frame - followed by projection onto H_rho nullspace; works well enough

Browse Source
This commit is contained in:
Raphael Maenle
2019-05-21 14:50:53 +02:00
parent e4dbe2f060
commit 5e9149eacc
3 changed files with 60 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/msckf_vio.cpp
View File

@ -1253,7 +1253,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
Matrix<double, 3, 3> dCpij_dGpC = Matrix<double, 3, 3>::Zero();
// one line of the NxN Jacobians
Eigen::Matrix<double, 2, 3> H_f;
Eigen::Matrix<double, 2, 1> H_rho;
Eigen::Matrix<double, 2, 6> H_plj;
Eigen::Matrix<double, 2, 6> H_pAj;
@ -1288,6 +1288,10 @@ void MsckfVio::PhotometricMeasurementJacobian(
//d{}^Gp_P{ij} / \rho_i
double rho = feature.anchor_rho;
// Isometry T_anchor_w takes a vector in anchor frame to world frame
dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho));
// alternative derivation towards feature
Matrix3d dCpc0_dpg = R_w_c0;
@ -1298,7 +1302,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
// Intermediate Jakobians
H_f = dh_dCpij * dCpc0_dpg; // 1 x 1
H_rho = dh_dGpij * dGpj_drhoj; // 1 x 1
H_plj = dh_dXplj; // 1 x 6
H_pAj = dh_dGpij * dGpj_XpAj; // 1 x 6
@ -1313,7 +1317,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
//calculate residual
cv::Point2f und_p_in_c0;
image_handler::undistortPoints(p_in_c0,
image_handler::undistortPoint(p_in_c0,
cam0.intrinsics,
cam0.distortion_model,
0,
@ -1322,8 +1326,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
r_i[0] = z[0] - und_p_in_c0.x;
r_i[1] = z[1] - und_p_in_c0.y;
cout << "r:\n" << r_i << endl;
MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7);
// set anchor Jakobi
@ -1342,7 +1344,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
H_xl.block(0, 21+cam_state_cntr*7, 2, 6) = H_plj;
H_x = H_xl;
H_y = H_f;
H_y = H_rho;
r = r_i;
cout << "h for patch done" << endl;
@ -1406,7 +1408,7 @@ void MsckfVio::PhotometricFeatureJacobian(
MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
21+state_server.cam_states.size()*7);
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 3);
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1);
VectorXd r_i = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;