diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index f5ea7e8..4facacc 100644 --- a/config/camchain-imucam-tum-scaled.yaml +++ b/config/camchain-imucam-tum-scaled.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182] - distortion_model: pre-equidistant + distortion_model: equidistant intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] resolution: [512, 512] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] - distortion_model: pre-equidistant + distortion_model: equidistant intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] resolution: [512, 512] rostopic: /cam1/image_raw diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 17e0ce1..6c55861 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -208,7 +208,7 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, bool estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, std::vector& anchorPatch_estimate, IlluminationParameter& estimatedIllumination) const; @@ -549,7 +549,7 @@ return delta; bool Feature::estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, std::vector& anchorPatch_estimate, IlluminationParameter& estimated_illumination) const { @@ -558,11 +558,11 @@ bool Feature::estimate_FrameIrradiance( // muliply by a and add b of this frame auto anchor = observations.begin(); - if(cam0.moving_window.find(anchor->first) == cam0.moving_window.end()) + if(cam.moving_window.find(anchor->first) == cam.moving_window.end()) return false; - double anchorExposureTime_ms = cam0.moving_window.find(anchor->first)->second.exposureTime_ms; - double frameExposureTime_ms = cam0.moving_window.find(cam_state_id)->second.exposureTime_ms; + double anchorExposureTime_ms = cam.moving_window.find(anchor->first)->second.exposureTime_ms; + double frameExposureTime_ms = cam.moving_window.find(cam_state_id)->second.exposureTime_ms; double a_A = anchorExposureTime_ms; diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index 53dd99a..0e5985c 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -14,8 +14,8 @@ - - + + diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index b57704e..6237037 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -18,7 +18,7 @@ output="screen"> - + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index d5de69a..28ae944 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -25,7 +25,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index ef8f76e..172fda2 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -277,9 +277,9 @@ bool MsckfVio::createRosIO() { truth_sub = nh.subscribe("ground_truth", 100, &MsckfVio::truthCallback, this); - cam0_img_sub.subscribe(nh, "cam0_image", 10); - cam1_img_sub.subscribe(nh, "cam1_image", 10); - feature_sub.subscribe(nh, "features", 10); + cam0_img_sub.subscribe(nh, "cam0_image", 100); + cam1_img_sub.subscribe(nh, "cam1_image", 100); + feature_sub.subscribe(nh, "features", 100); image_sub.connectInput(cam0_img_sub, cam1_img_sub, feature_sub); image_sub.registerCallback(&MsckfVio::imageCallback, this); @@ -312,7 +312,7 @@ bool MsckfVio::initialize() { for (int i = 1; i < 1000; ++i) { boost::math::chi_squared chi_squared_dist(i); chi_squared_test_table[i] = - boost::math::quantile(chi_squared_dist, 0.4); + boost::math::quantile(chi_squared_dist, 0.05); } if (!createRosIO()) return false; @@ -703,7 +703,7 @@ bool MsckfVio::resetCallback( is_first_img = true; // Restart the subscribers. - imu_sub = nh.subscribe("imu", 100, + imu_sub = nh.subscribe("imu", 1000, &MsckfVio::imuCallback, this); truth_sub = nh.subscribe("ground_truth", 100, @@ -1563,25 +1563,28 @@ bool MsckfVio::PhotometricPatchPointResidual( const Feature& feature, VectorXd& r) { - VectorXd r_photo = VectorXd::Zero(2*N*N); int count = 0; const CAMState& cam_state = state_server.cam_states[cam_state_id]; //estimate photometric measurement std::vector estimate_irradiance; - std::vector estimate_photo_z; + IlluminationParameter estimated_illumination; + std::vector estimate_photo_z_c0, estimate_photo_z_c1; 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);// * + estimate_photo_z_c0.push_back (estimate_irradiance_j);// * + //estimated_illumination.frame_gain * estimated_illumination.feature_gain + + //estimated_illumination.frame_bias + estimated_illumination.feature_bias); + feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam1, estimate_irradiance, estimated_illumination); + for (auto& estimate_irradiance_j : estimate_irradiance) + estimate_photo_z_c1.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; @@ -1615,9 +1618,11 @@ bool MsckfVio::PhotometricPatchPointResidual( // 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]; - + //r_photo(count*2) = photo_z_c0[count] - estimate_photo_z_c0[count]; + //r_photo(count*2+1) = photo_z_c1[count] - estimate_photo_z_c1[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]; @@ -1747,13 +1752,12 @@ bool MsckfVio::PhotometricMeasurementJacobian( const FeatureIDType& feature_id, MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) { - // Prepare all the required data. const CAMState& cam_state = state_server.cam_states[cam_state_id]; const Feature& feature = map_server[feature_id]; //photometric observation - VectorXd r_photo = VectorXd::Zero(N*N); + VectorXd r_photo; // one line of the NxN Jacobians Eigen::Matrix H_rhoj; @@ -1771,7 +1775,6 @@ bool MsckfVio::PhotometricMeasurementJacobian( if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) return false; - //cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0; @@ -1796,13 +1799,13 @@ bool MsckfVio::PhotometricMeasurementJacobian( count++; } - cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl; - //Eigen::Matrix h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo; - //cout << "h photo: \n" << h_photo << endl; + // cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl; + // 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(2*N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(2*N*N, 1); + MatrixXd H_xl; + MatrixXd H_yl; ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); @@ -1844,6 +1847,9 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, Eigen::MatrixXd& H_xl, Eigen::MatrixXd& H_yl) { + H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7); + H_yl = MatrixXd::Zero(2*N*N, 1); + // 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); @@ -1867,9 +1873,9 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, /*for(int i = 0; i< N*N; i++) H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; - */ + */ - H_yl = H_rho; + H_yl.block(0, 0, H_rho.rows(), H_rho.cols()) = H_rho; } @@ -1894,23 +1900,35 @@ bool MsckfVio::PhotometricFeatureJacobian( } int jacobian_row_size = 0; - jacobian_row_size = 2 * N * N * valid_cam_state_ids.size(); + + // stacked feature jacobians + MatrixXd H_xi; + MatrixXd H_yi; + VectorXd r_i; - MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, - 21+state_server.cam_states.size()*7); - MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); // CHANGED N*N+1 to 1 - VectorXd r_i = VectorXd::Zero(jacobian_row_size); + // temporary measurement jacobians + MatrixXd H_xl; + MatrixXd H_yl; + Eigen::VectorXd r_l; int stack_cntr = 0; + bool first = true; + // go through every valid state the feature was observed in for (const auto& cam_id : valid_cam_state_ids) { - MatrixXd H_xl; - MatrixXd H_yl; - Eigen::VectorXd r_l = VectorXd::Zero(2*N*N); - + // skip observation if measurement is not valid if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) continue; + // set size of stacking jacobians, once the returned jacobians are known + if(first) + { + first = not first; + jacobian_row_size = r_l.size() * valid_cam_state_ids.size(); + H_xi = MatrixXd::Zero(jacobian_row_size, H_xl.cols()); + H_yi = MatrixXd::Zero(jacobian_row_size, H_yl.cols()); // CHANGED N*N+1 to 1 + r_i = VectorXd::Zero(jacobian_row_size); + } 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); @@ -1918,12 +1936,12 @@ bool MsckfVio::PhotometricFeatureJacobian( // 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, 2*N*N) = r_l; - stack_cntr += 2*N*N; + r_i.segment(stack_cntr, r_l.size()) = r_l; + stack_cntr += r_l.size(); } // if not enough to propper nullspace (in paper implementation) - if(stack_cntr < 2*N*N) + if(stack_cntr < r_l.size()) return false; // Project the residual and Jacobians onto the nullspace @@ -1936,6 +1954,7 @@ bool MsckfVio::PhotometricFeatureJacobian( if(H_yi(i,0) != 0) valid = true; + FullPivLU lu(H_yi.transpose()); MatrixXd A_null_space = lu.kernel(); @@ -2004,7 +2023,9 @@ bool MsckfVio::PhotometricFeatureJacobian( cout << "---------- LOGGED -------- " << endl; } - return true; + if(valid) + return true; + return false; } void MsckfVio::measurementJacobian( @@ -2435,10 +2456,7 @@ 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) - { - cout << "zero" << endl; - return; - } + return; // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -2572,12 +2590,6 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - // cout << "r" << r << endl; - // cout << "procov" << P1+P2 << endl; - if(filter == 1) - cout << "gate: " << dof << " " << gamma << " " << - chi_squared_test_table[dof] << endl; - if(gamma > 1000000) { cout << " logging " << endl; @@ -2608,6 +2620,9 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, return false; if (gamma < chi_squared_test_table[dof]) { // cout << "passed" << endl; + if(filter == 1) + cout << "gate: " << dof << " " << gamma << " " << + chi_squared_test_table[dof] << endl; return true; } else { // cout << "failed" << endl; @@ -2713,13 +2728,11 @@ void MsckfVio::removeLostFeatures() { if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) { if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) { - cout << "passed" << endl; 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(); } } - featureJacobian(feature.id, cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2735,7 +2748,6 @@ void MsckfVio::removeLostFeatures() { twostack_cntr += twoH_xj.rows(); } - // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. //if (stack_cntr > 1500) break; @@ -2745,11 +2757,8 @@ void MsckfVio::removeLostFeatures() { { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); - photometricMeasurementUpdate(pH_x, pr); } - - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2898,8 +2907,8 @@ 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(), 1)) { //, cam_state_ids.size()-1)) { - cout << "passed" << endl; 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(); @@ -2921,27 +2930,24 @@ 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); } - if(pstack_cntr) { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); - photometricMeasurementUpdate(pH_x, pr); } - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); - + // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); @@ -3081,17 +3087,14 @@ 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(), 1)) {// involved_cam_state_ids.size())) { - cout << "passed" << endl; 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(); } } - - 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; @@ -3104,11 +3107,11 @@ void MsckfVio::pruneCamStateBuffer() { twostack_cntr += twoH_xj.rows(); } + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); + } - - if(pstack_cntr) { @@ -3117,7 +3120,6 @@ void MsckfVio::pruneCamStateBuffer() { photometricMeasurementUpdate(pH_x, pr); } - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr);