diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 8a6cc27..addadab 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -173,7 +173,7 @@ struct Feature { std::vector& anchorPatch_estimate, IlluminationParameter& estimatedIllumination) const; -bool MarkerGeneration( + bool MarkerGeneration( ros::Publisher& marker_pub, const CamStateServer& cam_states) const; @@ -183,6 +183,17 @@ bool MarkerGeneration( CameraCalibration& cam0, const std::vector photo_r, std::stringstream& ss) const; + + + /* @brief takes a pure pixel position (1m from image) + * converts to actual pixel value and returns patch irradiance + * around this pixel + */ + void PatchAroundPurePixel(cv::Point2f p, + int N, + const CameraCalibration& cam, + const StateIDType& cam_state_id, + std::vector& return_i) const; /* * @brief projectPixelToPosition uses the calcualted pixels * of the anchor patch to generate 3D positions of all of em @@ -665,6 +676,25 @@ bool Feature::VisualizePatch( cvWaitKey(0); } +void Feature::PatchAroundPurePixel(cv::Point2f p, + int N, + const CameraCalibration& cam, + const StateIDType& cam_state_id, + std::vector& return_i) const +{ + int n = (int)(N-1)/2; + cv::Mat image = cam.moving_window.find(cam_state_id)->second.image; + cv::Point2f img_p = image_handler::distortPoint(p, + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs); + + for(double u_run = -n; u_run <= n; u_run++) + for(double v_run = -n; v_run <= n; v_run++) + return_i.push_back(PixelIrradiance(cv::Point2f(img_p.x+u_run, img_p.y+v_run), image)); + +} + float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const { diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 31a891e..59c838f 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1229,163 +1229,74 @@ void MsckfVio::PhotometricMeasurementJacobian( Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); const Vector3d& t_c0_w = cam_state.position; - // Cam1 pose. - Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); - Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0; - Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation(); - - - //photometric observation - std::vector photo_z; - - // individual Jacobians - Matrix dI_dhj = Matrix::Zero(); - Matrix dh_dCpij = Matrix::Zero(); - Matrix dh_dGpij = Matrix::Zero(); - Matrix dh_dXplj = Matrix::Zero(); - Matrix dGpj_drhoj = Matrix::Zero(); - Matrix dGpj_XpAj = Matrix::Zero(); - - Matrix dCpij_dGpij = Matrix::Zero(); - Matrix dCpij_dCGtheta = Matrix::Zero(); - Matrix dCpij_dGpC = Matrix::Zero(); - // one line of the NxN Jacobians - Eigen::Matrix H_rhoj; - Eigen::Matrix H_plj; - Eigen::Matrix H_pAj; + Matrix H_xi; + Matrix H_fi; - // combined Jacobians - Eigen::MatrixXd H_rho(N*N, 1); - Eigen::MatrixXd H_pl(N*N, 6); - Eigen::MatrixXd H_pA(N*N, 6); + MatrixXd H_xl = 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; int count = 0; double dx, dy; + std::vector z_irr_est; + for (auto point : feature.anchorPatch_3d) { Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); - //add observation - photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); + z_irr_est.push_back(feature.PixelIrradiance(p_in_c0, frame)); - // add jacobian + Matrix dI_dhj = Matrix::Zero(); // frame derivative calculated convoluting with kernel [-1, 0, 1] dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame); dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame); dI_dhj(0, 0) = dx; dI_dhj(0, 1) = dy; + + // Compute the Jacobians. + Matrix dz_dpc0 = Matrix::Zero(); + dz_dpc0(0, 0) = 1 / p_c0(2); + dz_dpc0(1, 1) = 1 / p_c0(2); + dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2)); + dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2)); + + Matrix dpc0_dxc = Matrix::Zero(); - //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)); + // original jacobi + dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); + dpc0_dxc.rightCols(3) = -R_w_c0; - dCpij_dGpij = quaternionToRotation(cam_state.orientation); + Matrix3d dpc0_dpg = R_w_c0; - //orientation takes camera frame to world frame, we wa - dh_dGpij = dh_dCpij * dCpij_dGpij; + H_xi = dI_dhj*dz_dpc0*dpc0_dxc; + H_fi = dI_dhj*dz_dpc0*dpc0_dpg; - //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)); - - 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 1 - H_plj = dI_dhj * dh_dXplj; // 1 x 6 - H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 - - H_rho.block<1, 1>(count, 0) = H_rhoj; - H_pl.block<1, 6>(count, 0) = H_plj; - H_pA.block<1, 6>(count, 0) = H_pAj; + H_xl.block<1, 6>(count, 0) = H_xi; + H_yl.block<1, 3>(count, 0) = H_fi; count++; } - // calculate residual - //observation - const Vector4d& z = feature.observations.find(cam_state_id)->second; + // Compute the residual. + std::vector z_irr; + cv::Point2f z = cv::Point2f(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1)); + feature.PatchAroundPurePixel(z, N, cam0, cam_state_id, z_irr); - //estimate photometric measurement - std::vector estimate_irradiance; - std::vector estimate_photo_z; - 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 + - estimated_illumination.frame_bias + estimated_illumination.feature_bias); + Eigen::VectorXd r_l = Eigen::VectorXd::Zero(count-1); - std::vector photo_r; - - //calculate photom. residual - for(int i = 0; i < photo_z.size(); i++) - photo_r.push_back(photo_z[i] - estimate_photo_z[i]); + for(int i = 0; i < r_l.size(); i++) + r_l(i) = z_irr[i]- z_irr_est[i]; - MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); - - // set anchor Jakobi - // get position of anchor in cam states - - 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); - H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA; - - // set frame Jakobi - //get position of current frame in cam states - auto cam_state_iter = state_server.cam_states.find(cam_state_id); - int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); - - // set jakobi of state - H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl; - - // set ones for irradiance bias - H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); - - // set irradiance error Block - H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N); - - // TODO make this calculation more fluent - for(int i = 0; i< N*N; i++) - H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; - H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho; + r = r_l; H_x = H_xl; H_y = H_yl; - - //TODO make this more fluent as well - 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) - { - feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss); - } + // calculate residual return; } @@ -1455,7 +1366,7 @@ void MsckfVio::PhotometricFeatureJacobian( state_server.cam_states.begin(), cam_state_iter); // Stack the Jacobians. - H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl; + 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; @@ -1500,8 +1411,7 @@ void MsckfVio::PhotometricFeatureJacobian( void MsckfVio::measurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, - Matrix& H_x, Matrix& H_f, Vector4d& r) -{ + Matrix& H_x, Matrix& H_f, Vector4d& r) { // Prepare all the required data. const CAMState& cam_state = state_server.cam_states[cam_state_id]; @@ -1540,8 +1450,6 @@ void MsckfVio::measurementJacobian( dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2)); Matrix dpc0_dxc = Matrix::Zero(); - - // original jacobi dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); dpc0_dxc.rightCols(3) = -R_w_c0; @@ -1555,6 +1463,17 @@ void MsckfVio::measurementJacobian( H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc; H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg; + // Modifty the measurement Jacobian to ensure + // observability constrain. + Matrix A = H_x; + Matrix u = Matrix::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. 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)); @@ -1562,6 +1481,7 @@ void MsckfVio::measurementJacobian( return; } + void MsckfVio::featureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, @@ -1853,6 +1773,12 @@ void MsckfVio::removeLostFeatures() { else featureJacobian(feature.id, cam_state_ids, H_xj, r_j); + cout << "\n" << endl; + cout << "H_xj: \n" << H_xj << endl; + cout << "res: \n" << endl; + cout << r_j << endl; + + if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; @@ -2015,6 +1941,12 @@ void MsckfVio::pruneCamStateBuffer() { else featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + cout << "\n" << endl; + cout << "H_xj: \n" << H_xj << endl; + cout << "res: \n" << endl; + cout << r_j << endl; + + if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j;