diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index 880c70c..9eda6f5 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -41,6 +41,7 @@ struct CameraCalibration{ cv::Vec4d distortion_coeffs; movingWindow moving_window; cv::Mat featureVisu; + int id; }; diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index e30df3c..a2a3d63 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -507,6 +507,7 @@ double Feature::CompleteCvKernel( delta = ((double)cam.moving_window.find(cam_state_id)->second.dximage.at(pose.y, pose.x))/255.; else if (type == "Sobel_y") delta = ((double)cam.moving_window.find(cam_state_id)->second.dyimage.at(pose.y, pose.x))/255.; + return delta; } @@ -990,28 +991,39 @@ cv::Point2f Feature::projectPositionToCamera( cv::Point2f out_p; cv::Point2f my_p; // transfrom position to camera frame + + // cam0 position Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); const Eigen::Vector3d& t_c0_w = cam_state.position; - Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); - out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); - // if(cam_state_id == observations.begin()->first) - //printf("undist:\n \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y); - + // project point according to model + if(cam.id == 0) + { + Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); + out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); + + } + // if camera is one, calcualte the cam1 position from cam0 position first + else if(cam.id == 1) + { + // cam1 position + Eigen::Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); + Eigen::Matrix3d R_w_c1 = R_c0_c1 * R_w_c0; + Eigen::Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation(); + + Eigen::Vector3d p_c1 = R_w_c1 * (in_p-t_c1_w); + out_p = cv::Point2f(p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); + } + + // undistort point according to camera model if (cam.distortion_model.substr(0,3) == "pre-") my_p = cv::Point2f(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]); else my_p = image_handler::distortPoint(out_p, - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs); - - - // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); - // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2)); - // printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y); - + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs); return my_p; } diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 90a5750..06bad0f 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -233,10 +233,10 @@ class MsckfVio { const Feature& feature, Eigen::Vector3d point, int count, - Eigen::Matrix& H_rhoj, - Eigen::Matrix& H_plj, - Eigen::Matrix& H_pAj, - Eigen::Matrix& dI_dhj); + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj, + Eigen::Matrix& dI_dhj); bool PhotometricMeasurementJacobian( const StateIDType& cam_state_id, diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index d4fbc31..28ae944 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index af56991..5e3dd8c 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -184,6 +184,8 @@ bool MsckfVio::loadParameters() { cam1.distortion_coeffs[2] = cam1_distortion_coeffs_temp[2]; cam1.distortion_coeffs[3] = cam1_distortion_coeffs_temp[3]; + cam0.id = 0; + cam1.id = 1; state_server.state_cov = MatrixXd::Zero(21, 21); @@ -551,8 +553,8 @@ void MsckfVio::manageMovingWindow( cv::Mat xder; cv::Mat yder; cv::Mat deeper_frame; - cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); + cam0_img_ptr->image.convertTo(deeper_frame,CV_16S); cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); xder/=8.; @@ -561,6 +563,15 @@ void MsckfVio::manageMovingWindow( cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone(); + cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); + cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); + xder/=8.; + yder/=8.; + + cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); + cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone(); + //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) @@ -1540,51 +1551,63 @@ bool MsckfVio::PhotometricPatchPointResidual( VectorXd& r) { - VectorXd r_photo = VectorXd::Zero(N*N); + VectorXd r_photo = VectorXd::Zero(2*N*N); int count = 0; - auto frame = cam0.moving_window.find(cam_state_id)->second.image; - const CAMState& cam_state = state_server.cam_states[cam_state_id]; //estimate photometric measurement std::vector estimate_irradiance; std::vector estimate_photo_z; - std::vector photo_z; + std::vector photo_z_c0, photo_z_c1; // estimate irradiance based on anchor frame + /* IlluminationParameter estimated_illumination; feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); 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); + */ + // irradiance measurement around feature point in c0 and c1 + std::vector true_irradiance_c0, true_irradiance_c1; + cv::Point2f p_f_c0(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1)); + cv::Point2f p_f_c1(feature.observations.find(cam_state_id)->second(2), feature.observations.find(cam_state_id)->second(3)); - // irradiance measurement around feature point - std::vector true_irradiance; - cv::Point2f p_f(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1)); - p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs); - cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; - for(int i = 0; isecond.image; + cv::Mat current_image_c1 = cam1.moving_window.find(cam_state_id)->second.image; + for(int i = 0; i frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) + if(p_in_c0.x < 0 or p_in_c0.x > current_image_c0.cols-1 or p_in_c0.y < 0 or p_in_c0.y > current_image_c0.rows-1) + return false; + if(p_in_c1.x < 0 or p_in_c1.x > current_image_c1.cols-1 or p_in_c1.y < 0 or p_in_c1.y > current_image_c1.rows-1) return false; - // add observation - photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); + // add observation + photo_z_c0.push_back(feature.PixelIrradiance(p_in_c0, current_image_c0)); + photo_z_c1.push_back(feature.PixelIrradiance(p_in_c1, current_image_c1)); // calculate photom. residual acc. to paper // r_photo(count) = photo_z[count] - estimate_photo_z[count]; - // calculate alternate photom. residual - r_photo(count) = true_irradiance[count] - photo_z[count]; + // calculate photom. residual alternating between frames + r_photo(count*2) = true_irradiance_c0[count] - photo_z_c0[count]; + r_photo(count*2+1) = true_irradiance_c1[count] - photo_z_c1[count]; count++; } r = r_photo; @@ -1598,10 +1621,10 @@ bool MsckfVio::PhotometricPatchPointJacobian( const Feature& feature, Eigen::Vector3d point, int count, - Eigen::Matrix& H_rhoj, - Eigen::Matrix& H_plj, - Eigen::Matrix& H_pAj, - Matrix& dI_dhj) + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj, + Matrix& dI_dhj) { const StateIDType anchor_state_id = feature.observations.begin()->first; @@ -1618,67 +1641,82 @@ bool MsckfVio::PhotometricPatchPointJacobian( // individual Jacobians - /*Matrix */dI_dhj = Matrix::Zero(); - Matrix dh_dCpij = Matrix::Zero(); - Matrix dh_dGpij = Matrix::Zero(); - Matrix dh_dXplj = Matrix::Zero(); + /*Matrix */dI_dhj = Matrix::Zero(); + + Matrix dh_dC0pij = Matrix::Zero(); + Matrix dh_dC1pij = 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(); + Matrix dC0pij_dGpij = Matrix::Zero(); + Matrix dC1pij_dGpij = Matrix::Zero(); + Matrix dC0pij_dXplj = Matrix::Zero(); + Matrix dC1pij_dXplj = Matrix::Zero(); - double dx, dy; + // irradiance jacobian + double dx_c0, dy_c0, dx_c1, dy_c1; Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); + Eigen::Vector3d p_c1 = R_w_c1 * (point-t_c1_w); + cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); + cv::Point2f p_in_c1 = feature.projectPositionToCamera(cam_state, cam_state_id, cam1, point); + cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); - - auto frame = cam0.moving_window.find(cam_state_id)->second.image; - // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_x"); - dy = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_y"); - //cout << "dx: " << dx << " : " << feature.cvKernel(p_in_c0, "Sobel_x") << " : " << feature.Kernel(p_in_c0, frame, "Sobel_x") << endl; + dx_c0 = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_x"); + dy_c0 = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_y"); - dI_dhj(0, 0) = dx * cam0.intrinsics[0]; - dI_dhj(0, 1) = dy * cam0.intrinsics[1]; + dx_c1 = feature.CompleteCvKernel(p_in_c1, cam_state_id, cam1, "Sobel_x"); + dy_c1 = feature.CompleteCvKernel(p_in_c1, cam_state_id, cam1, "Sobel_y"); + + dI_dhj(0, 0) = dx_c0 * cam0.intrinsics[0]; + dI_dhj(0, 1) = dy_c0 * cam0.intrinsics[1]; + dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0]; + dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1]; + + cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; + + // add jacobian + + //dh / d{}^Cp_{ij} + dh_dC0pij(0, 0) = 1. / p_c0(2); + dh_dC0pij(1, 1) = 1. / p_c0(2); + dh_dC0pij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); + dh_dC0pij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); //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)); + dh_dC1pij(2, 0) = 1. / p_c1(2); + dh_dC1pij(3, 1) = 1. / p_c1(2); + dh_dC1pij(2, 2) = -(p_c1(0))/(p_c1(2)*p_c1(2)); + dh_dC1pij(3, 2) = -(p_c1(1))/(p_c1(2)*p_c1(2)); - dCpij_dGpij = quaternionToRotation(cam_state.orientation); + dC0pij_dGpij = R_w_c0; + dC1pij_dGpij = R_c0_c1 * R_w_c0; - //orientation takes camera frame to world frame, we wa - dh_dGpij = dh_dCpij * dCpij_dGpij; + dC0pij_dXplj.leftCols(3) = skewSymmetric(p_c0); + dC0pij_dXplj.rightCols(3) = -R_w_c0; - //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_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(N*N-1)/2].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), + * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho), + feature.anchorPatch_ideal[(N*N-1)/2].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_rhoj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_drhoj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_drhoj; // 4 x 1 + H_plj = dI_dhj * dh_dC0pij * dC0pij_dXplj + dI_dhj * dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6 + H_pAj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6 // check if point nullspaceable if (H_rhoj(0, 0) != 0) @@ -1701,26 +1739,25 @@ bool MsckfVio::PhotometricMeasurementJacobian( VectorXd r_photo = VectorXd::Zero(N*N); // one line of the NxN Jacobians - Eigen::Matrix H_rhoj; - Eigen::Matrix H_plj; - Eigen::Matrix H_pAj; + Eigen::Matrix H_rhoj; + Eigen::Matrix H_plj; + Eigen::Matrix H_pAj; - Eigen::MatrixXd dI_dh(N*N, 2); + Eigen::MatrixXd dI_dh(2* N*N, 4); // combined Jacobians - Eigen::MatrixXd H_rho(N*N, 1); - Eigen::MatrixXd H_pl(N*N, 6); - Eigen::MatrixXd H_pA(N*N, 6); - - auto frame = cam0.moving_window.find(cam_state_id)->second.image; + Eigen::MatrixXd H_rho(2 * N*N, 1); + Eigen::MatrixXd H_pl(2 * N*N, 6); + Eigen::MatrixXd H_pA(2 * N*N, 6); // calcualte residual of patch PhotometricPatchPointResidual(cam_state_id, feature, r_photo); + cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0; bool valid = false; - Matrix dI_dhj;// = Matrix::Zero(); + Matrix dI_dhj;// = Matrix::Zero(); for (auto point : feature.anchorPatch_3d) { // get jacobi of single point in patch @@ -1728,11 +1765,11 @@ bool MsckfVio::PhotometricMeasurementJacobian( valid = true; // stack point into entire jacobi - 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_rho.block<2, 1>(count*2, 0) = H_rhoj; + H_pl.block<2, 6>(count*2, 0) = H_plj; + H_pA.block<2, 6>(count*2, 0) = H_pAj; - dI_dh.block<1, 2>(count, 0) = dI_dhj; + dI_dh.block<2, 4>(count*2, 0) = dI_dhj; count++; } @@ -1741,8 +1778,8 @@ bool MsckfVio::PhotometricMeasurementJacobian( //cout << "h photo: \n" << h_photo << endl; // construct the jacobian structure needed for nullspacing - MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(N*N, 1); + MatrixXd H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7); + MatrixXd H_yl = MatrixXd::Zero(2*N*N, 1); ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); @@ -1788,7 +1825,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, 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); // set anchor Jakobi - H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = H_pA; + H_xl.block(0, 21+cam_state_cntr_anchor*7, 2*N*N, 6) = H_pA; //get position of current frame in cam states auto cam_state_iter = state_server.cam_states.find(cam_state_id); @@ -1796,7 +1833,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, 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; + H_xl.block(0, 21+cam_state_cntr*7, 2*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); @@ -1819,7 +1856,6 @@ bool MsckfVio::PhotometricFeatureJacobian( MatrixXd& H_x, VectorXd& r) { - return false; const auto& feature = map_server[feature_id]; // Check how many camera states in the provided camera @@ -1835,7 +1871,7 @@ bool MsckfVio::PhotometricFeatureJacobian( } int jacobian_row_size = 0; - jacobian_row_size = N * N * valid_cam_state_ids.size(); + jacobian_row_size = 2 * N * N * valid_cam_state_ids.size(); MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, 21+state_server.cam_states.size()*7); @@ -1847,7 +1883,7 @@ bool MsckfVio::PhotometricFeatureJacobian( MatrixXd H_xl; MatrixXd H_yl; - Eigen::VectorXd r_l = VectorXd::Zero(N*N); + Eigen::VectorXd r_l = VectorXd::Zero(2*N*N); if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) continue; @@ -1855,15 +1891,16 @@ bool MsckfVio::PhotometricFeatureJacobian( 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. 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, N*N) = r_l; - stack_cntr += N*N; + r_i.segment(stack_cntr, 2*N*N) = r_l; + stack_cntr += 2*N*N; } + // if not enough to propper nullspace (in paper implementation) - if(stack_cntr < N*N) + if(stack_cntr < 2*N*N) return false; // Project the residual and Jacobians onto the nullspace @@ -2263,10 +2300,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { if (H.rows() == 0 || r.rows() == 0) - { - cout << "zero" << endl; return; - } // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -2316,7 +2350,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // Update the IMU state. if (FILTER != 2) return; - cout << "two: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + //cout << "two: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); @@ -2377,8 +2411,11 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { - if (H.rows() == 0 || r.rows() == 0) + if (H.rows() == 0 || r.rows() == 0) + { + cout << "zero" << endl; return; + } // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -2423,7 +2460,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r VectorXd delta_x = K * r; // Update the IMU state. - // cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; if (FILTER != 1) return; @@ -2504,7 +2541,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - //return true; + return true; MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P2 = Feature::observation_noise * @@ -2571,7 +2608,7 @@ void MsckfVio::removeLostFeatures() { } } - pjacobian_row_size += N*N*feature.observations.size(); + pjacobian_row_size += 2*N*N*feature.observations.size(); twojacobian_row_size += 4*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; @@ -2640,7 +2677,6 @@ void MsckfVio::removeLostFeatures() { stack_cntr += H_xj.rows(); } if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { - cout << "passed" << endl; twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); @@ -2769,7 +2805,7 @@ void MsckfVio::pruneLastCamStateBuffer() } } - pjacobian_row_size += N*N*feature.observations.size(); + pjacobian_row_size += 2*N*N*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; twojacobian_row_size += 4*feature.observations.size(); @@ -2827,7 +2863,6 @@ void MsckfVio::pruneLastCamStateBuffer() } if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { - cout << "passed" << endl; twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); @@ -2954,7 +2989,7 @@ void MsckfVio::pruneCamStateBuffer() { } twojacobian_row_size += 4*involved_cam_state_ids.size(); - pjacobian_row_size += N*N*involved_cam_state_ids.size(); + pjacobian_row_size += 2*N*N*involved_cam_state_ids.size(); jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } @@ -3010,7 +3045,6 @@ void MsckfVio::pruneCamStateBuffer() { } if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { - cout << "passed" << endl; twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows();