From 715ca6a6b46a273727c76f52a9ab71775cebadb7 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 19:22:08 +0200 Subject: [PATCH] added two filter, not working yet - compare with htest --- include/msckf_vio/msckf_vio.h | 14 +- launch/msckf_vio_euroc.launch | 5 +- launch/msckf_vio_tinytum.launch | 4 +- src/msckf_vio.cpp | 408 ++++++++++++++++++++++++++++++-- 4 files changed, 411 insertions(+), 20 deletions(-) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 9e9975d..49af3cd 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -207,6 +207,11 @@ class MsckfVio { Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + void twodotMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r); + void PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, @@ -214,6 +219,11 @@ class MsckfVio { Eigen::MatrixXd& H_y, Eigen::VectorXd& r); + void twodotFeatureJacobian( + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + void PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, @@ -222,6 +232,8 @@ class MsckfVio { void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); void measurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); + void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); + bool gatingTest(const Eigen::MatrixXd& H, const Eigen::VectorXd&r, const int& dof); void removeLostFeatures(); @@ -234,7 +246,7 @@ class MsckfVio { void onlineReset(); // Photometry flag - bool PHOTOMETRIC; + int FILTER; // debug flag bool STREAMPAUSE; diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index 5f6de1e..b818735 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -17,9 +17,8 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - - + + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 06d4c7f..28ae944 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -17,8 +17,8 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 10b9cd7..30ee2e2 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -67,7 +67,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh): bool MsckfVio::loadParameters() { //Photometry Flag - nh.param("PHOTOMETRIC", PHOTOMETRIC, false); + nh.param("FILTER", FILTER, 0); nh.param("PrintImages", PRINTIMAGES, false); nh.param("StreamPause", STREAMPAUSE, false); nh.param("GroundTruth", GROUNDTRUTH, false); @@ -405,8 +405,7 @@ void MsckfVio::imageCallback( const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) { - cout << "hello" << endl; - // stop playing bagfile if printing images + // stop playing bagfile if printing images if(STREAMPAUSE) nh.setParam("/play_bag", false); // Return if the gravity vector has not been set. @@ -455,7 +454,7 @@ void MsckfVio::imageCallback( double imu_processing_time = ( ros::Time::now()-start_time).toSec(); - //cout << "1" << endl; + cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); @@ -464,7 +463,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(); @@ -473,7 +472,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); @@ -481,20 +480,20 @@ 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 = ( ros::Time::now()-start_time).toSec(); - //cout << "6" << endl; + cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); publish(feature_msg->header.stamp); @@ -1243,6 +1242,223 @@ void MsckfVio::addFeatureObservations( return; } +void MsckfVio::twodotMeasurementJacobian( + const StateIDType& cam_state_id, + 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]; + + // Cam0 pose. + Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); + const Vector3d& t_c0_w = cam_state.position; + + //photometric observation + std::vector photo_z; + + // individual Jacobians + 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_rho; + Eigen::Matrix H_plj; + Eigen::Matrix H_pAj; + + auto frame = cam0.moving_window.find(cam_state_id)->second.image; + + int count = 0; + + auto point = feature.anchorPatch_3d[0]; + + Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); + // add jacobian + + //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)); + + dCpij_dGpij = quaternionToRotation(cam_state.orientation); + + //orientation takes camera frame to world frame + dh_dGpij = dh_dCpij * dCpij_dGpij; + + //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)); + + + + // alternative derivation towards feature + Matrix3d dCpc0_dpg = R_w_c0; + 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_rho = dh_dGpij * dGpj_drhoj; // 2 x 1 + H_plj = dh_dXplj; // 2 x 6 + H_pAj = dh_dGpij * dGpj_XpAj; // 2 x 6 + + // calculate residual + + //observation + const Vector4d& total_z = feature.observations.find(cam_state_id)->second; + const Vector2d z = Vector2d(total_z[0], total_z[1]); + + VectorXd r_i = VectorXd::Zero(2); + + //calculate residual + + r_i[0] = z[0] - p_c0(0)/p_c0(2); + r_i[1] = z[1] - p_c0(1)/p_c0(2); + + MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7); + + // 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, 2, 6) = H_pAj; + + // 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, 2, 6) = H_plj; + + H_x = H_xl; + H_y = H_rho; + r = r_i; + + //TODO make this more fluent as well + if(PRINTIMAGES) + { + 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); + } + + return; +} + +void MsckfVio::twodotFeatureJacobian( + const FeatureIDType& feature_id, + 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 + // id camera has actually seen this feature. + vector valid_cam_state_ids(0); + for (const auto& cam_id : cam_state_ids) { + if (feature.observations.find(cam_id) == + feature.observations.end()) continue; + + if (feature.observations.find(cam_id) == + feature.observations.begin()) continue; + valid_cam_state_ids.push_back(cam_id); + } + + int jacobian_row_size = 0; + jacobian_row_size = 2 * valid_cam_state_ids.size(); + + MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, + 21+state_server.cam_states.size()*7); + MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); + VectorXd r_i = VectorXd::Zero(jacobian_row_size); + int stack_cntr = 0; + + for (const auto& cam_id : valid_cam_state_ids) { + + MatrixXd H_xl; + MatrixXd H_yl; + Eigen::VectorXd r_l = VectorXd::Zero(2); + + twodotMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l); + 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, 2) = r_l; + stack_cntr += 2; + } + + // Project the residual and Jacobians onto the nullspace + // of H_yj. + + // get Nullspace + 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; + + if(PRINTIMAGES) + { + + ofstream myfile; + myfile.open("/home/raphael/dev/octave/log2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: Hx\n" + << "# type: matrix\n" + << "# rows: " << H_xi.rows() << "\n" + << "# columns: " << H_xi.cols() << "\n" + << H_xi << endl; + + myfile << "# name: Hy\n" + << "# type: matrix\n" + << "# rows: " << H_yi.rows() << "\n" + << "# columns: " << H_yi.cols() << "\n" + << H_yi << endl; + + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r_i.rows() << "\n" + << "# columns: " << 1 << "\n" + << r_i << endl; + + myfile.close(); + std::cout << "resume playback" << std::endl; + nh.setParam("/play_bag", true); + } + return; +} + + + void MsckfVio::PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, @@ -1431,6 +1647,7 @@ void MsckfVio::PhotometricMeasurementJacobian( return; } + void MsckfVio::PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, @@ -1737,7 +1954,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. - if(PHOTOMETRIC) return; + if(FILTER != 0) return; const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1793,6 +2010,108 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { return; } +void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { + + if (H.rows() == 0 || r.rows() == 0) + return; + // Decompose the final Jacobian matrix to reduce computational + // complexity as in Equation (28), (29). + MatrixXd H_thin; + VectorXd r_thin; + + // QR decomposition to make stuff faster + if (H.rows() > H.cols()) { + // Convert H to a sparse matrix. + SparseMatrix H_sparse = H.sparseView(); + + // Perform QR decompostion on H_sparse. + SPQR > spqr_helper; + spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); + spqr_helper.compute(H_sparse); + + MatrixXd H_temp; + VectorXd r_temp; + (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp); + (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp); + + H_thin = H_temp.topRows(21+state_server.cam_states.size()*7); + r_thin = r_temp.head(21+state_server.cam_states.size()*7); + + } else { + H_thin = H; + r_thin = r; + } + + + // Compute the Kalman gain. + const MatrixXd& P = state_server.state_cov; + MatrixXd S = H_thin*P*H_thin.transpose() + + Feature::observation_noise*MatrixXd::Identity( + H_thin.rows(), H_thin.rows()); + //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); + MatrixXd K_transpose = S.ldlt().solve(H_thin*P); + MatrixXd K = K_transpose.transpose(); + // Compute the error of the state. + + VectorXd delta_x = K * r; + cout << "two rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; + cout << "two update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + // Update the IMU state. + if (FILTER != 2) return; + + const VectorXd& delta_x_imu = delta_x.head<21>(); + + if (//delta_x_imu.segment<3>(0).norm() > 0.15 || + //delta_x_imu.segment<3>(3).norm() > 0.15 || + delta_x_imu.segment<3>(6).norm() > 0.5 || + //delta_x_imu.segment<3>(9).norm() > 0.5 || + delta_x_imu.segment<3>(12).norm() > 1.0) { + printf("delta velocity: %f\n", delta_x_imu.segment<3>(6).norm()); + printf("delta position: %f\n", delta_x_imu.segment<3>(12).norm()); + ROS_WARN("Update change is too large."); + //return; + } + + const Vector4d dq_imu = + smallAngleQuaternion(delta_x_imu.head<3>()); + state_server.imu_state.orientation = quaternionMultiplication( + dq_imu, state_server.imu_state.orientation); + state_server.imu_state.gyro_bias += delta_x_imu.segment<3>(3); + state_server.imu_state.velocity += delta_x_imu.segment<3>(6); + state_server.imu_state.acc_bias += delta_x_imu.segment<3>(9); + state_server.imu_state.position += delta_x_imu.segment<3>(12); + + const Vector4d dq_extrinsic = + smallAngleQuaternion(delta_x_imu.segment<3>(15)); + state_server.imu_state.R_imu_cam0 = quaternionToRotation( + dq_extrinsic) * state_server.imu_state.R_imu_cam0; + state_server.imu_state.t_cam0_imu += delta_x_imu.segment<3>(18); + + // Update the camera states. + auto cam_state_iter = state_server.cam_states.begin(); + for (int i = 0; i < state_server.cam_states.size(); + ++i, ++cam_state_iter) { + const VectorXd& delta_x_cam = delta_x.segment(21+i*7, 6); + const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>()); + cam_state_iter->second.orientation = quaternionMultiplication( + dq_cam, cam_state_iter->second.orientation); + cam_state_iter->second.position += delta_x_cam.tail<3>(); + cam_state_iter->second.illumination.frame_bias += delta_x(21+i*7+6); + } + + // Update state covariance. + MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin; + //state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() + + // K*K.transpose()*Feature::observation_noise; + state_server.state_cov = I_KH*state_server.state_cov; + + // Fix the covariance to be symmetric + MatrixXd state_cov_fixed = (state_server.state_cov + + state_server.state_cov.transpose()) / 2.0; + state_server.state_cov = state_cov_fixed; + return; +} + void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { @@ -1841,7 +2160,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. - if (not PHOTOMETRIC) return; + if (FILTER != 1) return; const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1925,6 +2244,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 twojacobian_row_size = 0; vector invalid_feature_ids(0); vector processed_feature_ids(0); @@ -1965,7 +2285,9 @@ void MsckfVio::removeLostFeatures() { } pjacobian_row_size += N*N*feature.observations.size(); + twojacobian_row_size += 2*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; + processed_feature_ids.push_back(feature.id); } @@ -1974,6 +2296,9 @@ void MsckfVio::removeLostFeatures() { // processed_feature_ids.size() << endl; //cout << "jacobian row #: " << jacobian_row_size << endl; + + cout << "sizing" << endl; + // Remove the features that do not have enough measurements. for (const auto& feature_id : invalid_feature_ids) map_server.erase(feature_id); @@ -1991,6 +2316,12 @@ void MsckfVio::removeLostFeatures() { VectorXd pr = VectorXd::Zero(pjacobian_row_size); int pstack_cntr = 0; + + MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, + 21+7*state_server.cam_states.size()); + VectorXd twor = VectorXd::Zero(twojacobian_row_size); + int twostack_cntr = 0; + // Process the features which lose track. for (const auto& feature_id : processed_feature_ids) { auto& feature = map_server[feature_id]; @@ -2003,10 +2334,18 @@ void MsckfVio::removeLostFeatures() { VectorXd r_j; MatrixXd pH_xj; VectorXd pr_j; + MatrixXd twoH_xj; + VectorXd twor_j; + + + cout << "measuring" << endl; PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j); - + twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); + + cout << "gating" << endl; + if (gatingTest(H_xj, r_j, r_j.size())) { //, 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; @@ -2017,20 +2356,29 @@ void MsckfVio::removeLostFeatures() { pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { + 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(); + } // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. //if (stack_cntr > 1500) break; } + cout << "resizing" << endl; H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); // Perform the measurement update step. measurementUpdate(H_x, r); photometricMeasurementUpdate(pH_x, pr); + twoMeasurementUpdate(twoH_x, twor); // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) @@ -2094,6 +2442,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Set the size of the Jacobian matrix. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int twojacobian_row_size = 0; //initialize all the features which are going to be removed @@ -2132,6 +2481,7 @@ void MsckfVio::pruneLastCamStateBuffer() pjacobian_row_size += N*N*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; + twojacobian_row_size += 2*feature.observations.size(); } @@ -2143,9 +2493,14 @@ void MsckfVio::pruneLastCamStateBuffer() 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); + MatrixXd twoH_xj; + VectorXd twor_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 twostack_cntr = 0; for (auto& item : map_server) { auto& feature = item.second; @@ -2162,6 +2517,7 @@ void MsckfVio::pruneLastCamStateBuffer() PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); 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, r_j.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; @@ -2174,6 +2530,11 @@ void MsckfVio::pruneLastCamStateBuffer() pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + 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(); + } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2184,12 +2545,16 @@ void MsckfVio::pruneLastCamStateBuffer() pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); + // Perform measurement update. + + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); // Perform measurement update. measurementUpdate(H_x, r); photometricMeasurementUpdate(pH_x, pr); - + twoMeasurementUpdate(twoH_x, twor); //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -2241,6 +2606,7 @@ void MsckfVio::pruneCamStateBuffer() { // Find the size of the Jacobian matrix. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int twojacobian_row_size = 0; for (auto& item : map_server) { auto& feature = item.second; @@ -2285,6 +2651,7 @@ void MsckfVio::pruneCamStateBuffer() { } } + twojacobian_row_size += 2*involved_cam_state_ids.size(); pjacobian_row_size += N*N*involved_cam_state_ids.size(); jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } @@ -2302,6 +2669,11 @@ void MsckfVio::pruneCamStateBuffer() { 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 twoH_xj; + VectorXd twor_j; + MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size()); + VectorXd twor = VectorXd::Zero(twojacobian_row_size); + int twostack_cntr = 0; for (auto& item : map_server) { auto& feature = item.second; // Check how many camera states to be removed are associated @@ -2317,6 +2689,7 @@ void MsckfVio::pruneCamStateBuffer() { PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); 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; @@ -2329,6 +2702,11 @@ void MsckfVio::pruneCamStateBuffer() { pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + 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(); + } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); @@ -2339,11 +2717,13 @@ void MsckfVio::pruneCamStateBuffer() { r.conservativeResize(stack_cntr); pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); // Perform measurement update. measurementUpdate(H_x, r); photometricMeasurementUpdate(pH_x, pr); - + twoMeasurementUpdate(twoH_x, twor); //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(),