diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 2cf42fd..6e013ea 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -496,7 +496,7 @@ bool Feature::initializeAnchor( //initialize patch Size //TODO make N size a ros parameter - int N = 3; + int N = 13; int n = (int)(N-1)/2; auto anchor = observations.begin(); diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index fcc4290..536c8ca 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -162,6 +162,7 @@ class MsckfVio { // Measurement update void stateAugmentation(const double& time); + void PhotometricStateAugmentation(const double& time); void addFeatureObservations(const CameraMeasurementConstPtr& msg); // This function is used to compute the measurement Jacobian // for a single feature observed at a single camera frame. @@ -180,9 +181,9 @@ class MsckfVio { void PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, - Eigen::Matrix& H_x, - Eigen::Matrix& H_f, - Eigen::Vector4d& r); + Eigen::MatrixXd& H_x, + Eigen::MatrixXd& H_y, + Eigen::VectorXd& r); void PhotometricFeatureJacobian( const FeatureIDType& feature_id, @@ -207,7 +208,7 @@ class MsckfVio { StateServer state_server; // Maximum number of camera states int max_cam_state_size; - + // Features used MapServer map_server; @@ -224,6 +225,8 @@ class MsckfVio { CameraCalibration cam0; CameraCalibration cam1; + // covariance data + double irradiance_frame_bias; ros::Time image_save_time; diff --git a/launch/msckf_vio_debug_tum.launch b/launch/msckf_vio_debug_tum.launch new file mode 100644 index 0000000..7597f79 --- /dev/null +++ b/launch/msckf_vio_debug_tum.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 247e824..18a5f4b 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -51,6 +51,7 @@ + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index de5e217..e7a696a 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -117,6 +117,10 @@ bool MsckfVio::loadParameters() { nh.param("initial_covariance/extrinsic_translation_cov", extrinsic_translation_cov, 1e-4); + // Get the initial irradiance covariance + nh.param("initial_covariance/irradiance_frame_bias", + irradiance_frame_bias, 0.1); + // get camera information (used for back projection) nh.param("cam0/distortion_model", cam0.distortion_model, string("radtan")); @@ -336,7 +340,7 @@ void MsckfVio::imageCallback( // Augment the state vector. start_time = ros::Time::now(); - stateAugmentation(feature_msg->header.stamp.toSec()); + PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); double state_augmentation_time = ( ros::Time::now()-start_time).toSec(); @@ -511,6 +515,10 @@ bool MsckfVio::resetCallback( nh.param("initial_covariance/extrinsic_translation_cov", extrinsic_translation_cov, 1e-4); + // Reset the irradiance covariance + nh.param("initial_covariance/irradiance_frame_bias", + irradiance_frame_bias, 0.1); + state_server.state_cov = MatrixXd::Zero(21, 21); for (int i = 3; i < 6; ++i) state_server.state_cov(i, i) = gyro_bias_cov; @@ -820,6 +828,8 @@ void MsckfVio::stateAugmentation(const double& time) { // Resize the state covariance matrix. size_t old_rows = state_server.state_cov.rows(); size_t old_cols = state_server.state_cov.cols(); + + // add 7 for camera state + irradiance bias eta = b_l state_server.state_cov.conservativeResize(old_rows+6, old_cols+6); // Rename some matrix blocks for convenience. @@ -839,10 +849,78 @@ void MsckfVio::stateAugmentation(const double& time) { 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::PhotometricStateAugmentation(const double& time) { + + const Matrix3d& R_i_c = state_server.imu_state.R_imu_cam0; + const Vector3d& t_c_i = state_server.imu_state.t_cam0_imu; + + // Add a new camera state to the state server. + Matrix3d R_w_i = quaternionToRotation( + state_server.imu_state.orientation); + Matrix3d R_w_c = R_i_c * R_w_i; + Vector3d t_c_w = state_server.imu_state.position + + R_w_i.transpose()*t_c_i; + + state_server.cam_states[state_server.imu_state.id] = + CAMState(state_server.imu_state.id); + CAMState& cam_state = state_server.cam_states[ + state_server.imu_state.id]; + + cam_state.time = time; + cam_state.orientation = rotationToQuaternion(R_w_c); + cam_state.position = t_c_w; + + cam_state.orientation_null = cam_state.orientation; + cam_state.position_null = cam_state.position; + + + // Update the covariance matrix of the state. + // To simplify computation, the matrix J below is the nontrivial block + // in Equation (16) in "A Multi-State Constraint Kalman Filter for Vision + // -aided Inertial Navigation". + Matrix J = Matrix::Zero(); + J.block<3, 3>(0, 0) = R_i_c; + J.block<3, 3>(0, 15) = Matrix3d::Identity(); + J.block<3, 3>(3, 0) = skewSymmetric(R_w_i.transpose()*t_c_i); + //J.block<3, 3>(3, 0) = -R_w_i.transpose()*skewSymmetric(t_c_i); + J.block<3, 3>(3, 12) = Matrix3d::Identity(); + J.block<3, 3>(3, 18) = Matrix3d::Identity(); + + // Resize the state covariance matrix. + size_t old_rows = state_server.state_cov.rows(); + size_t old_cols = state_server.state_cov.cols(); + + // add 7 for camera state + irradiance bias eta = b_l + state_server.state_cov.conservativeResize(old_rows+7, old_cols+7); + + // Rename some matrix blocks for convenience. + const Matrix& P11 = + state_server.state_cov.block<21, 21>(0, 0); + const MatrixXd& P12 = + state_server.state_cov.block(0, 21, 21, old_cols-21); + + // Fill in the augmented state covariance. + state_server.state_cov.block(old_rows, 0, 6, old_cols) << J*P11, J*P12; + state_server.state_cov.block(0, old_cols, old_rows, 6) = + state_server.state_cov.block(old_rows, 0, 6, old_cols).transpose(); + state_server.state_cov.block<6, 6>(old_rows, old_cols) = + J * P11 * J.transpose(); + + // Add photometry P_eta and surrounding Zeros + state_server.state_cov.block<1, 12>(old_rows+6, 0) = Matrix::Zero(); + state_server.state_cov.block<12, 1>(0, old_cols+6) = Matrix::Zero(); + state_server.state_cov(old_rows+6, old_cols+6) = irradiance_frame_bias; + // 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::addFeatureObservations( const CameraMeasurementConstPtr& msg) { @@ -879,7 +957,7 @@ void MsckfVio::addFeatureObservations( void MsckfVio::PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, - Matrix& H_x, Matrix& H_f, Vector4d& r) { + MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) { // Prepare all the required data. const CAMState& cam_state = state_server.cam_states[cam_state_id]; @@ -890,7 +968,7 @@ void MsckfVio::PhotometricMeasurementJacobian( const Vector3d& t_c0_w = cam_state.position; //temp N - const int N = 3; + const int N = 13; // Cam1 pose. Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); @@ -940,7 +1018,7 @@ void MsckfVio::PhotometricMeasurementJacobian( dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame)- feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame); dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame)- feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame); dI_dhj(0, 0) = dx; - dI_dhj(1, 0) = dy; + dI_dhj(0, 1) = dy; //dh / d{}^Cp_{ij} dh_dCpij.block<2, 2>(0, 0) = Eigen::Matrix::Identity(); @@ -949,15 +1027,15 @@ void MsckfVio::PhotometricMeasurementJacobian( dh_dGpij = dh_dCpij * quaternionToRotation(cam_state.orientation).transpose(); //dh / d X_{pl} - dh_dXplj.block<2, 3>(3, 0) = dh_dCpij * skewSymmetric(point); - dh_dXplj.block<2, 3>(3, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose(); + dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * skewSymmetric(point); + dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose(); //d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; dGpi_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)); - dGpi_XpAj.block<3, 3>(3, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho))); - dGpi_XpAj.block<3, 3>(3, 3) = Matrix::Identity(); + dGpi_XpAj.block<3, 3>(0, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho))); + dGpi_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); // Intermediate Jakobians H_rhoj = dI_dhj * dh_dGpij * dGpi_drhoj; // 1 x 3 @@ -973,9 +1051,6 @@ void MsckfVio::PhotometricMeasurementJacobian( // calculate residual - // visu -residual - //printf("-----\n"); - //observation const Vector4d& z = feature.observations.find(cam_state_id)->second; @@ -985,7 +1060,7 @@ void MsckfVio::PhotometricMeasurementJacobian( 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.push_back (estimate_irradiance_j * estimated_illumination.frame_gain * estimated_illumination.feature_gain + estimated_illumination.frame_bias + estimated_illumination.feature_bias); @@ -995,16 +1070,6 @@ void MsckfVio::PhotometricMeasurementJacobian( for(int i = 0; i < photo_z.size(); i++) photo_r.push_back(photo_z[i] - estimate_photo_z[i]); - // visu- residual - //for(int i = 0; i < photo_z.size(); i++) - // printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]); - - //Final Jakobians - // cout << "------------------------" << endl; - // cout << "rho" << H_rho.rows() << "x" << H_rho.cols() << "\n" << H_rho << endl; - // cout << "l" << H_pl.rows() << "x" << H_pl.cols() << "\n" << H_pl << endl; - // cout << "A" << H_pA.rows() << "x" << H_pA.cols() << "\n" << H_pA << endl; - 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); @@ -1033,46 +1098,14 @@ void MsckfVio::PhotometricMeasurementJacobian( H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho; - // Original calculation + H_x = H_xl; + H_y = H_yl; - // Convert the feature position from the world frame to - // the cam0 and cam1 frame. - Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w); - Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w); + //TODO make this more fluent as well + count = 0; + for(auto data : photo_r) + r[count++] = data; - // Compute the residual. - 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)); - - -// Compute the Jacobians. - Matrix dz_dpc0 = Matrix::Zero(); - dz_dpc0(0, 0) = 1 / p_c0(2); - dz_dpc0(1, 1) = 1 / p_c0(2); - dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2)); - dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2)); - - Matrix dz_dpc1 = Matrix::Zero(); - dz_dpc1(2, 0) = 1 / p_c1(2); - dz_dpc1(3, 1) = 1 / p_c1(2); - dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2)); - dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2)); - - Matrix dpc0_dxc = Matrix::Zero(); - dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); - dpc0_dxc.rightCols(3) = -R_w_c0; - - Matrix dpc1_dxc = Matrix::Zero(); - dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0); - dpc1_dxc.rightCols(3) = -R_w_c1; - - Matrix3d dpc0_dpg = R_w_c0; - Matrix3d dpc1_dpg = R_w_c1; - - H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc; - H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg; - - photo_z.clear(); return; } @@ -1083,6 +1116,7 @@ void MsckfVio::PhotometricFeatureJacobian( const auto& feature = map_server[feature_id]; + int N = 13; // Check how many camera states in the provided camera // id camera has actually seen this feature. vector valid_cam_state_ids(0); @@ -1094,36 +1128,38 @@ void MsckfVio::PhotometricFeatureJacobian( } int jacobian_row_size = 0; - jacobian_row_size = 4 * valid_cam_state_ids.size(); + jacobian_row_size = N * N * valid_cam_state_ids.size(); - MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size, - 21+state_server.cam_states.size()*6); - MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3); - VectorXd r_j = VectorXd::Zero(jacobian_row_size); + 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+state_server.cam_states.size()+1); + VectorXd r_i = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; - // visu - residual - printf("_____FEATURE:_____\n"); + //printf("_____FEATURE:_____\n"); // visu - feature //cam0.featureVisu.release(); for (const auto& cam_id : valid_cam_state_ids) { - Matrix H_xi = Matrix::Zero(); - Matrix H_fi = Matrix::Zero(); - Vector4d r_i = Vector4d::Zero(); - PhotometricMeasurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i); + //Matrix H_xi = Matrix::Zero(); + //Matrix H_fi = Matrix::Zero(); + MatrixXd H_xl; + MatrixXd H_yl; + Eigen::VectorXd r_l = VectorXd::Zero(N*N); + + PhotometricMeasurementJacobian(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_xj.block<4, 6>(stack_cntr, 21+6*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; + 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, N*N) = r_l; + stack_cntr += N*N; } // visu - feature /* @@ -1141,13 +1177,14 @@ void MsckfVio::PhotometricFeatureJacobian( */ // Project the residual and Jacobians onto the nullspace - // of H_fj. - JacobiSVD svd_helper(H_fj, ComputeFullU | ComputeThinV); + // of H_yj. + + JacobiSVD svd_helper(H_yi, ComputeFullU | ComputeThinV); MatrixXd A = svd_helper.matrixU().rightCols( jacobian_row_size - 3); - H_x = A.transpose() * H_xj; - r = A.transpose() * r_j; + H_x = A.transpose() * H_xi; + r = A.transpose() * r_i; return; } @@ -1290,6 +1327,7 @@ void MsckfVio::measurementUpdate( // complexity as in Equation (28), (29). MatrixXd H_thin; VectorXd r_thin; + cout << " measurement update ..." << endl; if (H.rows() > H.cols()) { // Convert H to a sparse matrix. @@ -1480,12 +1518,18 @@ void MsckfVio::removeLostFeatures() { MatrixXd H_xj; VectorXd r_j; PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j); - if (gatingTest(H_xj, r_j, 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(); + cout << "made gating test" << endl; } + else + { + cout << "failed gating test" << endl; + } + + cout << " stacked features up" << endl; // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. @@ -1628,18 +1672,27 @@ void MsckfVio::pruneCamStateBuffer() { MatrixXd H_xj; VectorXd r_j; - + + cout << "getting featureJacobian..."; PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + cout << "done" << endl; + if (gatingTest(H_xj, r_j, 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(); + cout << "made gating test" << endl; + } + else + { + cout << "failed gating test" << endl; } - for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } + cout << " stacked features up" << endl; + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -1728,6 +1781,10 @@ void MsckfVio::onlineReset() { nh.param("initial_covariance/extrinsic_translation_cov", extrinsic_translation_cov, 1e-4); + // Reset the irradiance covariance + nh.param("initial_covariance/irradiance_frame_bias", + irradiance_frame_bias, 0.1); + state_server.state_cov = MatrixXd::Zero(21, 21); for (int i = 3; i < 6; ++i) state_server.state_cov(i, i) = gyro_bias_cov;