From 2ee7c248c116d82b8466bc9374133df5d4a4ea74 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 14 May 2019 16:03:24 +0200 Subject: [PATCH] alterations at nullspaceing, jakobi changes --- launch/msckf_vio_tum.launch | 4 +-- src/msckf_vio.cpp | 55 +++++++++++++++++++------------------ 2 files changed, 30 insertions(+), 29 deletions(-) diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 38f93a2..c3a91d9 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -21,8 +21,8 @@ - - + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 2473996..fb9bdde 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1158,8 +1158,6 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) size_t old_rows = state_server.state_cov.rows(); size_t old_cols = state_server.state_cov.cols(); - MatrixXd temp_cov = state_server.state_cov; - // add 7 for camera state + irradiance bias eta = b_l state_server.state_cov.conservativeResizeLike(Eigen::MatrixXd::Zero(old_rows+7, old_cols+7)); @@ -1287,7 +1285,8 @@ void MsckfVio::PhotometricMeasurementJacobian( dI_dhj(0, 1) = dy; //dh / d{}^Cp_{ij} - dh_dCpij.block<2, 2>(0, 0) = Eigen::Matrix::Identity(); + dh_dCpij(0, 0) = 1 / p_c0(2); + dh_dCpij(1, 1) = 1 / p_c0(2); dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); @@ -1305,16 +1304,16 @@ 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)); + 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)); - dGpj_XpAj.block<3, 3>(0, 0) = - skewSymmetric(feature.T_anchor_w.linear() - * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), + 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), 1/(rho))); dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); // Intermediate Jakobians - H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 3 + H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1 H_plj = dI_dhj * dh_dXplj; // 1 x 6 H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 @@ -1335,6 +1334,7 @@ void MsckfVio::PhotometricMeasurementJacobian( IlluminationParameter estimated_illumination; feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); + // calculated here, because we need true 'estimate_irradiance' later for jacobi for (auto& estimate_irradiance_j : estimate_irradiance) estimate_photo_z.push_back (estimate_irradiance_j * estimated_illumination.frame_gain * estimated_illumination.feature_gain + @@ -1382,7 +1382,6 @@ void MsckfVio::PhotometricMeasurementJacobian( count = 0; for(auto data : photo_r) r[count++] = data; - std::stringstream ss; ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; if(PRINTIMAGES) @@ -1469,25 +1468,21 @@ void MsckfVio::PhotometricFeatureJacobian( // of H_yj. // get Nullspace + FullPivLU lu(H_yi.transpose()); + MatrixXd A_null_space = lu.kernel(); + /* JacobiSVD 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-9) + if(singularValues[i] > 1e-12) sv_size++; - int null_space_size = svd_helper.matrixU().cols() - sv_size; //TEST used instead of svd_helper.singularValues().size(); - MatrixXd A = svd_helper.matrixU().rightCols(null_space_size); - - H_x = A.transpose() * H_xi; - r = A.transpose() * r_i; - - ofstream myfile; - myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "nulls:\n" << A.transpose() * H_yi < 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-5) sv_size++; - - int null_space_size = svd_helper.matrixU().cols() - sv_size; - + cout << "sv size: " << sv_size << endl; + MatrixXd A = svd_helper.matrixU().rightCols( - jacobian_row_size - sv_size); - + jacobian_row_size - 3); + */ + FullPivLU lu(H_fj.transpose()); + MatrixXd A = lu.kernel(); + H_x = A.transpose() * H_xj; r = A.transpose() * r_j; + /* ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <