diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 68dc124..b555767 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -206,12 +206,6 @@ struct Feature { const StateIDType& cam_state_id, std::vector& return_i) const; - /* - * @brief AnchorPixelToPosition uses the calcualted pixels - * of the anchor patch to generate 3D positions of all of em - */ -inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p, - const CameraCalibration& cam); /* * @brief Irradiance returns irradiance value of a pixel @@ -821,20 +815,6 @@ Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCal //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); } - -Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam) -{ - // use undistorted position of point of interest - // project it back into 3D space using pinhole model - // save resulting NxN positions for this feature - - Eigen::Vector3d PositionInCamera(in_p.x/anchor_rho, in_p.y/anchor_rho, 1/anchor_rho); - Eigen::Vector3d PositionInWorld = T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation(); - return PositionInWorld; - //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); -} - - //@test center projection must always be initial feature projection bool Feature::initializeAnchor(const CameraCalibration& cam, int N) { diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 3d87a0d..cced538 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1231,9 +1231,11 @@ void MsckfVio::PhotometricMeasurementJacobian( // one line of the NxN Jacobians Matrix H_xi; + Matrix H_xA; Matrix H_fi; MatrixXd H_xl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 6); + MatrixXd H_xAl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 6); MatrixXd H_yl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 3); auto frame = cam0.moving_window.find(cam_state_id)->second.image; @@ -1301,34 +1303,12 @@ void MsckfVio::PhotometricMeasurementJacobian( H_xi = dI_dhj*dz_dpc0*dpc0_dxc; H_fi = dI_dhj*dz_dpc0*dpc0_dpg; - gradientVector.x += dx; - gradientVector.y += dy; - - residualVector.x += dx * photo_r[count]; - residualVector.y += dy * photo_r[count]; - res_sum += photo_r[count]; - - //dh / d{}^Cp_{ij} - 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)); - dCpij_dGpij = quaternionToRotation(cam_state.orientation); //orientation takes camera frame to world frame, we wa - dh_dGpij = dh_dCpij * dCpij_dGpij; + dh_dGpij = dz_dpc0 * dCpij_dGpij; - //dh / d X_{pl} - dCpij_dCGtheta = skewSymmetric(p_c0); - dCpij_dGpC = -quaternionToRotation(cam_state.orientation); - dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta; - dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC; - - //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)); + double rho = feature.rh dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), @@ -1336,8 +1316,11 @@ void MsckfVio::PhotometricMeasurementJacobian( 1/(rho))); dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); + H_xA = dI_dhj*dh_dGpij*dGpj_XpAj + H_xl.block<1, 6>(count, 0) = H_xi; H_yl.block<1, 3>(count, 0) = H_fi; + H_xAl.block<1, 6>(count, 0) = H_xA; count++; } @@ -1350,12 +1333,12 @@ void MsckfVio::PhotometricMeasurementJacobian( Eigen::VectorXd r_l = Eigen::VectorXd::Zero(count-1); for(int i = 0; i < r_l.size(); i++) - r_l(i) = z_irr[i]- z_irr_est[i]; + r_l(i) = z_irr[i]- feature.anchorPatch[i]; r = r_l; H_x = H_xl; - H_y = H_yl; + H_y = H_xAl; //TODO make this more fluent as well @@ -1438,32 +1421,21 @@ void MsckfVio::PhotometricFeatureJacobian( int cam_state_cntr = std::distance( state_server.cam_states.begin(), cam_state_iter); + auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); + int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); + + // Stack the Jacobians. + H_xi.block(stack_cntr, 21+cam_state_cntr_anchor*7, H_yl.rows(), H_yl.cols()) = -H_yi; H_xi.block(stack_cntr, 21+cam_state_cntr*7, 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, N*N) = r_l; stack_cntr += N*N; } - // Project the residual and Jacobians onto the nullspace - // 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-12) - sv_size++; - - MatrixXd A = svd_helper.matrixU().rightCols(jacobian_row_size - singularValues.size()); - */ - H_x = A_null_space.transpose() * H_xi; - r = A_null_space.transpose() * r_i; + H_x = H_xi; + r = r_i; ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");