From 0f96c916f1d65e78d7031f4ab48bff71982495c2 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 21 May 2019 13:34:58 +0200 Subject: [PATCH] minor output changes, added arrows for gradient and residual visualization --- include/msckf_vio/feature.hpp | 129 ++++++++++++++++++++++++---------- include/msckf_vio/msckf_vio.h | 4 +- launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 96 ++++++++++++++----------- 4 files changed, 151 insertions(+), 80 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 8a6cc27..ce090ae 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -15,7 +15,7 @@ #include #include #include - +#include #include #include #include @@ -182,12 +182,14 @@ bool MarkerGeneration( const StateIDType& cam_state_id, CameraCalibration& cam0, const std::vector photo_r, - std::stringstream& ss) const; + std::stringstream& ss, + cv::Point2f gradientVector, + cv::Point2f residualVector) const; /* - * @brief projectPixelToPosition uses the calcualted pixels + * @brief AnchorPixelToPosition uses the calcualted pixels * of the anchor patch to generate 3D positions of all of em */ -inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, +inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam); /* @@ -533,7 +535,9 @@ bool Feature::VisualizePatch( const StateIDType& cam_state_id, CameraCalibration& cam0, const std::vector photo_r, - std::stringstream& ss) const + std::stringstream& ss, + cv::Point2f gradientVector, + cv::Point2f residualVector) const { double rescale = 1; @@ -573,45 +577,107 @@ bool Feature::VisualizePatch( cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu); - // irradiance grid anchor + + // patches visualization int N = sqrt(anchorPatch_3d.size()); - int scale = 20; + int scale = 30; cv::Mat irradianceFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255)); cv::resize(irradianceFrame, irradianceFrame, cv::Size(), rescale, rescale); + + // irradiance grid anchor + std::stringstream namer; + namer << "anchor"; + cv::putText(irradianceFrame, namer.str() , cvPoint(30, 25), + cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA); + + for(int i = 0; isecond(0),observations.find(cam_state_id)->second(1)); + // move to real pixels + p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs); + + for(int i = 0; i0) cv::rectangle(irradianceFrame, - cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)), - cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), + cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), + cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), cv::Scalar(255 - photo_r[i*N+j]*255, 255 - photo_r[i*N+j]*255, 255), CV_FILLED); else cv::rectangle(irradianceFrame, - cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)), - cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), + cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), + cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255), CV_FILLED); - cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); + // gradient arrow + /* + cv::arrowedLine(irradianceFrame, + cv::Point(30+scale*(N/2 +0.5), 50+scale*(N+(N/2)+0.5)), + cv::Point(30+scale*(N/2+0.5)+scale*gradientVector.x, 50+scale*(N+(N/2)+0.5)+scale*gradientVector.y), + cv::Scalar(100, 0, 255), + 1); + */ + + // residual gradient direction + cv::arrowedLine(irradianceFrame, + cv::Point(40+scale*(N+N/2+0.5), 15+scale*((N-0.5))), + cv::Point(40+scale*(N+N/2+0.5)+scale*residualVector.x, 15+scale*(N-0.5)+scale*residualVector.y), + cv::Scalar(0, 255, 175), + 3); + + + cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); /* // visualize position of used observations and resulting feature position @@ -703,7 +769,7 @@ cv::Point2f Feature::projectPositionToCamera( return my_p; } -Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam) +Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam) { // use undistorted position of point of interest // project it back into 3D space using pinhole model @@ -742,27 +808,19 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), cam.intrinsics, cam.distortion_model, - 0); + cam.distortion_coeffs); // create vector of patch in pixel plane - std::vectorund_pix_v; for(double u_run = -n; u_run <= n; u_run++) for(double v_run = -n; v_run <= n; v_run++) - und_pix_v.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run)); + anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run)); //create undistorted pure points - std::vector und_v; - image_handler::undistortPoints(und_pix_v, + image_handler::undistortPoints(anchorPatch_real, cam.intrinsics, cam.distortion_model, - 0, - und_v); - - //create distorted pixel points - anchorPatch_real = image_handler::distortPoints(und_v, - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs); + cam.distortion_coeffs, + anchorPatch_ideal); // save anchor position for later visualisaztion @@ -770,19 +828,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) // save true pixel Patch position for(auto point : anchorPatch_real) - { if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1)) return false; - } + for(auto point : anchorPatch_real) anchorPatch.push_back(PixelIrradiance(point, anchorImage)); // project patch pixel to 3D space in camera coordinate system - for(auto point : und_v) - { - anchorPatch_ideal.push_back(point); - anchorPatch_3d.push_back(projectPixelToPosition(point, cam)); - } + for(auto point : anchorPatch_ideal) + anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam)); + is_anchored = true; return true; } diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index bcf5ee2..77a8099 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -14,7 +14,7 @@ #include #include #include - +#include #include #include #include @@ -38,6 +38,8 @@ #include #include +#define PI 3.14159265 + namespace msckf_vio { /* * @brief MsckfVio Implements the algorithm in diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index eca1ebc..9e103b7 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -24,7 +24,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 31a891e..2e8e574 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1238,6 +1238,8 @@ void MsckfVio::PhotometricMeasurementJacobian( //photometric observation std::vector photo_z; + std::vector photo_r; + // individual Jacobians Matrix dI_dhj = Matrix::Zero(); Matrix dh_dCpij = Matrix::Zero(); @@ -1262,9 +1264,31 @@ void MsckfVio::PhotometricMeasurementJacobian( auto frame = cam0.moving_window.find(cam_state_id)->second.image; + //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); + + // 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); + int count = 0; double dx, dy; + // gradient visualization parameters + cv::Point2f gradientVector(0,0); + + // residual change visualization + cv::Point2f residualVector(0,0); + double res_sum = 0; + for (auto point : feature.anchorPatch_3d) { Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); @@ -1273,14 +1297,24 @@ void MsckfVio::PhotometricMeasurementJacobian( //add observation photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); - // add jacobian + //calculate photom. residual + photo_r.push_back(photo_z[count] - estimate_photo_z[count]); + + // add jacobians // 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; + + gradientVector.x += dx; + gradientVector.y += dy; + residualVector.x += dx * photo_r[count]; + residualVector.y += dy * photo_r[count]; + res_sum += photo_r[count]; + //dh / d{}^Cp_{ij} dh_dCpij(0, 0) = 1 / p_c0(2); dh_dCpij(1, 1) = 1 / p_c0(2); @@ -1320,28 +1354,6 @@ void MsckfVio::PhotometricMeasurementJacobian( 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); - - // 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); @@ -1379,12 +1391,13 @@ void MsckfVio::PhotometricMeasurementJacobian( 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, gradientVector, residualVector); } return; @@ -1481,13 +1494,11 @@ void MsckfVio::PhotometricFeatureJacobian( H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; - ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov < H.cols()) { // Convert H to a sparse matrix. SparseMatrix H_sparse = H.sparseView(); @@ -1663,8 +1676,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()*augmentationSize); + r_thin = r_temp.head(21+state_server.cam_states.size()*augmentationSize); //HouseholderQR qr_helper(H); //MatrixXd Q = qr_helper.householderQ(); @@ -1676,18 +1689,19 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { 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() + + MatrixXd S = H*P*H.transpose() + Feature::observation_noise*MatrixXd::Identity( - H_thin.rows(), H_thin.rows()); - //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H_thin*P); - MatrixXd K_transpose = S.ldlt().solve(H_thin*P); + H.rows(), H.rows()); + //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); + MatrixXd K_transpose = S.ldlt().solve(H*P); MatrixXd K = K_transpose.transpose(); // Compute the error of the state. - VectorXd delta_x = K * r_thin; + VectorXd delta_x = K * r; // Update the IMU state. const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1722,7 +1736,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(21+i*augmentationSize, augmentationSize); const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>()); cam_state_iter->second.orientation = quaternionMultiplication( dq_cam, cam_state_iter->second.orientation); @@ -1730,7 +1744,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { } // Update state covariance. - MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin; + MatrixXd I_KH = MatrixXd::Identity(K.rows(), H.cols()) - K*H; //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; @@ -1744,7 +1758,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { } bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - + return true; MatrixXd P1 = H * state_server.state_cov * H.transpose();