From 30daf37202d2ffaf422dff09b0196db56fe2ef16 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 11 Jul 2019 13:57:49 +0200 Subject: [PATCH] added structure to compare two patch sizes --- config/camchain-imucam-tum-scaled.yaml | 4 +- config/camchain-imucam-tum.yaml | 4 +- include/msckf_vio/msckf_vio.h | 13 +- launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 224 +++++++++++++++++++------ 5 files changed, 184 insertions(+), 63 deletions(-) diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index 4facacc..f5ea7e8 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: equidistant + distortion_model: pre-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: equidistant + distortion_model: pre-equidistant intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] resolution: [512, 512] rostopic: /cam1/image_raw diff --git a/config/camchain-imucam-tum.yaml b/config/camchain-imucam-tum.yaml index 7edbd87..141035e 100644 --- a/config/camchain-imucam-tum.yaml +++ b/config/camchain-imucam-tum.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756, -0.001662284667857643] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506] resolution: [1024, 1024] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326, -0.002347858896562788] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407] resolution: [1024, 1024] rostopic: /cam1/image_raw diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 3ffc586..c035798 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -220,12 +220,14 @@ class MsckfVio { const Feature& feature, const StateIDType& cam_state_id, Eigen::MatrixXd& H_xl, - Eigen::MatrixXd& H_yl); + Eigen::MatrixXd& H_yl, + int patchsize); bool PhotometricPatchPointResidual( const StateIDType& cam_state_id, const Feature& feature, - Eigen::VectorXd& r); + Eigen::VectorXd& r, + int patchsize); bool PhotometricPatchPointJacobian( const CAMState& cam_state, @@ -233,6 +235,7 @@ class MsckfVio { const Feature& feature, Eigen::Vector3d point, int count, + int patchsize, Eigen::Matrix& H_rhoj, Eigen::Matrix& H_plj, Eigen::Matrix& H_pAj, @@ -243,7 +246,8 @@ class MsckfVio { const FeatureIDType& feature_id, Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, - Eigen::VectorXd& r); + Eigen::VectorXd& r, + int patchsize); void twodotFeatureJacobian( const FeatureIDType& feature_id, @@ -253,7 +257,8 @@ class MsckfVio { bool PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, - Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + Eigen::MatrixXd& H_x, Eigen::VectorXd& r, + int patchsize); void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); void measurementUpdate(const Eigen::MatrixXd& H, diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 7b1b86a..b1d722c 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -33,7 +33,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 172fda2..1a44fe4 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -449,7 +449,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - // cout << "2" << endl; + cout << "2" << endl; // Add new observations for existing features or new // features in the map server. start_time = ros::Time::now(); @@ -458,7 +458,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - // cout << "3" << endl; + cout << "3" << endl; // Add new images to moving window start_time = ros::Time::now(); manageMovingWindow(cam0_img, cam1_img, feature_msg); @@ -466,14 +466,14 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - // cout << "4" << endl; + cout << "4" << endl; // Perform measurement update if necessary. start_time = ros::Time::now(); removeLostFeatures(); double remove_lost_features_time = ( ros::Time::now()-start_time).toSec(); - // cout << "5" << endl; + cout << "5" << endl; start_time = ros::Time::now(); pruneLastCamStateBuffer(); double prune_cam_states_time = ( @@ -483,7 +483,7 @@ void MsckfVio::imageCallback( batchTruthProcessing(feature_msg->header.stamp.toSec()); - // cout << "6" << endl; + cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); publish(feature_msg->header.stamp); @@ -1561,9 +1561,9 @@ void MsckfVio::twodotFeatureJacobian( bool MsckfVio::PhotometricPatchPointResidual( const StateIDType& cam_state_id, const Feature& feature, - VectorXd& r) + VectorXd& r, int patchsize) { - VectorXd r_photo = VectorXd::Zero(2*N*N); + VectorXd r_photo = VectorXd::Zero(2*patchsize*patchsize); int count = 0; const CAMState& cam_state = state_server.cam_states[cam_state_id]; @@ -1574,18 +1574,19 @@ bool MsckfVio::PhotometricPatchPointResidual( std::vector photo_z_c0, photo_z_c1; // estimate irradiance based on anchor frame - + feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); for (auto& estimate_irradiance_j : estimate_irradiance) 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; cv::Point2f p_f_c0(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1)); @@ -1596,15 +1597,32 @@ bool MsckfVio::PhotometricPatchPointResidual( cv::Mat current_image_c0 = cam0.moving_window.find(cam_state_id)->second.image; cv::Mat current_image_c1 = cam1.moving_window.find(cam_state_id)->second.image; - for(int i = 0; i new_anchorPatch_3d; + if (patchsize == 3 and N == 5) + { + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[6]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[7]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[8]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[11]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[12]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[13]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[16]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[17]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[18]); + } + else + { + new_anchorPatch_3d = feature.anchorPatch_3d; + } - // get residual - for(auto point : feature.anchorPatch_3d) + for(auto point : new_anchorPatch_3d) { cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); @@ -1629,6 +1647,8 @@ bool MsckfVio::PhotometricPatchPointResidual( count++; } r = r_photo; + + cout << "returning" << endl; return true; } @@ -1639,6 +1659,7 @@ bool MsckfVio::PhotometricPatchPointJacobian( const Feature& feature, Eigen::Vector3d point, int count, + int patchsize, Eigen::Matrix& H_rhoj, Eigen::Matrix& H_plj, Eigen::Matrix& H_pAj, @@ -1723,11 +1744,11 @@ bool MsckfVio::PhotometricPatchPointJacobian( // 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[(N*N-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(N*N-1)/2].y/(rho*rho), 1/(rho*rho)); + dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(patchsize*patchsize-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[(N*N-1)/2].x/(rho), - feature.anchorPatch_ideal[(N*N-1)/2].y/(rho), + * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].x/(rho), + feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].y/(rho), 1/(rho))); dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); @@ -1750,41 +1771,61 @@ bool MsckfVio::PhotometricPatchPointJacobian( bool MsckfVio::PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, - MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) + MatrixXd& H_x, MatrixXd& H_y, VectorXd& r, int patchsize) { // 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 r_photo = VectorXd::Zero(2*patchsize*patchsize); - // one line of the NxN Jacobians + // one line of the patchsizexpatchsize Jacobians Eigen::Matrix H_rhoj; Eigen::Matrix H_plj; Eigen::Matrix H_pAj; - Eigen::MatrixXd dI_dh(2* N*N, 4); + Eigen::MatrixXd dI_dh(2* patchsize*patchsize, 4); // combined Jacobians - Eigen::MatrixXd H_rho(2 * N*N, 1); - Eigen::MatrixXd H_pl(2 * N*N, 6); - Eigen::MatrixXd H_pA(2 * N*N, 6); + Eigen::MatrixXd H_rho(2 * patchsize*patchsize, 1); + Eigen::MatrixXd H_pl(2 * patchsize*patchsize, 6); + Eigen::MatrixXd H_pA(2 * patchsize*patchsize, 6); // calcualte residual of patch - if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) + if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo, patchsize)) + { return false; - + } //cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0; bool valid = false; Matrix dI_dhj;// = Matrix::Zero(); int valid_count = 0; - for (auto point : feature.anchorPatch_3d) + + std::vector new_anchorPatch_3d; + if (patchsize == 3 and N == 5) + { + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[6]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[7]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[8]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[11]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[12]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[13]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[16]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[17]); + new_anchorPatch_3d.push_back(feature.anchorPatch_3d[18]); + } + else + { + new_anchorPatch_3d = feature.anchorPatch_3d; + } + + for(auto point : new_anchorPatch_3d) { // get jacobi of single point in patch - if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj)) + if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, patchsize, H_rhoj, H_plj, H_pAj, dI_dhj)) { valid_count++; valid = true; @@ -1796,7 +1837,6 @@ bool MsckfVio::PhotometricMeasurementJacobian( H_pA.block<2, 6>(count*2, 0) = H_pAj; dI_dh.block<2, 4>(count*2, 0) = dI_dhj; - count++; } // cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl; @@ -1807,7 +1847,7 @@ bool MsckfVio::PhotometricMeasurementJacobian( MatrixXd H_xl; MatrixXd H_yl; - ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); + ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl, patchsize); // set to return values H_x = H_xl; @@ -1845,16 +1885,17 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, const Feature& feature, const StateIDType& cam_state_id, Eigen::MatrixXd& H_xl, - Eigen::MatrixXd& H_yl) + Eigen::MatrixXd& H_yl, + int patchsize) { - H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7); - H_yl = MatrixXd::Zero(2*N*N, 1); + H_xl = MatrixXd::Zero(2*patchsize*patchsize, 21+state_server.cam_states.size()*7); + H_yl = MatrixXd::Zero(2*patchsize*patchsize, 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); // set anchor Jakobi - H_xl.block(0, 21+cam_state_cntr_anchor*7, 2*N*N, 6) = H_pA; + H_xl.block(0, 21+cam_state_cntr_anchor*7, 2*patchsize*patchsize, 6) = H_pA; //get position of current frame in cam states auto cam_state_iter = state_server.cam_states.find(cam_state_id); @@ -1862,7 +1903,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, 2*N*N, 6) = H_pl; + H_xl.block(0, 21+cam_state_cntr*7, 2*patchsize*patchsize, 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); @@ -1882,7 +1923,8 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, bool MsckfVio::PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, - MatrixXd& H_x, VectorXd& r) + MatrixXd& H_x, VectorXd& r, + int patchsize) { const auto& feature = map_server[feature_id]; @@ -1917,7 +1959,7 @@ bool MsckfVio::PhotometricFeatureJacobian( for (const auto& cam_id : valid_cam_state_ids) { // skip observation if measurement is not valid - if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) + if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l, patchsize)) continue; // set size of stacking jacobians, once the returned jacobians are known @@ -1939,7 +1981,7 @@ bool MsckfVio::PhotometricFeatureJacobian( 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 < r_l.size()) return false; @@ -2023,6 +2065,9 @@ bool MsckfVio::PhotometricFeatureJacobian( cout << "---------- LOGGED -------- " << endl; } + cout << H_x.rows() << ":" << H_x.cols() << endl; + cout << r.rows() << ":" << r.cols() << endl; + if(valid) return true; return false; @@ -2590,6 +2635,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, double gamma = r.transpose() * (P1+P2).ldlt().solve(r); + /* if(gamma > 1000000) { cout << " logging " << endl; @@ -2615,14 +2661,16 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, << state_server.state_cov << endl; myfile.close(); } + */ + + if(filter == 1) + cout << "gate: " << dof << " " << gamma << " " << + chi_squared_test_table[dof] << endl; if (chi_squared_test_table[dof] == 0) 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; @@ -2635,6 +2683,7 @@ void MsckfVio::removeLostFeatures() { // BTW, find the size the final Jacobian matrix and residual vector. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int p2jacobian_row_size = 0; int twojacobian_row_size = 0; vector invalid_feature_ids(0); @@ -2676,6 +2725,7 @@ void MsckfVio::removeLostFeatures() { } pjacobian_row_size += 2*N*N*feature.observations.size(); + p2jacobian_row_size += 2*3*3*feature.observations.size(); twojacobian_row_size += 4*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; @@ -2704,6 +2754,10 @@ void MsckfVio::removeLostFeatures() { VectorXd pr = VectorXd::Zero(pjacobian_row_size); int pstack_cntr = 0; + MatrixXd p2H_x = MatrixXd::Zero(p2jacobian_row_size, + 21+7*state_server.cam_states.size()); + VectorXd p2r = VectorXd::Zero(p2jacobian_row_size); + int p2stack_cntr = 0; MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size()); @@ -2720,12 +2774,18 @@ void MsckfVio::removeLostFeatures() { MatrixXd H_xj; VectorXd r_j; - MatrixXd pH_xj; - VectorXd pr_j; + MatrixXd twoH_xj; VectorXd twor_j; - - if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) + + MatrixXd pH_xj; + VectorXd pr_j; + + MatrixXd p2H_xj; + VectorXd p2r_j; + + cout << "5: " << endl; + if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j, N)) { if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; @@ -2734,6 +2794,20 @@ void MsckfVio::removeLostFeatures() { } } + + cout << "3: " << endl; + + if(PhotometricFeatureJacobian(feature.id, cam_state_ids, p2H_xj, p2r_j, 3)) + { + if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) { + + cout << p2H_x.rows() << ":" << p2H_x.cols() << ", " << p2H_xj.rows() << ":" << p2H_xj.cols() << endl; + p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj; + p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j; + p2stack_cntr += p2H_xj.rows(); + } + } + featureJacobian(feature.id, cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2831,6 +2905,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Set the size of the Jacobian matrix. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int p2jacobian_row_size = 0; int twojacobian_row_size = 0; @@ -2869,6 +2944,7 @@ void MsckfVio::pruneLastCamStateBuffer() } pjacobian_row_size += 2*N*N*feature.observations.size(); + p2jacobian_row_size += 2*3*3*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; twojacobian_row_size += 4*feature.observations.size(); @@ -2884,11 +2960,16 @@ void MsckfVio::pruneLastCamStateBuffer() VectorXd pr = VectorXd::Zero(pjacobian_row_size); MatrixXd twoH_xj; VectorXd twor_j; + MatrixXd p2H_x = MatrixXd::Zero(p2jacobian_row_size, 21+7*state_server.cam_states.size()); + VectorXd p2r = VectorXd::Zero(p2jacobian_row_size); + MatrixXd p2H_xj; + VectorXd p2r_j; MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size()); VectorXd twor = VectorXd::Zero(twojacobian_row_size); int stack_cntr = 0; int pruned_cntr = 0; int pstack_cntr = 0; + int p2stack_cntr = 0; int twostack_cntr = 0; for (auto& item : map_server) { @@ -2904,17 +2985,28 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - - if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) + cout << "5: " << endl; + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j, N)) { - if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, 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(); } } + + /* + cout << "3: " << endl; + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3)) + { + if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) { + p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj; + p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j; + p2stack_cntr += p2H_xj.rows(); + } + } + */ featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -3003,6 +3095,7 @@ void MsckfVio::pruneCamStateBuffer() { // Find the size of the Jacobian matrix. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int p2jacobian_row_size = 0; int twojacobian_row_size = 0; for (auto& item : map_server) { @@ -3047,9 +3140,10 @@ void MsckfVio::pruneCamStateBuffer() { continue; } } - + twojacobian_row_size += 4*involved_cam_state_ids.size(); pjacobian_row_size += 2*N*N*involved_cam_state_ids.size(); + p2jacobian_row_size += 2*3*3*involved_cam_state_ids.size(); jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } @@ -3061,11 +3155,19 @@ void MsckfVio::pruneCamStateBuffer() { MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+7*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; + MatrixXd pH_xj; VectorXd pr_j; MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, 21+7*state_server.cam_states.size()); VectorXd pr = VectorXd::Zero(pjacobian_row_size); int pstack_cntr = 0; + + MatrixXd p2H_x = MatrixXd::Zero(p2jacobian_row_size, 21+7*state_server.cam_states.size()); + VectorXd p2r = VectorXd::Zero(p2jacobian_row_size); + MatrixXd p2H_xj; + VectorXd p2r_j; + int p2stack_cntr = 0; + MatrixXd twoH_xj; VectorXd twor_j; MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size()); @@ -3084,14 +3186,28 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; - if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) + cout << "5: " << endl; + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j, N)) { if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// 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(); + 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(); } } + + cout << "3: " << endl; + + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3)) + { + if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) { + p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj; + p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j; + p2stack_cntr += p2H_xj.rows(); + } + } + + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);