diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index b818dc6..c45f6a0 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -184,6 +184,11 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, const CameraCalibration& cam, Eigen::Vector3d& in_p) const; +double CompleteCvKernel( + const cv::Point2f pose, + const cv::Mat& frame, + std::string type) const; + double cvKernel( const cv::Point2f pose, std::string type) const; @@ -488,6 +493,45 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const else return false; } +double Feature::CompleteCvKernel( + const cv::Point2f pose, + const cv::Mat& frame, + std::string type) const +{ + + double delta = 0; + + cv::Mat xder; + cv::Mat yder; + cv::Mat deeper_frame; + frame.convertTo(deeper_frame,CV_16S); + //TODO remove this? + + + cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); + + xder/=8.; + yder/=8.; + + /* + cv::Mat norm_abs_xderImage; + cv::Mat abs_xderImage2; + cv::convertScaleAbs(xder, abs_xderImage2); + + cv::normalize(abs_xderImage2, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1); + + cv::imshow("xder", norm_abs_xderImage); + cvWaitKey(0); + */ + + if(type == "Sobel_x") + delta = ((double)xder.at(pose.y, pose.x))/255.; + else if (type == "Sobel_y") + delta = ((double)yder.at(pose.y, pose.x))/255.; + return delta; +} + double Feature::cvKernel( const cv::Point2f pose, std::string type) const @@ -823,6 +867,7 @@ bool Feature::VisualizePatch( // residual grid projection, positive - red, negative - blue colored namer.str(std::string()); namer << "residual"; + std::cout << "-- photo_r -- \n" << photo_r << " -- " << std::endl; cv::putText(irradianceFrame, namer.str() , cvPoint(30+scale*N, scale*N/2-5), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA); diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 250dfac..90a5750 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -229,12 +229,14 @@ class MsckfVio { bool PhotometricPatchPointJacobian( const CAMState& cam_state, + const StateIDType& cam_state_id, const Feature& feature, Eigen::Vector3d point, int count, Eigen::Matrix& H_rhoj, Eigen::Matrix& H_plj, - Eigen::Matrix& H_pAj); + 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 89112e1..28ae944 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index a8432b2..b7e9365 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -25,7 +25,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 51a56b0..2ba011b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -496,8 +496,8 @@ void MsckfVio::imageCallback( processing_end_time - processing_start_time; if (processing_time > 1.0/frame_rate) { ++critical_time_cntr; - ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", - processing_time, critical_time_cntr); + //ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", + // processing_time, critical_time_cntr); //printf("IMU processing time: %f/%f\n", // imu_processing_time, imu_processing_time/processing_time); //printf("State augmentation time: %f/%f\n", @@ -1418,8 +1418,7 @@ void MsckfVio::twodotMeasurementJacobian( { 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); + feature.MarkerGeneration(marker_pub, state_server.cam_states); } return; @@ -1540,27 +1539,40 @@ bool MsckfVio::PhotometricPatchPointResidual( std::vector estimate_photo_z; std::vector photo_z; + // 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); + 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 + 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; i frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) return false; - //add observation + // add observation photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); - //calculate photom. residual - r_photo(count) = photo_z[count] - estimate_photo_z[count]; + // 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]; count++; } r = r_photo; @@ -1570,12 +1582,14 @@ bool MsckfVio::PhotometricPatchPointResidual( // generates the jacobian of one patch point regarding rho, anchor and current frame bool MsckfVio::PhotometricPatchPointJacobian( const CAMState& cam_state, + const StateIDType& cam_state_id, const Feature& feature, Eigen::Vector3d point, int count, Eigen::Matrix& H_rhoj, Eigen::Matrix& H_plj, - Eigen::Matrix& H_pAj) + Eigen::Matrix& H_pAj, + Matrix& dI_dhj) { const StateIDType anchor_state_id = feature.observations.begin()->first; @@ -1592,7 +1606,7 @@ bool MsckfVio::PhotometricPatchPointJacobian( // individual Jacobians - Matrix dI_dhj = Matrix::Zero(); + /*Matrix */dI_dhj = Matrix::Zero(); Matrix dh_dCpij = Matrix::Zero(); Matrix dh_dGpij = Matrix::Zero(); Matrix dh_dXplj = Matrix::Zero(); @@ -1606,13 +1620,17 @@ bool MsckfVio::PhotometricPatchPointJacobian( double dx, dy; 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); 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.cvKernel(p_in_anchor, "Sobel_x"); - dy = feature.cvKernel(p_in_anchor, "Sobel_y"); + dx = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_x"); + dy = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_y"); + //cout << "dx: " << dx << " : " << feature.cvKernel(p_in_c0, "Sobel_x") << " : " << feature.Kernel(p_in_c0, frame, "Sobel_x") << endl; dI_dhj(0, 0) = dx * cam0.intrinsics[0]; dI_dhj(0, 1) = dy * cam0.intrinsics[1]; @@ -1650,7 +1668,11 @@ bool MsckfVio::PhotometricPatchPointJacobian( H_plj = dI_dhj * dh_dXplj; // 1 x 6 H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 - return true; + // check if point nullspaceable + if (H_rhoj(0, 0) != 0) + return true; + + return false; } bool MsckfVio::PhotometricMeasurementJacobian( @@ -1671,6 +1693,8 @@ bool MsckfVio::PhotometricMeasurementJacobian( Eigen::Matrix H_plj; Eigen::Matrix H_pAj; + Eigen::MatrixXd dI_dh(N*N, 2); + // combined Jacobians Eigen::MatrixXd H_rho(N*N, 1); Eigen::MatrixXd H_pl(N*N, 6); @@ -1683,26 +1707,33 @@ bool MsckfVio::PhotometricMeasurementJacobian( // calculate jacobian for patch int count = 0; + bool valid = false; + Matrix dI_dhj;// = Matrix::Zero(); for (auto point : feature.anchorPatch_3d) { // get jacobi of single point in patch - PhotometricPatchPointJacobian(cam_state, feature, point, count, H_rhoj, H_plj, H_pAj); - + if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj)) + 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; + dI_dh.block<1, 2>(count, 0) = dI_dhj; + count++; } + //Eigen::Matrix h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo; + //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, N*N+1); - + MatrixXd H_yl = MatrixXd::Zero(N*N, 1); + ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); - // set to return values H_x = H_xl; H_y = H_yl; @@ -1726,9 +1757,13 @@ bool MsckfVio::PhotometricMeasurementJacobian( //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } - return true; + if(valid) + return true; + else + return false; } +// uses the calcualted jacobians to construct the final Hx Hy jacobians used for nullspacing bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, Eigen::MatrixXd& H_pl, Eigen::MatrixXd& H_pA, @@ -1741,7 +1776,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, 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); @@ -1749,21 +1784,20 @@ 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, 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); + //H_yl.block(0, 0, N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N); /*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, N*N, 1) = -H_rho; - + H_yl = H_rho; } @@ -1772,7 +1806,6 @@ bool MsckfVio::PhotometricFeatureJacobian( const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) { - const auto& feature = map_server[feature_id]; // Check how many camera states in the provided camera @@ -1792,7 +1825,7 @@ bool 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, N*N+1); + MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); // CHANGED N*N+1 to 1 VectorXd r_i = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; @@ -1803,7 +1836,7 @@ bool MsckfVio::PhotometricFeatureJacobian( Eigen::VectorXd r_l = VectorXd::Zero(N*N); if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) - continue; + continue; auto cam_state_iter = state_server.cam_states.find(cam_id); int cam_state_cntr = std::distance( @@ -1815,19 +1848,25 @@ bool MsckfVio::PhotometricFeatureJacobian( r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; } - if(stack_cntr < 2*N*N) - return false; + // if not enough to propper nullspace (in paper implementation) + if(stack_cntr < N*N) + return false; // Project the residual and Jacobians onto the nullspace // of H_yj. // get Nullspace + + bool valid = false; + for(int i = 0; i < H_yi.rows(); i++) + if(H_yi(i,0) != 0) + valid = true; + FullPivLU lu(H_yi.transpose()); MatrixXd A_null_space = lu.kernel(); H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; - //cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; if(PRINTIMAGES) @@ -1956,6 +1995,7 @@ void MsckfVio::measurementJacobian( 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)); + // cout << "h reg: \n" << r(0) << "\n" << r(1) << endl; return; } @@ -2333,9 +2373,8 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r SPQR > spqr_helper; spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); - cout << "comp" << endl; spqr_helper.compute(H_sparse); - cout << "done" << endl; + MatrixXd H_temp; VectorXd r_temp; @@ -2364,6 +2403,9 @@ 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; + + if (FILTER != 1) return; if(PRINTIMAGES) @@ -2387,9 +2429,6 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r myfile.close(); } - - cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; - const VectorXd& delta_x_imu = delta_x.head<21>(); if (//delta_x_imu.segment<3>(0).norm() > 0.15 || @@ -2445,7 +2484,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - + return true; MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P2 = Feature::observation_noise * @@ -2453,8 +2492,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - //cout << "gate: " << dof << " " << gamma << " " << - //chi_squared_test_table[dof] << endl; + cout << "gate: " << dof << " " << gamma << " " << + chi_squared_test_table[dof] << endl; if (chi_squared_test_table[dof] == 0) return false; @@ -2562,7 +2601,7 @@ void MsckfVio::removeLostFeatures() { MatrixXd twoH_xj; VectorXd twor_j; - if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true) + if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) { if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; @@ -2747,7 +2786,7 @@ void MsckfVio::pruneLastCamStateBuffer() if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { - if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); @@ -2769,6 +2808,7 @@ void MsckfVio::pruneLastCamStateBuffer() twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2928,7 +2968,7 @@ void MsckfVio::pruneCamStateBuffer() { if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { - if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { + if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); @@ -2937,7 +2977,7 @@ void MsckfVio::pruneCamStateBuffer() { featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); - + if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// 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; @@ -2949,7 +2989,7 @@ void MsckfVio::pruneCamStateBuffer() { twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); }