From 6ee756941c062d779763903a8a90070bd3b8b602 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Wed, 3 Jul 2019 16:11:23 +0200 Subject: [PATCH] added stereo camera residual and jacobi to twomsckf - works --- include/msckf_vio/cam_state.h | 2 + include/msckf_vio/feature.hpp | 36 ++------ launch/msckf_vio_tinytum.launch | 4 +- src/msckf_vio.cpp | 150 +++++++++++++++++++------------- 4 files changed, 99 insertions(+), 93 deletions(-) diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index 723f252..880c70c 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -18,6 +18,8 @@ namespace msckf_vio { struct Frame{ cv::Mat image; + cv::Mat dximage; + cv::Mat dyimage; double exposureTime_ms; }; diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index c45f6a0..e30df3c 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -184,9 +184,10 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, const CameraCalibration& cam, Eigen::Vector3d& in_p) const; -double CompleteCvKernel( + double CompleteCvKernel( const cv::Point2f pose, - const cv::Mat& frame, + const StateIDType& cam_state_id, + CameraCalibration& cam, std::string type) const; double cvKernel( @@ -495,40 +496,17 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const double Feature::CompleteCvKernel( const cv::Point2f pose, - const cv::Mat& frame, + const StateIDType& cam_state_id, + CameraCalibration& cam, std::string type) const { double delta = 0; - cv::Mat xder; - cv::Mat yder; - cv::Mat deeper_frame; - frame.convertTo(deeper_frame,CV_16S); - //TODO remove this? - - - cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); - cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); - - xder/=8.; - yder/=8.; - - /* - cv::Mat norm_abs_xderImage; - cv::Mat abs_xderImage2; - cv::convertScaleAbs(xder, abs_xderImage2); - - cv::normalize(abs_xderImage2, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1); - - cv::imshow("xder", norm_abs_xderImage); - cvWaitKey(0); - */ - if(type == "Sobel_x") - delta = ((double)xder.at(pose.y, pose.x))/255.; + delta = ((double)cam.moving_window.find(cam_state_id)->second.dximage.at(pose.y, pose.x))/255.; else if (type == "Sobel_y") - delta = ((double)yder.at(pose.y, pose.x))/255.; + delta = ((double)cam.moving_window.find(cam_state_id)->second.dyimage.at(pose.y, pose.x))/255.; return delta; } diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 28ae944..d4fbc31 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 2ba011b..af56991 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -544,13 +544,22 @@ void MsckfVio::manageMovingWindow( image_handler::undistortImage(cam1_img_ptr->image, cam1_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); - // save image information into moving window cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone(); cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone(); + cv::Mat xder; + cv::Mat yder; + cv::Mat deeper_frame; + cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); - + cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); + xder/=8.; + yder/=8.; + + cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); + cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone(); @@ -1310,96 +1319,98 @@ void MsckfVio::twodotMeasurementJacobian( const CAMState& cam_state = state_server.cam_states[cam_state_id]; const Feature& feature = map_server[feature_id]; - // Cam0 pose. + // Cam0 pose Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); const Vector3d& t_c0_w = cam_state.position; - //photometric observation - std::vector photo_z; + // Cam1 pose + Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); + Matrix3d R_w_c1 = R_c0_c1 * R_w_c0; + Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation(); // individual Jacobians - Matrix dh_dCpij = Matrix::Zero(); - Matrix dh_dGpij = Matrix::Zero(); - Matrix dh_dXplj = Matrix::Zero(); + Matrix dh_dC0pij = Matrix::Zero(); + Matrix dh_dC1pij = 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(); + Matrix dC0pij_dGpij = Matrix::Zero(); + Matrix dC1pij_dGpij = Matrix::Zero(); + Matrix dC0pij_dXplj = Matrix::Zero(); + Matrix dC1pij_dXplj = Matrix::Zero(); // one line of the NxN Jacobians - Eigen::Matrix H_rho; - Eigen::Matrix H_plj; - Eigen::Matrix H_pAj; + 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); + Eigen::Vector3d p_c1 = R_w_c1 * (point-t_c1_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)); + dh_dC0pij(0, 0) = 1. / p_c0(2); + dh_dC0pij(1, 1) = 1. / p_c0(2); + dh_dC0pij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); + dh_dC0pij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); - dCpij_dGpij = quaternionToRotation(cam_state.orientation); + //dh / d{}^Cp_{ij} + dh_dC1pij(2, 0) = 1. / p_c1(2); + dh_dC1pij(3, 1) = 1. / p_c1(2); + dh_dC1pij(2, 2) = -(p_c1(0))/(p_c1(2)*p_c1(2)); + dh_dC1pij(3, 2) = -(p_c1(1))/(p_c1(2)*p_c1(2)); - //orientation takes camera frame to world frame - dh_dGpij = dh_dCpij * dCpij_dGpij; + dC0pij_dGpij = R_w_c0; + dC1pij_dGpij = R_c0_c1 * R_w_c0; - //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; + dC0pij_dXplj.leftCols(3) = skewSymmetric(p_c0); + dC0pij_dXplj.rightCols(3) = -R_w_c0; - //d{}^Gp_P{ij} / \rho_i + + // 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_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)); - - - // 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), + * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho), + feature.anchorPatch_ideal[(N*N-1)/2].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 + H_rho = dh_dC0pij * dC0pij_dGpij * dGpj_drhoj + dh_dC1pij * dC1pij_dGpij * dGpj_drhoj; // 4 x 1 + H_plj = dh_dC0pij * dC0pij_dXplj + dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6 + H_pAj = dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 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); + VectorXd r_i = VectorXd::Zero(4); //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); + r_i[0] = total_z[0] - p_c0(0)/p_c0(2); + r_i[1] = total_z[1] - p_c0(1)/p_c0(2); + r_i[2] = total_z[2] - p_c1(0)/p_c1(2); + r_i[3] = total_z[3] - p_c1(1)/p_c1(2); + + MatrixXd H_xl = MatrixXd::Zero(4, 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; + H_xl.block(0, 21+cam_state_cntr_anchor*7, 4, 6) = H_pAj; // set frame Jakobi //get position of current frame in cam states @@ -1407,7 +1418,7 @@ void MsckfVio::twodotMeasurementJacobian( 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_xl.block(0, 21+cam_state_cntr*7, 4, 6) = H_plj; H_x = H_xl; H_y = H_rho; @@ -1446,7 +1457,7 @@ void MsckfVio::twodotFeatureJacobian( } int jacobian_row_size = 0; - jacobian_row_size = 2 * valid_cam_state_ids.size(); + jacobian_row_size = 4 * valid_cam_state_ids.size(); MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, 21+state_server.cam_states.size()*7); @@ -1458,7 +1469,7 @@ void MsckfVio::twodotFeatureJacobian( MatrixXd H_xl; MatrixXd H_yl; - Eigen::VectorXd r_l = VectorXd::Zero(2); + Eigen::VectorXd r_l = VectorXd::Zero(4); twodotMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l); auto cam_state_iter = state_server.cam_states.find(cam_id); @@ -1468,13 +1479,14 @@ void MsckfVio::twodotFeatureJacobian( // 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; + r_i.segment(stack_cntr, 4) = r_l; + stack_cntr += 4; } // Project the residual and Jacobians onto the nullspace // of H_yj. + // get Nullspace FullPivLU lu(H_yi.transpose()); MatrixXd A_null_space = lu.kernel(); @@ -1518,10 +1530,10 @@ void MsckfVio::twodotFeatureJacobian( std::cout << "resume playback" << std::endl; nh.setParam("/play_bag", true); } - return; } + bool MsckfVio::PhotometricPatchPointResidual( const StateIDType& cam_state_id, const Feature& feature, @@ -1628,8 +1640,8 @@ bool MsckfVio::PhotometricPatchPointJacobian( // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_x"); - dy = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_y"); + dx = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_x"); + dy = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_y"); //cout << "dx: " << dx << " : " << feature.cvKernel(p_in_c0, "Sobel_x") << " : " << feature.Kernel(p_in_c0, frame, "Sobel_x") << endl; dI_dhj(0, 0) = dx * cam0.intrinsics[0]; @@ -1806,6 +1818,8 @@ bool MsckfVio::PhotometricFeatureJacobian( const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) { + + return false; const auto& feature = map_server[feature_id]; // Check how many camera states in the provided camera @@ -2164,7 +2178,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { // cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; - cout << "reg: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + // cout << "reg: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; if(FILTER != 0) return; @@ -2247,8 +2261,12 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { + if (H.rows() == 0 || r.rows() == 0) + { + cout << "zero" << endl; return; + } // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -2298,6 +2316,8 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // Update the IMU state. if (FILTER != 2) return; + cout << "two: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); @@ -2403,7 +2423,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r VectorXd delta_x = K * r; // Update the IMU state. - cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + // cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; if (FILTER != 1) return; @@ -2484,7 +2504,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - return true; + //return true; MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P2 = Feature::observation_noise * @@ -2492,8 +2512,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - cout << "gate: " << 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; @@ -2552,7 +2572,7 @@ void MsckfVio::removeLostFeatures() { } pjacobian_row_size += N*N*feature.observations.size(); - twojacobian_row_size += 2*feature.observations.size(); + twojacobian_row_size += 4*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; processed_feature_ids.push_back(feature.id); @@ -2604,6 +2624,7 @@ void MsckfVio::removeLostFeatures() { if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) { if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + //cout << "passed" << endl; 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(); @@ -2619,6 +2640,7 @@ void MsckfVio::removeLostFeatures() { stack_cntr += H_xj.rows(); } if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { + cout << "passed" << endl; 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(); @@ -2749,7 +2771,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(); + twojacobian_row_size += 4*feature.observations.size(); } @@ -2787,6 +2809,7 @@ void MsckfVio::pruneLastCamStateBuffer() if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + //cout << "passed" << endl; 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(); @@ -2804,6 +2827,7 @@ void MsckfVio::pruneLastCamStateBuffer() } if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + cout << "passed" << endl; 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(); @@ -2929,7 +2953,7 @@ void MsckfVio::pruneCamStateBuffer() { } } - twojacobian_row_size += 2*involved_cam_state_ids.size(); + twojacobian_row_size += 4*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; } @@ -2969,6 +2993,7 @@ void MsckfVio::pruneCamStateBuffer() { if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { + //cout << "passed" << endl; 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(); @@ -2985,6 +3010,7 @@ void MsckfVio::pruneCamStateBuffer() { } if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + cout << "passed" << endl; 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();