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

removed error in photom. res. calculation

Browse Source
This commit is contained in:
Raphael Maenle 2019-05-21 14:23:49 +02:00
parent 6b208dbc44
commit e4dbe2f060
2 changed files with 25 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
launch/msckf_vio_tum.launch
View File

@ -18,7 +18,7 @@
output="screen">
<!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="false"/>
<param name="PHOTOMETRIC" value="true"/>
<!-- Debugging Flaggs -->
<param name="PrintImages" value="false"/>

41
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, 1> H_rhoj;
Eigen::Matrix<double, 2, 3> H_f;
Eigen::Matrix<double, 2, 6> H_plj;
Eigen::Matrix<double, 2, 6> H_pAj;
@ -1288,9 +1288,9 @@ 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;
dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho),
feature.anchorPatch_ideal[count].y/(rho),
@ -1298,7 +1298,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
// Intermediate Jakobians
H_rhoj = dh_dGpij * dGpj_drhoj; // 1 x 1
H_f = dh_dCpij * dCpc0_dpg; // 1 x 1
H_plj = dh_dXplj; // 1 x 6
H_pAj = dh_dGpij * dGpj_XpAj; // 1 x 6
@ -1311,8 +1311,18 @@ void MsckfVio::PhotometricMeasurementJacobian(
VectorXd r_i = VectorXd::Zero(2);
//calculate residual
r_i[0] = z[0] - p_in_c0.x;
r_i[1] = z[1] - p_in_c0.y;
cv::Point2f und_p_in_c0;
image_handler::undistortPoints(p_in_c0,
cam0.intrinsics,
cam0.distortion_model,
0,
und_p_in_c0);
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);
@ -1332,15 +1342,15 @@ void MsckfVio::PhotometricMeasurementJacobian(
H_xl.block(0, 21+cam_state_cntr*7, 2, 6) = H_plj;
H_x = H_xl;
H_y = H_rhoj;
H_y = H_f;
r = r_i;
cout << "h for patch done" << endl;
//TODO make this more fluent as well
std::stringstream ss;
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
if(PRINTIMAGES)
{
std::stringstream ss;
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
feature.MarkerGeneration(marker_pub, state_server.cam_states);
//feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss);
}
@ -1396,7 +1406,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, 1);
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 3);
VectorXd r_i = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
@ -1405,20 +1415,17 @@ void MsckfVio::PhotometricFeatureJacobian(
MatrixXd H_xl;
MatrixXd H_yl;
Eigen::VectorXd r_l = VectorXd::Zero(2);
cout << "getting jacobi" << endl;
PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l);
cout << "done" << endl;
auto cam_state_iter = state_server.cam_states.find(cam_id);
int cam_state_cntr = std::distance(
state_server.cam_states.begin(), cam_state_iter);
// Stack the Jacobians.
cout << "stacking" << endl;
H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl;
H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
r_i.segment(stack_cntr, 2) = r_l;
stack_cntr += 2;
cout << "done" << endl;
}
// Project the residual and Jacobians onto the nullspace
@ -1597,7 +1604,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
// complexity as in Equation (28), (29).
MatrixXd H_thin;
VectorXd r_thin;
/*
if (H.rows() > H.cols()) {
// Convert H to a sparse matrix.
SparseMatrix<double> H_sparse = H.sparseView();
@ -1621,10 +1628,10 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
//H_thin = Q1.transpose() * H;
//r_thin = Q1.transpose() * r;
} else {
} else {*/
H_thin = H;
r_thin = r;
}
//}
// Compute the Kalman gain.
const MatrixXd& P = state_server.state_cov;