diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index bcf5ee2..c0b2d24 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -195,9 +195,9 @@ class MsckfVio { // for a single feature observed at a single camera frame. void measurementJacobian(const StateIDType& cam_state_id, const FeatureIDType& feature_id, - Eigen::Matrix& H_x, - Eigen::Matrix& H_f, - Eigen::Vector4d& r); + Eigen::Matrix& H_x, + Eigen::Matrix& H_f, + Eigen::Vector2d& r); // This function computes the Jacobian of all measurements viewed // in the given camera states of this feature. void featureJacobian(const FeatureIDType& feature_id, diff --git a/launch/msckf_vio_debug_tum.launch b/launch/msckf_vio_debug_tum.launch index c4e12e7..b7013fd 100644 --- a/launch/msckf_vio_debug_tum.launch +++ b/launch/msckf_vio_debug_tum.launch @@ -22,7 +22,7 @@ - + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index c3a91d9..11f6ecc 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,13 +18,13 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index fb9bdde..03b1d22 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -454,7 +454,7 @@ void MsckfVio::imageCallback( PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); } else - stateAugmentation(feature_msg->header.stamp.toSec()); + PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); double state_augmentation_time = ( ros::Time::now()-start_time).toSec(); @@ -1175,7 +1175,7 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) J * P11 * J.transpose(); // Add photometry P_eta and surrounding Zeros - state_server.state_cov(old_rows+6, old_cols+6) = irradiance_frame_bias; + state_server.state_cov(old_rows+6, old_cols+6) = 0; // Fix the covariance to be symmetric MatrixXd state_cov_fixed = (state_server.state_cov + @@ -1242,7 +1242,6 @@ void MsckfVio::PhotometricMeasurementJacobian( std::vector photo_z; // individual Jacobians - Matrix dI_dhj = Matrix::Zero(); Matrix dh_dCpij = Matrix::Zero(); Matrix dh_dGpij = Matrix::Zero(); Matrix dh_dXplj = Matrix::Zero(); @@ -1254,107 +1253,75 @@ void MsckfVio::PhotometricMeasurementJacobian( Matrix dCpij_dGpC = Matrix::Zero(); // one line of the NxN Jacobians - Eigen::Matrix H_rhoj; - Eigen::Matrix H_plj; - Eigen::Matrix H_pAj; - - // combined Jacobians - Eigen::MatrixXd H_rho(N*N, 1); - Eigen::MatrixXd H_pl(N*N, 6); - Eigen::MatrixXd H_pA(N*N, 6); + Eigen::Matrix H_rhoj; + Eigen::Matrix H_plj; + Eigen::Matrix H_pAj; auto frame = cam0.moving_window.find(cam_state_id)->second.image; int count = 0; double dx, dy; - for (auto point : feature.anchorPatch_3d) - { - Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); - cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); + auto point = feature.anchorPatch_3d[0]; - //add observation - photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); + Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); + cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); - // add jacobian + // 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)); - // frame derivative calculated convoluting with kernel [-1, 0, 1] - 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(0, 1) = dy; - - //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); - dCpij_dGpij = quaternionToRotation(cam_state.orientation); + //orientation takes camera frame to world frame, we wa + dh_dGpij = dh_dCpij * dCpij_dGpij; - //orientation takes camera frame to world frame, we wa - 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; - //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)); - //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)); + 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(); - 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_rhoj = dh_dGpij * dGpj_drhoj; // 1 x 1 + H_plj = dh_dXplj; // 1 x 6 + H_pAj = dh_dGpij * dGpj_XpAj; // 1 x 6 - // Intermediate Jakobians - 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 - - H_rho.block<1, 1>(count, 0) = H_rhoj; - H_pl.block<1, 6>(count, 0) = H_plj; - H_pA.block<1, 6>(count, 0) = H_pAj; - - count++; - } // calculate residual //observation - const Vector4d& z = feature.observations.find(cam_state_id)->second; - - //estimate photometric measurement - std::vector estimate_irradiance; - std::vector estimate_photo_z; - IlluminationParameter estimated_illumination; - feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); + 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); - // calculated here, because we need true 'estimate_irradiance' later for jacobi - for (auto& estimate_irradiance_j : estimate_irradiance) - estimate_photo_z.push_back (estimate_irradiance_j * - estimated_illumination.frame_gain * estimated_illumination.feature_gain + - estimated_illumination.frame_bias + estimated_illumination.feature_bias); - - std::vector photo_r; - - //calculate photom. residual - for(int i = 0; i < photo_z.size(); i++) - photo_r.push_back(photo_z[i] - estimate_photo_z[i]); - - 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); + //calculate residual + r_i[0] = z[0] - p_in_c0.x; + r_i[1] = z[1] - p_in_c0.y; + 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, N*N, 6) = -H_pA; + 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 @@ -1362,32 +1329,20 @@ void MsckfVio::PhotometricMeasurementJacobian( 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, N*N, 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); - - // set irradiance error Block - H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N); - - // TODO make this calculation more fluent - for(int i = 0; i< N*N; i++) - 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; + H_xl.block(0, 21+cam_state_cntr*7, 2, 6) = H_plj; H_x = H_xl; - H_y = H_yl; + H_y = H_rhoj; + r = r_i; + cout << "h for patch done" << endl; //TODO make this more fluent as well - count = 0; - for(auto data : photo_r) - r[count++] = data; std::stringstream ss; ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss); + //feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss); } return; @@ -1437,11 +1392,11 @@ void MsckfVio::PhotometricFeatureJacobian( } int jacobian_row_size = 0; - jacobian_row_size = N * N * valid_cam_state_ids.size(); + 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, N*N+state_server.cam_states.size()+1); + MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); VectorXd r_i = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; @@ -1449,19 +1404,21 @@ void MsckfVio::PhotometricFeatureJacobian( MatrixXd H_xl; MatrixXd H_yl; - Eigen::VectorXd r_l = VectorXd::Zero(N*N); - + Eigen::VectorXd r_l = VectorXd::Zero(2); + cout << "getting jacobi" << endl; PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l); - + cout << "done" << endl; 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. + cout << "stacking" << endl; 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; + r_i.segment(stack_cntr, 2) = r_l; + stack_cntr += 2; + cout << "done" << endl; } // Project the residual and Jacobians onto the nullspace @@ -1495,7 +1452,7 @@ void MsckfVio::PhotometricFeatureJacobian( void MsckfVio::measurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, - Matrix& H_x, Matrix& H_f, Vector4d& r) + Matrix& H_x, Matrix& H_f, Vector2d& r) { // Prepare all the required data. @@ -1514,48 +1471,42 @@ void MsckfVio::measurementJacobian( // 3d feature position in the world frame. // And its observation with the stereo cameras. const Vector3d& p_w = feature.position; - const Vector4d& z = feature.observations.find(cam_state_id)->second; + const Vector2d& z = feature.observations.find(cam_state_id)->second.topRows(2); // 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); + //Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w); // Compute the Jacobians. - Matrix dz_dpc0 = Matrix::Zero(); + 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(); // original jacobi - //dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); - // my version of calculation dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); - //dpc0_dxc.leftCols(3) = - skewSymmetric(R_w_c0.transpose() * (t_c0_w - p_w)) * R_w_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; + H_x = dz_dpc0*dpc0_dxc; //+ dz_dpc1*dpc1_dxc; + H_f = dz_dpc0*dpc0_dpg; // + dz_dpc1*dpc1_dpg; // 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)); + r = z - Vector2d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));//, + //p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); return; } @@ -1579,19 +1530,19 @@ void MsckfVio::featureJacobian( } int jacobian_row_size = 0; - jacobian_row_size = 4 * valid_cam_state_ids.size(); + jacobian_row_size = 2 * 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; for (const auto& cam_id : valid_cam_state_ids) { - Matrix H_xi = Matrix::Zero(); - Matrix H_fi = Matrix::Zero(); - Vector4d r_i = Vector4d::Zero(); + Matrix H_xi = Matrix::Zero(); + Matrix H_fi = Matrix::Zero(); + Vector2d r_i = Vector2d::Zero(); measurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i); auto cam_state_iter = state_server.cam_states.find(cam_id); @@ -1599,10 +1550,10 @@ 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_fj.block<4, 3>(stack_cntr, 0) = H_fi; - r_j.segment<4>(stack_cntr) = r_i; - stack_cntr += 4; + H_xj.block<2, 6>(stack_cntr, 21+7*cam_state_cntr) = H_xi; + H_fj.block<2, 3>(stack_cntr, 0) = H_fi; + r_j.segment<2>(stack_cntr) = r_i; + stack_cntr += 2; } // Project the residual and Jacobians onto the nullspace @@ -1661,8 +1612,8 @@ 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()*6); - r_thin = r_temp.head(21+state_server.cam_states.size()*6); + H_thin = H_temp.topRows(21+state_server.cam_states.size()*7); + r_thin = r_temp.head(21+state_server.cam_states.size()*7); //HouseholderQR qr_helper(H); //MatrixXd Q = qr_helper.householderQ(); @@ -1720,7 +1671,7 @@ 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<6>(21+i*6); + const VectorXd& delta_x_cam = delta_x.segment<6>(21+i*7); const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>()); cam_state_iter->second.orientation = quaternionMultiplication( dq_cam, cam_state_iter->second.orientation); @@ -1812,9 +1763,9 @@ void MsckfVio::removeLostFeatures() { if(PHOTOMETRIC) //just use max. size, as gets shrunken down after anyway - jacobian_row_size += N*N*feature.observations.size(); + jacobian_row_size += 2*feature.observations.size(); else - jacobian_row_size += 4*feature.observations.size() - 3; + jacobian_row_size += 2*feature.observations.size() - 3; processed_feature_ids.push_back(feature.id); } @@ -1831,7 +1782,7 @@ 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; @@ -1982,9 +1933,9 @@ void MsckfVio::pruneCamStateBuffer() { } } if(PHOTOMETRIC) - jacobian_row_size += N*N*involved_cam_state_ids.size(); + jacobian_row_size += 2*involved_cam_state_ids.size(); else - jacobian_row_size += 4*involved_cam_state_ids.size() - 3; + jacobian_row_size += 2*involved_cam_state_ids.size() - 3; } //cout << "jacobian row #: " << jacobian_row_size << endl; @@ -1992,7 +1943,7 @@ 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; for (auto& item : map_server) { @@ -2037,8 +1988,8 @@ void MsckfVio::pruneCamStateBuffer() { 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 @@ -2059,10 +2010,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.