diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml new file mode 100644 index 0000000..529828c --- /dev/null +++ b/config/camchain-imucam-tum-scaled.yaml @@ -0,0 +1,36 @@ +cam0: + T_cam_imu: + [-0.9995378259923383, 0.02917807204183088, -0.008530798463872679, 0.047094247958417004, + 0.007526588843243184, -0.03435493139706542, -0.9993813532126198, -0.04788273017221637, + -0.029453096117288798, -0.9989836729399656, 0.034119442089241274, -0.0697294754693238, + 0.0, 0.0, 0.0, 1.0] + camera_model: pinhole + distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756, + -0.001662284667857643] + distortion_model: equidistant + intrinsics: [95.2026071784, 95.2029854486, 127.573663262, 128.582615763] + resolution: [256, 256] + rostopic: /cam0/image_raw +cam1: + T_cam_imu: + [-0.9995240747493029, 0.02986739485347808, -0.007717688852024281, -0.05374086123613335, + 0.008095979457928231, 0.01256553460985914, -0.9998882749870535, -0.04648588412432889, + -0.02976708103202316, -0.9994748851595197, -0.0128013601698453, -0.07333210787623645, + 0.0, 0.0, 0.0, 1.0] + T_cn_cnm1: + [0.9999994317488622, -0.0008361847221513937, -0.0006612844045898121, -0.10092123225528335, + 0.0008042457277382264, 0.9988989443471681, -0.04690684567228134, -0.001964540595211977, + 0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572, + 0.0, 0.0, 0.0, 1.0] + camera_model: pinhole + distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326, + -0.002347858896562788] + distortion_model: equidistant + intrinsics: [94.8217471066, 94.8164593555, 126.391667581, 127.571024044] + resolution: [256, 256] + rostopic: /cam1/image_raw +T_imu_body: + [1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000] \ No newline at end of file diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index ad20d49..9e9975d 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -219,6 +219,7 @@ class MsckfVio { const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); void measurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); bool gatingTest(const Eigen::MatrixXd& H, @@ -252,6 +253,7 @@ class MsckfVio { // State vector StateServer state_server; + StateServer photometric_state_server; // Ground truth state vector StateServer true_state_server; diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch new file mode 100644 index 0000000..6fe2855 --- /dev/null +++ b/launch/image_processor_tinytum.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch new file mode 100644 index 0000000..12fcaee --- /dev/null +++ b/launch/msckf_vio_tinytum.launch @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index ebf0946..390ab03 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,14 +18,14 @@ output="screen"> - + - - + + - + @@ -72,6 +72,6 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 330bd54..323e791 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -451,13 +451,8 @@ void MsckfVio::imageCallback( //cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); - if(PHOTOMETRIC) - { - truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); - PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); - } - else - stateAugmentation(feature_msg->header.stamp.toSec()); + //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); + PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); double state_augmentation_time = ( ros::Time::now()-start_time).toSec(); @@ -488,7 +483,7 @@ void MsckfVio::imageCallback( //cout << "5" << endl; start_time = ros::Time::now(); - pruneCamStateBuffer(); + pruneLastCamStateBuffer(); double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); @@ -1306,10 +1301,6 @@ void MsckfVio::PhotometricMeasurementJacobian( cv::Point2f residualVector(0,0); double res_sum = 0; - - ofstream myfile; - myfile.open ("/home/raphael/dev/MSCKF_ws/log_jacobi.txt"); - for (auto point : feature.anchorPatch_3d) { //cout << "____feature-measurement_____\n" << endl; @@ -1324,9 +1315,6 @@ void MsckfVio::PhotometricMeasurementJacobian( r_photo(count) = photo_z[count] - estimate_photo_z[count]; //cout << "residual: " << photo_r.back() << endl; - // add jacobians - cv::Point2f pixelDistance = feature.pixelDistanceAt(anchor_state, anchor_state_id, cam0, point); - // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] @@ -1371,16 +1359,6 @@ void MsckfVio::PhotometricMeasurementJacobian( 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 - myfile << " --------- \n" << endl; - myfile << "H_rhoj\n" << H_rhoj << endl; - myfile << "H_plj\n" << H_plj << endl; - myfile << "H_pAj\n" << H_pAj << endl; - myfile << "\n" << endl; - myfile << "dI_dhj\n" << dI_dhj << endl; - myfile << "dh_dGpij\n" << dh_dGpij << endl; - myfile << "dGpj_XpAj\n" << dGpj_XpAj << endl; - - // myfile << "pixel pos change based on residual:\n" << dI_dhj.colPivHouseholderQr().solve(r_photo(count)) << endl; H_rho.block<1, 1>(count, 0) = H_rhoj; H_pl.block<1, 6>(count, 0) = H_plj; @@ -1389,8 +1367,6 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } - myfile.close(); - MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); @@ -1639,7 +1615,7 @@ void MsckfVio::featureJacobian( jacobian_row_size = 4 * valid_cam_state_ids.size(); MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size, - 21+state_server.cam_states.size()*6); + 21+state_server.cam_states.size()*7); MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3); VectorXd r_j = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; @@ -1656,7 +1632,7 @@ void MsckfVio::featureJacobian( state_server.cam_states.begin(), cam_state_iter); // Stack the Jacobians. - H_xj.block<4, 6>(stack_cntr, 21+6*cam_state_cntr) = H_xi; + H_xj.block<4, 6>(stack_cntr, 21+7*cam_state_cntr) = H_xi; H_fj.block<4, 3>(stack_cntr, 0) = H_fi; r_j.segment<4>(stack_cntr) = r_i; stack_cntr += 4; @@ -1706,10 +1682,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { // complexity as in Equation (28), (29). MatrixXd H_thin; VectorXd r_thin; - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; - + // QR decomposition to make stuff faster if (H.rows() > H.cols()) { // Convert H to a sparse matrix. @@ -1725,14 +1698,13 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { (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()*augmentationSize); - r_thin = r_temp.head(21+state_server.cam_states.size()*augmentationSize); + 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; @@ -1740,13 +1712,16 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { Feature::observation_noise*MatrixXd::Identity( H_thin.rows(), H_thin.rows()); //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); - cout << "inverting: " << S.rows() << ":" << S.cols() << endl; 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 << "update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; + cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. + + if(PHOTOMETRIC) return; + const VectorXd& delta_x_imu = delta_x.head<21>(); if (//delta_x_imu.segment<3>(0).norm() > 0.15 || @@ -1779,13 +1754,12 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { 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*augmentationSize, 6); + 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>(); - if(PHOTOMETRIC) - cam_state_iter->second.illumination.frame_bias += delta_x(21+i*augmentationSize+6); + cam_state_iter->second.illumination.frame_bias += delta_x(21+i*7+6); } // Update state covariance. @@ -1802,6 +1776,110 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { return; } + +void MsckfVio::photometricMeasurementUpdate(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 << "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; + + 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; +} + + bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { MatrixXd P1 = H * state_server.state_cov * H.transpose(); @@ -1811,8 +1889,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - cout << 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; @@ -1829,10 +1907,7 @@ void MsckfVio::removeLostFeatures() { // Remove the features that lost track. // BTW, find the size the final Jacobian matrix and residual vector. int jacobian_row_size = 0; - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; - + int pjacobian_row_size = 0; vector invalid_feature_ids(0); vector processed_feature_ids(0); @@ -1872,11 +1947,8 @@ void MsckfVio::removeLostFeatures() { } } - if(PHOTOMETRIC) - //just use max. size, as gets shrunken down after anyway - jacobian_row_size += N*N*feature.observations.size(); - else - jacobian_row_size += 4*feature.observations.size() - 3; + pjacobian_row_size += N*N*feature.observations.size(); + jacobian_row_size += 4*feature.observations.size() - 3; processed_feature_ids.push_back(feature.id); } @@ -1893,10 +1965,15 @@ void MsckfVio::removeLostFeatures() { if (processed_feature_ids.size() == 0) return; MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, - 21+augmentationSize*state_server.cam_states.size()); + 21+7*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; + 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; + // Process the features which lose track. for (const auto& feature_id : processed_feature_ids) { auto& feature = map_server[feature_id]; @@ -1907,30 +1984,36 @@ void MsckfVio::removeLostFeatures() { MatrixXd H_xj; VectorXd r_j; + MatrixXd pH_xj; + VectorXd pr_j; - if(PHOTOMETRIC) - PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j); - else - featureJacobian(feature.id, cam_state_ids, H_xj, r_j); - + PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); + featureJacobian(feature.id, cam_state_ids, H_xj, r_j); + 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; stack_cntr += H_xj.rows(); + } + 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(); } // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. - if (stack_cntr > 1500) break; + //if (stack_cntr > 1500) break; } - cout << "processed features: " << processed_feature_ids.size() << 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); + // Perform the measurement update step. measurementUpdate(H_x, r); + photometricMeasurementUpdate(pH_x, pr); // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) @@ -1988,14 +2071,13 @@ void MsckfVio::pruneLastCamStateBuffer() if (state_server.cam_states.size() < max_cam_state_size) return; - + auto rm_cam_state_id = state_server.cam_states.begin()->first; // Set the size of the Jacobian matrix. int jacobian_row_size = 0; - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; + int pjacobian_row_size = 0; + //initialize all the features which are going to be removed for (auto& item : map_server) { @@ -2031,20 +2113,23 @@ void MsckfVio::pruneLastCamStateBuffer() } } - if(PHOTOMETRIC) - //just use max. size, as gets shrunken down after anyway - jacobian_row_size += N*N*feature.observations.size(); - else - jacobian_row_size += 4*feature.observations.size() - 3; + pjacobian_row_size += N*N*feature.observations.size(); + jacobian_row_size += 4*feature.observations.size() - 3; } MatrixXd H_xj; VectorXd r_j; - MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size()); + MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+7*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); + 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 stack_cntr = 0; int pruned_cntr = 0; + int pstack_cntr = 0; + for (auto& item : map_server) { auto& feature = item.second; @@ -2058,38 +2143,41 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - if(PHOTOMETRIC) - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - else + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { + 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; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); pruned_cntr++; + } + 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(); } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } - if(pruned_cntr != 0) - { - cout << "pruned features: " << pruned_cntr << endl; - H_x.conservativeResize(stack_cntr, H_x.cols()); - r.conservativeResize(stack_cntr); - - // Perform measurement update. - measurementUpdate(H_x, r); - } + H_x.conservativeResize(stack_cntr, H_x.cols()); + r.conservativeResize(stack_cntr); + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + // Perform measurement update. + + + measurementUpdate(H_x, r); + photometricMeasurementUpdate(pH_x, pr); + //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); - int cam_state_start = 21 + augmentationSize*cam_sequence; - int cam_state_end = cam_state_start + augmentationSize; - + int cam_state_start = 21 + 7*cam_sequence; + int cam_state_end = cam_state_start + 7; // Remove the corresponding rows and columns in the state // covariance matrix. @@ -2109,12 +2197,13 @@ void MsckfVio::pruneLastCamStateBuffer() state_server.state_cov.cols()-cam_state_end); state_server.state_cov.conservativeResize( - state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); + state_server.state_cov.rows()-7, state_server.state_cov.cols()-7); } else { state_server.state_cov.conservativeResize( - state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); + state_server.state_cov.rows()-7, state_server.state_cov.cols()-7); } + // Remove this camera state in the state vector. state_server.cam_states.erase(rm_cam_state_id); cam0.moving_window.erase(rm_cam_state_id); @@ -2125,7 +2214,6 @@ void MsckfVio::pruneLastCamStateBuffer() void MsckfVio::pruneCamStateBuffer() { - if (state_server.cam_states.size() < max_cam_state_size) return; @@ -2135,9 +2223,7 @@ void MsckfVio::pruneCamStateBuffer() { // Find the size of the Jacobian matrix. int jacobian_row_size = 0; - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; + int pjacobian_row_size = 0; for (auto& item : map_server) { auto& feature = item.second; @@ -2181,10 +2267,9 @@ void MsckfVio::pruneCamStateBuffer() { continue; } } - if(PHOTOMETRIC) - jacobian_row_size += N*N*involved_cam_state_ids.size(); - else - jacobian_row_size += 4*involved_cam_state_ids.size() - 3; + + pjacobian_row_size += N*N*involved_cam_state_ids.size(); + jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } //cout << "jacobian row #: " << jacobian_row_size << endl; @@ -2192,9 +2277,14 @@ void MsckfVio::pruneCamStateBuffer() { // Compute the Jacobian and residual. MatrixXd H_xj; VectorXd r_j; - MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size()); + 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; for (auto& item : map_server) { auto& feature = item.second; // Check how many camera states to be removed are associated @@ -2207,33 +2297,42 @@ void MsckfVio::pruneCamStateBuffer() { } if (involved_cam_state_ids.size() == 0) continue; - - if(PHOTOMETRIC) - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - else - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - + + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_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; stack_cntr += H_xj.rows(); } + + 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(); + } + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); // Perform measurement update. measurementUpdate(H_x, r); + photometricMeasurementUpdate(pH_x, pr); //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_id)); - int cam_state_start = 21 + augmentationSize*cam_sequence; - int cam_state_end = cam_state_start + augmentationSize; + int cam_state_start = 21 + 7*cam_sequence; + int cam_state_end = cam_state_start + 7; // Remove the corresponding rows and columns in the state @@ -2254,10 +2353,10 @@ void MsckfVio::pruneCamStateBuffer() { state_server.state_cov.cols()-cam_state_end); state_server.state_cov.conservativeResize( - state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); + state_server.state_cov.rows()-7, state_server.state_cov.cols()-7); } else { state_server.state_cov.conservativeResize( - state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); + state_server.state_cov.rows()-7, state_server.state_cov.cols()-7); } // Remove this camera state in the state vector. diff --git a/src/shrinkImage.py b/src/shrinkImage.py new file mode 100755 index 0000000..89ca55d --- /dev/null +++ b/src/shrinkImage.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python +from __future__ import print_function + +import sys +import rospy +import cv2 +from std_msgs.msg import String +from sensor_msgs.msg import Image +from cv_bridge import CvBridge, CvBridgeError + +class image_converter: + + def __init__(self): + self.image0_pub = rospy.Publisher("/cam0/new_image_raw",Image, queue_size=10) + self.image1_pub = rospy.Publisher("/cam1/new_image_raw",Image, queue_size=10) + + self.bridge = CvBridge() + self.image0_sub = rospy.Subscriber("/cam0/image_raw",Image,self.callback_cam0) + self.image1_sub = rospy.Subscriber("/cam1/image_raw",Image,self.callback_cam1) + + def callback_cam0(self,data): + try: + cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") + except CvBridgeError as e: + print(e) + + imgScale = 0.25 + newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale + newimg = cv2.resize(cv_image,(int(newX),int(newY))) + + newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8") + newdata = data + newdata.height = newpub.height + newdata.width = newpub.width + newdata.step = newpub.step + newdata.data = newpub.data + try: + self.image0_pub.publish(newdata) + except CvBridgeError as e: + print(e) + + def callback_cam1(self,data): + try: + cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") + except CvBridgeError as e: + print(e) + + imgScale = 0.25 + newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale + newimg = cv2.resize(cv_image,(int(newX),int(newY))) + + newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8") + newdata = data + newdata.height = newpub.height + newdata.width = newpub.width + newdata.step = newpub.step + newdata.data = newpub.data + + try: + self.image1_pub.publish(newdata) + except CvBridgeError as e: + print(e) + + +def main(args): + ic = image_converter() + rospy.init_node('image_converter', anonymous=True) + try: + rospy.spin() + except KeyboardInterrupt: + print("Shutting down") + cv2.destroyAllWindows() + +if __name__ == '__main__': + main(sys.argv) \ No newline at end of file