From 010db87e4b1b9f69d38e19a1b1627a20cb2d0e79 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 16:28:45 +0200 Subject: [PATCH 01/19] tinytum works completely, image_handler equidistant distort/undistort work --- config/camchain-imucam-tum-scaled.yaml | 18 +++++++++--------- include/msckf_vio/image_handler.h | 10 ++++++++++ launch/image_processor_tinytum.launch | 4 ++-- launch/msckf_vio_tinytum.launch | 13 +++++-------- launch/msckf_vio_tum.launch | 6 +++--- 5 files changed, 29 insertions(+), 22 deletions(-) diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index 529828c..4facacc 100644 --- a/config/camchain-imucam-tum-scaled.yaml +++ b/config/camchain-imucam-tum-scaled.yaml @@ -5,11 +5,11 @@ cam0: -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_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, + 0.00020293673591811182] distortion_model: equidistant - intrinsics: [95.2026071784, 95.2029854486, 127.573663262, 128.582615763] - resolution: [256, 256] + intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] + resolution: [512, 512] rostopic: /cam0/image_raw cam1: T_cam_imu: @@ -23,14 +23,14 @@ cam1: 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_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, + 0.0003299517423931039] distortion_model: equidistant - intrinsics: [94.8217471066, 94.8164593555, 126.391667581, 127.571024044] - resolution: [256, 256] + intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] + resolution: [512, 512] 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 + 0.0000, 0.0000, 0.0000, 1.0000] diff --git a/include/msckf_vio/image_handler.h b/include/msckf_vio/image_handler.h index d664be6..3cf2ed2 100644 --- a/include/msckf_vio/image_handler.h +++ b/include/msckf_vio/image_handler.h @@ -16,6 +16,16 @@ namespace msckf_vio { */ namespace image_handler { +cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics); +cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics); + +void undistortImage( + cv::InputArray src, + cv::OutputArray dst, + const std::string& distortion_model, + const cv::Vec4d& intrinsics, + const cv::Vec4d& distortion_coeffs); + void undistortPoints( const std::vector& pts_in, const cv::Vec4d& intrinsics, diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index 6fe2855..53dd99a 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -25,8 +25,8 @@ - - + + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 94deb7c..06d4c7f 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -6,7 +6,7 @@ default="$(find msckf_vio)/config/camchain-imucam-tum-scaled.yaml"/> - + @@ -18,11 +18,11 @@ output="screen"> - + - + @@ -64,16 +64,13 @@ - - + + - - - diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 390ab03..0240de3 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,11 +18,11 @@ output="screen"> - + - + @@ -72,6 +72,6 @@ - + From 655416a49024681e280e1b4ff952d8a59b60c458 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 16:38:02 +0200 Subject: [PATCH 02/19] added image_handler undistorted image --- src/image_handler.cpp | 143 +++++++++++++++++++++++++++++++++++++--- src/image_processor.cpp | 6 ++ 2 files changed, 141 insertions(+), 8 deletions(-) diff --git a/src/image_handler.cpp b/src/image_handler.cpp index cb98426..8664922 100644 --- a/src/image_handler.cpp +++ b/src/image_handler.cpp @@ -14,6 +14,34 @@ namespace msckf_vio { namespace image_handler { +cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics) +{ + return cv::Point2f(p_in.x * intrinsics[0] + intrinsics[2], p_in.y * intrinsics[1] + intrinsics[3]); +} + +cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics) +{ + return cv::Point2f((p_in.x - intrinsics[2])/intrinsics[0], (p_in.y - intrinsics[3])/intrinsics[1]); +} + +void undistortImage( + cv::InputArray src, + cv::OutputArray dst, + const std::string& distortion_model, + const cv::Vec4d& intrinsics, + const cv::Vec4d& distortion_coeffs) +{ + + const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2], + 0.0, intrinsics[1], intrinsics[3], + 0.0, 0.0, 1.0); + + if (distortion_model == "pre-equidistant") + cv::fisheye::undistortImage(src, dst, K, distortion_coeffs, K); + else if (distortion_model == "equidistant") + src.copyTo(dst); +} + void undistortPoint( const cv::Point2f& pt_in, const cv::Vec4d& intrinsics, @@ -42,10 +70,36 @@ void undistortPoint( if (distortion_model == "radtan") { cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, rectification_matrix, K_new); - } else if (distortion_model == "equidistant") { + } + // equidistant + else if (distortion_model == "equidistant") { cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs, rectification_matrix, K_new); - } else { + } + // fov + else if (distortion_model == "fov") { + for(int i = 0; i < pts_in.size(); i++) + { + float omega = distortion_coeffs[0]; + float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y); + float ru = tan(rd * omega)/(2 * tan(omega / 2)); + + cv::Point2f newPoint( + ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd), + ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd)); + + pts_out.push_back(newPoint); + } + } + else if (distortion_model == "pre-equidistant") + { + std::vector temp_pts_out; + for(int i = 0; i < pts_in.size(); i++) + temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics)); + + pts_out = temp_pts_out; + } + else { ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...", distortion_model.c_str()); cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, @@ -79,10 +133,35 @@ void undistortPoints( if (distortion_model == "radtan") { cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, rectification_matrix, K_new); - } else if (distortion_model == "equidistant") { + } + else if (distortion_model == "equidistant") { cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs, rectification_matrix, K_new); - } else { + } + else if (distortion_model == "fov") { + for(int i = 0; i < pts_in.size(); i++) + { + float omega = distortion_coeffs[0]; + float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y); + float ru = tan(rd * omega)/(2 * tan(omega / 2)); + + cv::Point2f newPoint( + ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd), + ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd)); + + pts_out.push_back(newPoint); + } + } + else if (distortion_model == "pre-equidistant") + { + std::vector temp_pts_out; + for(int i = 0; i < pts_in.size(); i++) + temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics)); + + pts_out = temp_pts_out; + + } + else { ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...", distortion_model.c_str()); cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, @@ -110,7 +189,31 @@ std::vector distortPoints( distortion_coeffs, pts_out); } else if (distortion_model == "equidistant") { cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs); - } else { + } + else if (distortion_model == "fov") { + for(int i = 0; i < pts_in.size(); i++) + { + // based on 'straight lines have to be straight' + float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y); + float omega = distortion_coeffs[0]; + float rd = 1 / (omega)*atan(2*ru*tan(omega / 2)); + + cv::Point2f newPoint( + pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2], + pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]); + + pts_out.push_back(newPoint); + } + } + else if (distortion_model == "pre-equidistant") + { + std::vector temp_pts_out; + for(int i = 0; i < pts_in.size(); i++) + temp_pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics)); + + pts_out = temp_pts_out; + } + else { ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...", distortion_model.c_str()); std::vector homogenous_pts; @@ -143,7 +246,31 @@ cv::Point2f distortPoint( distortion_coeffs, pts_out); } else if (distortion_model == "equidistant") { cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs); - } else { + } + else if (distortion_model == "fov") { + for(int i = 0; i < pts_in.size(); i++) + { + // based on 'straight lines have to be straight' + float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y); + float omega = distortion_coeffs[0]; + float rd = 1 / (omega)*atan(2*ru*tan(omega / 2)); + + cv::Point2f newPoint( + pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2], + pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]); + + pts_out.push_back(newPoint); + } + } + else if (distortion_model == "pre-equidistant") + { + std::vector temp_pts_out; + for(int i = 0; i < pts_in.size(); i++) + pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics)); + + pts_out = temp_pts_out; + } + else { ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...", distortion_model.c_str()); std::vector homogenous_pts; @@ -155,5 +282,5 @@ cv::Point2f distortPoint( return pts_out[0]; } - } // namespace image_handler -} // namespace msckf_vio \ No newline at end of file + } // namespace image_handler +} // namespace msckf_vio diff --git a/src/image_processor.cpp b/src/image_processor.cpp index d35bb16..0e690b7 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -219,6 +219,12 @@ void ImageProcessor::stereoCallback( cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img, sensor_msgs::image_encodings::MONO8); + + image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + + + // Build the image pyramids once since they're used at multiple places createImagePyramids(); From af1820a238a31f44a3af957afd41d55cc6e115cd Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 17:27:47 +0200 Subject: [PATCH 03/19] added check if pre-undistorted, works for still distorted --- include/msckf_vio/feature.hpp | 75 ++++++++++++++++++++++------------- src/msckf_vio.cpp | 10 +++++ 2 files changed, 57 insertions(+), 28 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 466d1e9..d558460 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -969,7 +969,7 @@ cv::Point2f Feature::projectPositionToCamera( Eigen::Isometry3d T_c0_w; cv::Point2f out_p; - + cv::Point2f my_p; // transfrom position to camera frame Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); const Eigen::Vector3d& t_c0_w = cam_state.position; @@ -980,11 +980,15 @@ cv::Point2f Feature::projectPositionToCamera( // if(cam_state_id == observations.begin()->first) //printf("undist:\n \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y); - cv::Point2f my_p = image_handler::distortPoint(out_p, + if (cam.distortion_model.substr(0,3) == "pre-") + my_p = cv::Point2f(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]); + else + my_p = image_handler::distortPoint(out_p, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs); + // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2)); // printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y); @@ -1008,8 +1012,6 @@ Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCal bool Feature::initializeAnchor(const CameraCalibration& cam, int N) { - - //initialize patch Size int n = (int)(N-1)/2; @@ -1037,51 +1039,68 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2]; auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3]; - //testing - undist_anchor_center_pos = cv::Point2f(u,v); - //for NxN patch pixels around feature - int count = 0; + // check if image has been pre-undistorted + if(cam.distortion_model.substr(0,3) == "pre-") + { + std::cout << "is a pre" << std::endl; + //project onto pixel plane + undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]); - // get feature in undistorted pixel space - // this only reverts from 'pure' space into undistorted pixel space using camera matrix - cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs); - // create vector of patch in pixel plane - for(double u_run = -n; u_run <= n; u_run++) - for(double v_run = -n; v_run <= n; v_run++) - anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run)); + // create vector of patch in pixel plane + for(double u_run = -n; u_run <= n; u_run++) + for(double v_run = -n; v_run <= n; v_run++) + anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run)); + + //project back into u,v + for(int i = 0; i < N*N; i++) + anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1])); + } + + else + { + // get feature in undistorted pixel space + // this only reverts from 'pure' space into undistorted pixel space using camera matrix + cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs); + // create vector of patch in pixel plane + for(double u_run = -n; u_run <= n; u_run++) + for(double v_run = -n; v_run <= n; v_run++) + anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run)); - //create undistorted pure points - image_handler::undistortPoints(anchorPatch_real, - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs, - anchorPatch_ideal); + //create undistorted pure points + image_handler::undistortPoints(anchorPatch_real, + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs, + anchorPatch_ideal); + } // save anchor position for later visualisaztion anchor_center_pos = anchorPatch_real[(N*N-1)/2]; - + // 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)) + if(point.x - n < 0 || point.x + n >= cam.resolution(0)-1 || point.y - n < 0 || point.y + n >= cam.resolution(1)-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 : anchorPatch_ideal) anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam)); is_anchored = true; + return true; } + bool Feature::initializeRho(const CamStateServer& cam_states) { // Organize camera poses and feature observations properly. diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 323e791..e6ce390 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -546,10 +546,20 @@ void MsckfVio::manageMovingWindow( cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img, sensor_msgs::image_encodings::MONO8); + image_handler::undistortImage(cam0_img_ptr->image, cam0_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + 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(); + + + + + //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) while(cam0.moving_window.size() > 100) { From bfb2a551a7cfabaa34637b00005de814a3e94ed2 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 17:54:16 +0200 Subject: [PATCH 04/19] fixed timedelay for publishing structure when bigger time delays than queue allows for --- config/camchain-imucam-tum-scaled.yaml | 4 ++-- src/image_processor.cpp | 12 ++++++------ src/msckf_vio.cpp | 15 +++++++++++---- 3 files changed, 19 insertions(+), 12 deletions(-) diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index 4facacc..f5ea7e8 100644 --- a/config/camchain-imucam-tum-scaled.yaml +++ b/config/camchain-imucam-tum-scaled.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] resolution: [512, 512] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] resolution: [512, 512] rostopic: /cam1/image_raw diff --git a/src/image_processor.cpp b/src/image_processor.cpp index 0e690b7..168eebc 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -219,18 +219,18 @@ void ImageProcessor::stereoCallback( cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img, sensor_msgs::image_encodings::MONO8); + ros::Time start_time = ros::Time::now(); image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); - - - + ROS_INFO("Publishing: %f", + (ros::Time::now()-start_time).toSec()); // Build the image pyramids once since they're used at multiple places createImagePyramids(); // Detect features in the first frame. if (is_first_img) { - ros::Time start_time = ros::Time::now(); + start_time = ros::Time::now(); initializeFirstFrame(); //ROS_INFO("Detection time: %f", // (ros::Time::now()-start_time).toSec()); @@ -243,7 +243,7 @@ void ImageProcessor::stereoCallback( // (ros::Time::now()-start_time).toSec()); } else { // Track the feature in the previous image. - ros::Time start_time = ros::Time::now(); + start_time = ros::Time::now(); trackFeatures(); //ROS_INFO("Tracking time: %f", // (ros::Time::now()-start_time).toSec()); @@ -273,7 +273,7 @@ void ImageProcessor::stereoCallback( // (ros::Time::now()-start_time).toSec()); // Publish features in the current image. - ros::Time start_time = ros::Time::now(); + start_time = ros::Time::now(); publish(); //ROS_INFO("Publishing: %f", // (ros::Time::now()-start_time).toSec()); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index e6ce390..10b9cd7 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -405,12 +405,19 @@ void MsckfVio::imageCallback( const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) { - // Return if the gravity vector has not been set. - if (!is_gravity_set) return; - - // stop playing bagfile if printing images + cout << "hello" << endl; + // stop playing bagfile if printing images if(STREAMPAUSE) nh.setParam("/play_bag", false); + // Return if the gravity vector has not been set. + if (!is_gravity_set) + { + if(STREAMPAUSE) + nh.setParam("/play_bag", true); + return; + } + + // Start the system if the first image is received. // The frame where the first image is received will be From 9f528c1ea13e00d11c30352e5ce6683a7219838a Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 18:25:43 +0200 Subject: [PATCH 05/19] added support for euroc dataset --- launch/msckf_vio_euroc.launch | 11 +++++++++++ src/image_handler.cpp | 12 ++++++++---- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index 896feff..5f6de1e 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -17,6 +17,17 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> + + + + + + + + + + + diff --git a/src/image_handler.cpp b/src/image_handler.cpp index 8664922..f174fb7 100644 --- a/src/image_handler.cpp +++ b/src/image_handler.cpp @@ -40,6 +40,10 @@ void undistortImage( cv::fisheye::undistortImage(src, dst, K, distortion_coeffs, K); else if (distortion_model == "equidistant") src.copyTo(dst); + else if (distortion_model == "pre-radtan") + cv::undistort(src, dst, K, distortion_coeffs, K); + else if (distortion_model == "radtan") + src.copyTo(dst); } void undistortPoint( @@ -91,7 +95,7 @@ void undistortPoint( pts_out.push_back(newPoint); } } - else if (distortion_model == "pre-equidistant") + else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan") { std::vector temp_pts_out; for(int i = 0; i < pts_in.size(); i++) @@ -152,7 +156,7 @@ void undistortPoints( pts_out.push_back(newPoint); } } - else if (distortion_model == "pre-equidistant") + else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan") { std::vector temp_pts_out; for(int i = 0; i < pts_in.size(); i++) @@ -205,7 +209,7 @@ std::vector distortPoints( pts_out.push_back(newPoint); } } - else if (distortion_model == "pre-equidistant") + else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan") { std::vector temp_pts_out; for(int i = 0; i < pts_in.size(); i++) @@ -262,7 +266,7 @@ cv::Point2f distortPoint( pts_out.push_back(newPoint); } } - else if (distortion_model == "pre-equidistant") + else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan") { std::vector temp_pts_out; for(int i = 0; i < pts_in.size(); i++) From 715ca6a6b46a273727c76f52a9ab71775cebadb7 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 19:22:08 +0200 Subject: [PATCH 06/19] added two filter, not working yet - compare with htest --- include/msckf_vio/msckf_vio.h | 14 +- launch/msckf_vio_euroc.launch | 5 +- launch/msckf_vio_tinytum.launch | 4 +- src/msckf_vio.cpp | 408 ++++++++++++++++++++++++++++++-- 4 files changed, 411 insertions(+), 20 deletions(-) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 9e9975d..49af3cd 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -207,6 +207,11 @@ class MsckfVio { Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + void twodotMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r); + void PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, @@ -214,6 +219,11 @@ class MsckfVio { Eigen::MatrixXd& H_y, Eigen::VectorXd& r); + void twodotFeatureJacobian( + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + void PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, @@ -222,6 +232,8 @@ class MsckfVio { void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); void measurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); + void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); + bool gatingTest(const Eigen::MatrixXd& H, const Eigen::VectorXd&r, const int& dof); void removeLostFeatures(); @@ -234,7 +246,7 @@ class MsckfVio { void onlineReset(); // Photometry flag - bool PHOTOMETRIC; + int FILTER; // debug flag bool STREAMPAUSE; diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index 5f6de1e..b818735 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -17,9 +17,8 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - - + + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 06d4c7f..28ae944 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -17,8 +17,8 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 10b9cd7..30ee2e2 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -67,7 +67,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh): bool MsckfVio::loadParameters() { //Photometry Flag - nh.param("PHOTOMETRIC", PHOTOMETRIC, false); + nh.param("FILTER", FILTER, 0); nh.param("PrintImages", PRINTIMAGES, false); nh.param("StreamPause", STREAMPAUSE, false); nh.param("GroundTruth", GROUNDTRUTH, false); @@ -405,8 +405,7 @@ void MsckfVio::imageCallback( const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) { - cout << "hello" << endl; - // stop playing bagfile if printing images + // stop playing bagfile if printing images if(STREAMPAUSE) nh.setParam("/play_bag", false); // Return if the gravity vector has not been set. @@ -455,7 +454,7 @@ void MsckfVio::imageCallback( double imu_processing_time = ( ros::Time::now()-start_time).toSec(); - //cout << "1" << endl; + cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); @@ -464,7 +463,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "2" << endl; + cout << "2" << endl; // Add new observations for existing features or new // features in the map server. start_time = ros::Time::now(); @@ -473,7 +472,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "3" << endl; + cout << "3" << endl; // Add new images to moving window start_time = ros::Time::now(); manageMovingWindow(cam0_img, cam1_img, feature_msg); @@ -481,20 +480,20 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "4" << endl; + cout << "4" << endl; // Perform measurement update if necessary. start_time = ros::Time::now(); removeLostFeatures(); double remove_lost_features_time = ( ros::Time::now()-start_time).toSec(); - //cout << "5" << endl; + cout << "5" << endl; start_time = ros::Time::now(); pruneLastCamStateBuffer(); double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); - //cout << "6" << endl; + cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); publish(feature_msg->header.stamp); @@ -1243,6 +1242,223 @@ void MsckfVio::addFeatureObservations( return; } +void MsckfVio::twodotMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) +{ + + // Prepare all the required data. + const CAMState& cam_state = state_server.cam_states[cam_state_id]; + const Feature& feature = map_server[feature_id]; + + // Cam0 pose. + Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); + const Vector3d& t_c0_w = cam_state.position; + + //photometric observation + std::vector photo_z; + + // individual Jacobians + Matrix dh_dCpij = 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(); + + // one line of the NxN Jacobians + 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); + // 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)); + + dCpij_dGpij = quaternionToRotation(cam_state.orientation); + + //orientation takes camera frame to world frame + 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; + + //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)); + + + + // 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), + 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 + + // 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); + + //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); + + // 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; + + // set frame Jakobi + //get position of current frame in cam states + auto cam_state_iter = state_server.cam_states.find(cam_state_id); + 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_x = H_xl; + H_y = H_rho; + r = r_i; + + //TODO make this more fluent as well + if(PRINTIMAGES) + { + std::stringstream ss; + ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; + feature.MarkerGeneration(marker_pub, state_server.cam_states); + //feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss); + } + + return; +} + +void MsckfVio::twodotFeatureJacobian( + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + MatrixXd& H_x, VectorXd& r) +{ + + const auto& feature = map_server[feature_id]; + + + // Check how many camera states in the provided camera + // id camera has actually seen this feature. + vector valid_cam_state_ids(0); + for (const auto& cam_id : cam_state_ids) { + if (feature.observations.find(cam_id) == + feature.observations.end()) continue; + + if (feature.observations.find(cam_id) == + feature.observations.begin()) continue; + valid_cam_state_ids.push_back(cam_id); + } + + int jacobian_row_size = 0; + 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, 1); + VectorXd r_i = VectorXd::Zero(jacobian_row_size); + int stack_cntr = 0; + + for (const auto& cam_id : valid_cam_state_ids) { + + MatrixXd H_xl; + MatrixXd H_yl; + Eigen::VectorXd r_l = VectorXd::Zero(2); + + twodotMeasurementJacobian(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_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; + } + + // Project the residual and Jacobians onto the nullspace + // of H_yj. + + // get Nullspace + FullPivLU lu(H_yi.transpose()); + MatrixXd A_null_space = lu.kernel(); + + H_x = A_null_space.transpose() * H_xi; + r = A_null_space.transpose() * r_i; + + if(PRINTIMAGES) + { + + ofstream myfile; + myfile.open("/home/raphael/dev/octave/log2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: Hx\n" + << "# type: matrix\n" + << "# rows: " << H_xi.rows() << "\n" + << "# columns: " << H_xi.cols() << "\n" + << H_xi << endl; + + myfile << "# name: Hy\n" + << "# type: matrix\n" + << "# rows: " << H_yi.rows() << "\n" + << "# columns: " << H_yi.cols() << "\n" + << H_yi << endl; + + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r_i.rows() << "\n" + << "# columns: " << 1 << "\n" + << r_i << endl; + + myfile.close(); + std::cout << "resume playback" << std::endl; + nh.setParam("/play_bag", true); + } + return; +} + + + void MsckfVio::PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, @@ -1431,6 +1647,7 @@ void MsckfVio::PhotometricMeasurementJacobian( return; } + void MsckfVio::PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, @@ -1737,7 +1954,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. - if(PHOTOMETRIC) return; + if(FILTER != 0) return; const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1793,6 +2010,108 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { return; } +void MsckfVio::twoMeasurementUpdate(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 << "two rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; + cout << "two update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + // Update the IMU state. + if (FILTER != 2) 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; +} + void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { @@ -1841,7 +2160,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& 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; + if (FILTER != 1) return; const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1925,6 +2244,7 @@ void MsckfVio::removeLostFeatures() { // BTW, find the size the final Jacobian matrix and residual vector. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int twojacobian_row_size = 0; vector invalid_feature_ids(0); vector processed_feature_ids(0); @@ -1965,7 +2285,9 @@ void MsckfVio::removeLostFeatures() { } pjacobian_row_size += N*N*feature.observations.size(); + twojacobian_row_size += 2*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; + processed_feature_ids.push_back(feature.id); } @@ -1974,6 +2296,9 @@ void MsckfVio::removeLostFeatures() { // processed_feature_ids.size() << endl; //cout << "jacobian row #: " << jacobian_row_size << endl; + + cout << "sizing" << endl; + // Remove the features that do not have enough measurements. for (const auto& feature_id : invalid_feature_ids) map_server.erase(feature_id); @@ -1991,6 +2316,12 @@ void MsckfVio::removeLostFeatures() { VectorXd pr = VectorXd::Zero(pjacobian_row_size); int pstack_cntr = 0; + + MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, + 21+7*state_server.cam_states.size()); + VectorXd twor = VectorXd::Zero(twojacobian_row_size); + int twostack_cntr = 0; + // Process the features which lose track. for (const auto& feature_id : processed_feature_ids) { auto& feature = map_server[feature_id]; @@ -2003,10 +2334,18 @@ void MsckfVio::removeLostFeatures() { VectorXd r_j; MatrixXd pH_xj; VectorXd pr_j; + MatrixXd twoH_xj; + VectorXd twor_j; + + + cout << "measuring" << endl; PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j); - + twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); + + cout << "gating" << endl; + 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; @@ -2017,20 +2356,29 @@ void MsckfVio::removeLostFeatures() { pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { + 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(); + } // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. //if (stack_cntr > 1500) break; } + cout << "resizing" << 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); + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); // Perform the measurement update step. measurementUpdate(H_x, r); photometricMeasurementUpdate(pH_x, pr); + twoMeasurementUpdate(twoH_x, twor); // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) @@ -2094,6 +2442,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Set the size of the Jacobian matrix. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int twojacobian_row_size = 0; //initialize all the features which are going to be removed @@ -2132,6 +2481,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(); } @@ -2143,9 +2493,14 @@ void MsckfVio::pruneLastCamStateBuffer() 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); + MatrixXd twoH_xj; + VectorXd twor_j; + MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size()); + VectorXd twor = VectorXd::Zero(twojacobian_row_size); int stack_cntr = 0; int pruned_cntr = 0; int pstack_cntr = 0; + int twostack_cntr = 0; for (auto& item : map_server) { auto& feature = item.second; @@ -2162,6 +2517,7 @@ void MsckfVio::pruneLastCamStateBuffer() PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); 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; @@ -2174,6 +2530,11 @@ void MsckfVio::pruneLastCamStateBuffer() pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + 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(); + } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2184,12 +2545,16 @@ void MsckfVio::pruneLastCamStateBuffer() pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); + // Perform measurement update. + + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); // Perform measurement update. measurementUpdate(H_x, r); photometricMeasurementUpdate(pH_x, pr); - + twoMeasurementUpdate(twoH_x, twor); //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -2241,6 +2606,7 @@ void MsckfVio::pruneCamStateBuffer() { // Find the size of the Jacobian matrix. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int twojacobian_row_size = 0; for (auto& item : map_server) { auto& feature = item.second; @@ -2285,6 +2651,7 @@ void MsckfVio::pruneCamStateBuffer() { } } + twojacobian_row_size += 2*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; } @@ -2302,6 +2669,11 @@ void MsckfVio::pruneCamStateBuffer() { 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; + MatrixXd twoH_xj; + VectorXd twor_j; + MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size()); + VectorXd twor = VectorXd::Zero(twojacobian_row_size); + int twostack_cntr = 0; for (auto& item : map_server) { auto& feature = item.second; // Check how many camera states to be removed are associated @@ -2317,6 +2689,7 @@ void MsckfVio::pruneCamStateBuffer() { PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_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; @@ -2329,6 +2702,11 @@ void MsckfVio::pruneCamStateBuffer() { pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + 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(); + } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); @@ -2339,11 +2717,13 @@ void MsckfVio::pruneCamStateBuffer() { r.conservativeResize(stack_cntr); pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); // Perform measurement update. measurementUpdate(H_x, r); photometricMeasurementUpdate(pH_x, pr); - + twoMeasurementUpdate(twoH_x, twor); //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), From 3ae7bdb13af5a1adf283105bf8b1804ed467ffe3 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 28 Jun 2019 10:14:32 +0200 Subject: [PATCH 07/19] upated two calculation, still not working --- launch/msckf_vio_tum.launch | 4 ++-- src/msckf_vio.cpp | 5 ++++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 0240de3..b3a66e5 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -17,8 +17,8 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 30ee2e2..d70591a 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -405,7 +405,7 @@ void MsckfVio::imageCallback( const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) { - // stop playing bagfile if printing images + // stop playing bagfile if printing images if(STREAMPAUSE) nh.setParam("/play_bag", false); // Return if the gravity vector has not been set. @@ -1454,6 +1454,7 @@ void MsckfVio::twodotFeatureJacobian( std::cout << "resume playback" << std::endl; nh.setParam("/play_bag", true); } + return; } @@ -2341,7 +2342,9 @@ void MsckfVio::removeLostFeatures() { cout << "measuring" << endl; PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); + cout << "norm" << endl; featureJacobian(feature.id, cam_state_ids, H_xj, r_j); + cout << "two" << endl; twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); cout << "gating" << endl; From 53e2a5d5245b278c56b899fc96b9fbdd14678cd1 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 28 Jun 2019 10:52:11 +0200 Subject: [PATCH 08/19] added ocvis output for multiple filter --- config/camchain-imucam-tum.yaml | 4 +- include/msckf_vio/feature.hpp | 8 +--- launch/msckf_vio_tum.launch | 4 +- src/msckf_vio.cpp | 81 +++++++++++++++++++++++++-------- 4 files changed, 68 insertions(+), 29 deletions(-) diff --git a/config/camchain-imucam-tum.yaml b/config/camchain-imucam-tum.yaml index 7edbd87..141035e 100644 --- a/config/camchain-imucam-tum.yaml +++ b/config/camchain-imucam-tum.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756, -0.001662284667857643] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506] resolution: [1024, 1024] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326, -0.002347858896562788] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407] resolution: [1024, 1024] rostopic: /cam1/image_raw diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index d558460..435f22f 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -220,9 +220,7 @@ bool VisualizeKernel( const StateIDType& cam_state_id, CameraCalibration& cam0, const Eigen::VectorXd& photo_r, - std::stringstream& ss, - cv::Point2f gradientVector, - cv::Point2f residualVector) const; + std::stringstream& ss) const; /* * @brief AnchorPixelToPosition uses the calcualted pixels * of the anchor patch to generate 3D positions of all of em @@ -720,9 +718,7 @@ bool Feature::VisualizePatch( const StateIDType& cam_state_id, CameraCalibration& cam0, const Eigen::VectorXd& photo_r, - std::stringstream& ss, - cv::Point2f gradientVector, - cv::Point2f residualVector) const + std::stringstream& ss) const { double rescale = 1; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index b3a66e5..e8294ef 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,11 +18,11 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index d70591a..ff4c24b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1528,13 +1528,6 @@ void MsckfVio::PhotometricMeasurementJacobian( 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) { //cout << "____feature-measurement_____\n" << endl; @@ -1633,7 +1626,6 @@ void MsckfVio::PhotometricMeasurementJacobian( H_x = H_xl; H_y = H_yl; - //TODO make this more fluent as well r = r_photo; std::stringstream ss; @@ -1641,7 +1633,7 @@ void MsckfVio::PhotometricMeasurementJacobian( if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); + feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } @@ -1747,13 +1739,18 @@ void MsckfVio::PhotometricFeatureJacobian( << "# columns: " << H_yi.cols() << "\n" << H_yi << endl; - myfile << "# name: r\n" << "# type: matrix\n" << "# rows: " << r_i.rows() << "\n" << "# columns: " << 1 << "\n" << r_i << endl; + myfile << "# name: r\n" + << "# type: A\n" + << "# rows: " << A_null_space.rows() << "\n" + << "# columns: " << A_null_space.cols() << "\n" + << A_null_space << endl; + myfile.close(); cout << "---------- LOGGED -------- " << endl; cvWaitKey(0); @@ -1902,7 +1899,34 @@ void MsckfVio::featureJacobian( myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; myfile.close(); - cout << "---------- LOGGED -------- " << endl; + + myfile.open("/home/raphael/dev/octave/org2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: Hx\n" + << "# type: matrix\n" + << "# rows: " << H_xj.rows() << "\n" + << "# columns: " << H_xj.cols() << "\n" + << H_xj << endl; + + myfile << "# name: Hy\n" + << "# type: matrix\n" + << "# rows: " << H_fj.rows() << "\n" + << "# columns: " << H_fj.cols() << "\n" + << H_fj << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r_j.rows() << "\n" + << "# columns: " << 1 << "\n" + << r_j << endl; + + myfile << "# name: r\n" + << "# type: A\n" + << "# rows: " << A.rows() << "\n" + << "# columns: " << A.cols() << "\n" + << A << endl; + + myfile.close(); } return; @@ -2019,8 +2043,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // 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(); @@ -2038,10 +2061,17 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { 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 { + //HouseholderQR qr_helper(H); + //MatrixXd Q = qr_helper.householderQ(); + //MatrixXd Q1 = Q.leftCols(21+state_server.cam_states.size()*6); + + //H_thin = Q1.transpose() * H; + //r_thin = Q1.transpose() * r; + } else {*/ H_thin = H; r_thin = r; - } + //} + // Compute the Kalman gain. @@ -2518,10 +2548,15 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); + cout << "measuring" << endl; + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + cout << "norm" << endl; + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + cout << "two" << endl; + twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); + + cout << "gating" << endl; 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; @@ -2689,7 +2724,15 @@ void MsckfVio::pruneCamStateBuffer() { } if (involved_cam_state_ids.size() == 0) continue; - + cout << "measuring" << endl; + + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + cout << "norm" << endl; + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + cout << "two" << endl; + twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); + + cout << "gating" << endl; PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); From 7b7e9662174370b83dc7c327f7e4b6f44f23ce3f Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 28 Jun 2019 12:13:02 +0200 Subject: [PATCH 09/19] added ground truth visualization --- include/msckf_vio/msckf_vio.h | 1 + launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 42 ++++++++++++++++++++++++++--------- 3 files changed, 33 insertions(+), 12 deletions(-) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 49af3cd..f8e7d62 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -328,6 +328,7 @@ class MsckfVio { // Subscribers and publishers ros::Subscriber imu_sub; ros::Subscriber truth_sub; + ros::Publisher truth_odom_pub; ros::Publisher odom_pub; ros::Publisher marker_pub; ros::Publisher feature_pub; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index e8294ef..b7e9365 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -22,7 +22,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index ff4c24b..4dacfce 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -261,6 +261,7 @@ bool MsckfVio::createRosIO() { nh.setParam("/play_bag", true); odom_pub = nh.advertise("odom", 10); + truth_odom_pub = nh.advertise("true_odom", 10); feature_pub = nh.advertise( "feature_point_cloud", 10); marker_pub = nh.advertise("/visualization_marker_array", 10); @@ -351,11 +352,6 @@ void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg){ void MsckfVio::truthCallback(const geometry_msgs::TransformStampedPtr& msg){ static bool first_truth_odom_msg = true; - // errorstate - /*if(not ErrorState) - return; - */ - // If this is the first mocap odometry messsage, set // the initial frame. if (first_truth_odom_msg) { @@ -1429,7 +1425,7 @@ void MsckfVio::twodotFeatureJacobian( { ofstream myfile; - myfile.open("/home/raphael/dev/octave/log2octave"); + myfile.open("/home/raphael/dev/octave/two2octave"); myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" << "# name: Hx\n" << "# type: matrix\n" @@ -1450,6 +1446,13 @@ void MsckfVio::twodotFeatureJacobian( << "# columns: " << 1 << "\n" << r_i << endl; + + myfile << "# name: A\n" + << "# type: matrix\n" + << "# rows: " << A_null_space.rows() << "\n" + << "# columns: " << 1 << "\n" + << A_null_space << endl; + myfile.close(); std::cout << "resume playback" << std::endl; nh.setParam("/play_bag", true); @@ -1633,7 +1636,7 @@ void MsckfVio::PhotometricMeasurementJacobian( if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); + //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } @@ -1745,8 +1748,8 @@ void MsckfVio::PhotometricFeatureJacobian( << "# columns: " << 1 << "\n" << r_i << endl; - myfile << "# name: r\n" - << "# type: A\n" + myfile << "# name: A\n" + << "# type: matrix\n" << "# rows: " << A_null_space.rows() << "\n" << "# columns: " << A_null_space.cols() << "\n" << A_null_space << endl; @@ -1920,8 +1923,8 @@ void MsckfVio::featureJacobian( << "# columns: " << 1 << "\n" << r_j << endl; - myfile << "# name: r\n" - << "# type: A\n" + myfile << "# name: A\n" + << "# type: matrix\n" << "# rows: " << A.rows() << "\n" << "# columns: " << A.cols() << "\n" << A << endl; @@ -2915,6 +2918,23 @@ void MsckfVio::publish(const ros::Time& time) { T_i_c_tf, time, child_frame_id, "camera")); } + // Publish true odometry + // Ground truth odometry. + nav_msgs::Odometry truth_odom_msg; + truth_odom_msg.header.stamp = time; + truth_odom_msg.header.frame_id = fixed_frame_id; + truth_odom_msg.child_frame_id = child_frame_id+"_true"; + + Eigen::Isometry3d true_T_i_w = Eigen::Isometry3d::Identity(); + true_T_i_w.linear() = quaternionToRotation( + true_state_server.imu_state.orientation).transpose(); + true_T_i_w.translation() = true_state_server.imu_state.position; + tf::poseEigenToMsg(true_T_i_w, truth_odom_msg.pose.pose); + Eigen::Isometry3d true_T_b_w = IMUState::T_imu_body * true_T_i_w * + IMUState::T_imu_body.inverse(); + + tf::poseEigenToMsg(true_T_b_w, truth_odom_msg.pose.pose); + truth_odom_pub.publish(truth_odom_msg); // Publish the odometry nav_msgs::Odometry odom_msg; odom_msg.header.stamp = time; From d013a1b080cdbd4a0443b8d432f2ae2751f04a08 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 28 Jun 2019 17:47:47 +0200 Subject: [PATCH 10/19] added multiple couts for testing, not working --- include/msckf_vio/feature.hpp | 4 +- include/msckf_vio/msckf_vio.h | 9 +- launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 250 ++++++++++++++++++++++++++-------- 4 files changed, 203 insertions(+), 62 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 435f22f..b818dc6 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -1024,8 +1024,8 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3); cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3); - xderImage/=8; - yderImage/=8; + xderImage/=8.; + yderImage/=8.; cv::convertScaleAbs(xderImage, abs_xderImage); cv::convertScaleAbs(yderImage, abs_yderImage); diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index f8e7d62..995c5a3 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -212,7 +212,7 @@ class MsckfVio { const FeatureIDType& feature_id, Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r); - void PhotometricMeasurementJacobian( + bool PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, Eigen::MatrixXd& H_x, @@ -224,7 +224,7 @@ class MsckfVio { const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); - void PhotometricFeatureJacobian( + bool PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); @@ -263,6 +263,11 @@ class MsckfVio { // Chi squared test table. static std::map chi_squared_test_table; + + // change in position + Eigen::Vector3d delta_position; + Eigen::Vector3d delta_orientation; + // State vector StateServer state_server; StateServer photometric_state_server; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index b7e9365..b3a66e5 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,7 +18,7 @@ output="screen"> - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 4dacfce..f9178c2 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -412,8 +412,6 @@ void MsckfVio::imageCallback( return; } - - // Start the system if the first image is received. // The frame where the first image is received will be // the origin. @@ -433,10 +431,6 @@ void MsckfVio::imageCallback( batchImuProcessing(feature_msg->header.stamp.toSec()); - // save true state in seperate state vector - - batchTruthProcessing(feature_msg->header.stamp.toSec()); - if(GROUNDTRUTH) { state_server.imu_state.position = true_state_server.imu_state.position; @@ -489,6 +483,10 @@ void MsckfVio::imageCallback( double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); + // save true state in seperate state vector and calculate relative error in change + batchTruthProcessing(feature_msg->header.stamp.toSec()); + + cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); @@ -808,6 +806,8 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { // Counter how many IMU msgs in the buffer are used. int used_truth_msg_cntr = 0; + const IMUState old_true_state = true_state_server.imu_state; + for (const auto& truth_msg : truth_msg_buffer) { double truth_time = truth_msg.header.stamp.toSec(); if (truth_time < true_state_server.imu_state.time) { @@ -839,6 +839,22 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { truth_msg_buffer.erase(truth_msg_buffer.begin(), truth_msg_buffer.begin()+used_truth_msg_cntr); + // calculate error in position + + // calculate error in change + + // calculate change + Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - old_true_state.position; + Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), old_true_state.orientation); + Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2); + Eigen::Vector3d error_delta_position = delta_true_position - delta_position; + Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation - delta_orientation; + + double dx = (error_delta_position[0]/delta_true_position[0]); + double dy = (error_delta_position[1]/delta_true_position[1]); + double dz = (error_delta_position[2]/delta_true_position[2]); + cout << "relative pos error: " << sqrt(dx*dx + dy*dy + dz*dz) * 100 << "%" << endl; + } void MsckfVio::processTruthtoIMU(const double& time, @@ -1463,7 +1479,7 @@ void MsckfVio::twodotFeatureJacobian( -void MsckfVio::PhotometricMeasurementJacobian( +bool MsckfVio::PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) @@ -1531,6 +1547,7 @@ void MsckfVio::PhotometricMeasurementJacobian( int count = 0; double dx, dy; + cout << "patching" << endl; for (auto point : feature.anchorPatch_3d) { //cout << "____feature-measurement_____\n" << endl; @@ -1538,6 +1555,11 @@ void MsckfVio::PhotometricMeasurementJacobian( cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); + if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) + { + cout << "skipped" << endl; + return false; + } //add observation photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); @@ -1597,8 +1619,9 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } + cout << "done" << 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); + MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1); // set anchor Jakobi // get position of anchor in cam states @@ -1616,15 +1639,18 @@ void MsckfVio::PhotometricMeasurementJacobian( 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); + // 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); + 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++) + + /*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_yl.block(0, N*N, N*N, 1) = -H_rho; H_x = H_xl; H_y = H_yl; @@ -1636,15 +1662,16 @@ void MsckfVio::PhotometricMeasurementJacobian( if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); + feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } - return; + cout << "returning" << endl; + return true; } -void MsckfVio::PhotometricFeatureJacobian( +bool MsckfVio::PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) @@ -1669,7 +1696,7 @@ void MsckfVio::PhotometricFeatureJacobian( 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, N*N+1); VectorXd r_i = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; @@ -1679,7 +1706,8 @@ void MsckfVio::PhotometricFeatureJacobian( MatrixXd H_yl; Eigen::VectorXd r_l = VectorXd::Zero(N*N); - PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l); + if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) + continue; auto cam_state_iter = state_server.cam_states.find(cam_id); int cam_state_cntr = std::distance( @@ -1691,7 +1719,11 @@ void MsckfVio::PhotometricFeatureJacobian( r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; } - + if(stack_cntr == 0) + { + cout << "skip feature" << endl; + return false; + } // Project the residual and Jacobians onto the nullspace // of H_yj. @@ -1754,12 +1786,20 @@ void MsckfVio::PhotometricFeatureJacobian( << "# columns: " << A_null_space.cols() << "\n" << A_null_space << endl; + myfile << "# name: P\n" + << "# type: matrix\n" + << "# rows: " << state_server.state_cov.rows() << "\n" + << "# columns: " << state_server.state_cov.rows() << "\n" + << state_server.state_cov << endl; + + myfile.close(); cout << "---------- LOGGED -------- " << endl; - cvWaitKey(0); } - return; + cout << "donefeature" << endl; + + return true; } void MsckfVio::measurementJacobian( @@ -1929,6 +1969,13 @@ void MsckfVio::featureJacobian( << "# columns: " << A.cols() << "\n" << A << endl; + myfile << "# name: P\n" + << "# type: matrix\n" + << "# rows: " << state_server.state_cov.rows() << "\n" + << "# columns: " << state_server.state_cov.rows() << "\n" + << state_server.state_cov << endl; + + myfile.close(); } @@ -1970,19 +2017,50 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& 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 << "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(FILTER != 0) return; + + if(PRINTIMAGES) + { + //octave + ofstream myfile; + + myfile.open("/home/raphael/dev/octave/measurement2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: K\n" + << "# type: matrix\n" + << "# rows: " << K.rows() << "\n" + << "# columns: " << K.cols() << "\n" + << K << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << r.cols() << "\n" + << r << endl; + + myfile.close(); + } + 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]); + + // Update the IMU state. const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -2046,7 +2124,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // complexity as in Equation (28), (29). MatrixXd H_thin; VectorXd r_thin; -/* + if (H.rows() > H.cols()) { // Convert H to a sparse matrix. SparseMatrix H_sparse = H.sparseView(); @@ -2070,10 +2148,10 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { //H_thin = Q1.transpose() * H; //r_thin = Q1.transpose() * r; - } else {*/ + } else { H_thin = H; r_thin = r; - //} + } @@ -2093,6 +2171,9 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // Update the IMU state. if (FILTER != 2) return; + 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]); + const VectorXd& delta_x_imu = delta_x.head<21>(); if (//delta_x_imu.segment<3>(0).norm() > 0.15 || @@ -2196,6 +2277,33 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r // Update the IMU state. if (FILTER != 1) return; + cout << "here" << endl; + + if(PRINTIMAGES) + { + //octave + ofstream myfile; + + myfile.open("/home/raphael/dev/octave/measurement2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: K\n" + << "# type: matrix\n" + << "# rows: " << K.rows() << "\n" + << "# columns: " << K.cols() << "\n" + << K << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << r.cols() << "\n" + << r << endl; + + myfile.close(); + } + + 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]); + const VectorXd& delta_x_imu = delta_x.head<21>(); if (//delta_x_imu.segment<3>(0).norm() > 0.15 || @@ -2374,8 +2482,18 @@ void MsckfVio::removeLostFeatures() { cout << "measuring" << endl; - PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); - cout << "norm" << endl; + if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true); + { + cout << "p gating" << endl; + + 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(); + } + } + + cout << "donephoto" << endl; featureJacobian(feature.id, cam_state_ids, H_xj, r_j); cout << "two" << endl; twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2387,11 +2505,6 @@ void MsckfVio::removeLostFeatures() { 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(); - } if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; @@ -2403,17 +2516,23 @@ void MsckfVio::removeLostFeatures() { //if (stack_cntr > 1500) break; } + if(pstack_cntr) + { + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + + photometricMeasurementUpdate(pH_x, pr); + } + cout << "resizing" << 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); + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); // Perform the measurement update step. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); twoMeasurementUpdate(twoH_x, twor); // Remove all processed features from the map. @@ -2553,7 +2672,18 @@ void MsckfVio::pruneLastCamStateBuffer() cout << "measuring" << endl; - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true); + { + cout << "p gating" << endl; + + 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(); + } + } + + cout << "norm" << endl; featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); cout << "two" << endl; @@ -2566,11 +2696,7 @@ void MsckfVio::pruneLastCamStateBuffer() 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(); - } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; @@ -2580,12 +2706,17 @@ void MsckfVio::pruneLastCamStateBuffer() feature.observations.erase(cam_id); } + if (pstack_cntr) + { + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + + photometricMeasurementUpdate(pH_x, pr); + } + 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. twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); @@ -2594,7 +2725,6 @@ void MsckfVio::pruneLastCamStateBuffer() measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); twoMeasurementUpdate(twoH_x, twor); //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), @@ -2729,28 +2859,28 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; cout << "measuring" << endl; - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) + { + cout << "p gating" << endl; + 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(); + } + } cout << "norm" << endl; featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); cout << "two" << endl; twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); cout << "gating" << endl; - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_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(); - } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; @@ -2762,16 +2892,22 @@ void MsckfVio::pruneCamStateBuffer() { } + if(pstack_cntr > 0) + { + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + + photometricMeasurementUpdate(pH_x, pr); + } + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - pH_x.conservativeResize(pstack_cntr, pH_x.cols()); - pr.conservativeResize(pstack_cntr); + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); // Perform measurement update. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); twoMeasurementUpdate(twoH_x, twor); //reduction for (const auto& cam_id : rm_cam_state_ids) { From 58fe9916477aa5ebd283b0e7eaad53f8f4843cc7 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 28 Jun 2019 18:21:36 +0200 Subject: [PATCH 11/19] removed comments, reg. msckf not working currently --- config/camchain-imucam-tum.yaml | 4 +-- launch/msckf_vio_tum.launch | 4 +-- src/msckf_vio.cpp | 44 +++++---------------------------- 3 files changed, 10 insertions(+), 42 deletions(-) diff --git a/config/camchain-imucam-tum.yaml b/config/camchain-imucam-tum.yaml index 141035e..7edbd87 100644 --- a/config/camchain-imucam-tum.yaml +++ b/config/camchain-imucam-tum.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756, -0.001662284667857643] - distortion_model: pre-equidistant + distortion_model: equidistant intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506] resolution: [1024, 1024] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326, -0.002347858896562788] - distortion_model: pre-equidistant + distortion_model: equidistant intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407] resolution: [1024, 1024] rostopic: /cam1/image_raw diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index b3a66e5..a8432b2 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index f9178c2..22b5f4b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1547,7 +1547,6 @@ bool MsckfVio::PhotometricMeasurementJacobian( int count = 0; double dx, dy; - cout << "patching" << endl; for (auto point : feature.anchorPatch_3d) { //cout << "____feature-measurement_____\n" << endl; @@ -1666,7 +1665,6 @@ bool MsckfVio::PhotometricMeasurementJacobian( //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } - cout << "returning" << endl; return true; } @@ -1797,8 +1795,6 @@ bool MsckfVio::PhotometricFeatureJacobian( cout << "---------- LOGGED -------- " << endl; } - cout << "donefeature" << endl; - return true; } @@ -2166,8 +2162,6 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // Compute the error of the state. VectorXd delta_x = K * r; - cout << "two rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; - cout << "two update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. if (FILTER != 2) return; @@ -2272,13 +2266,9 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r // 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 (FILTER != 1) return; - cout << "here" << endl; - if(PRINTIMAGES) { //octave @@ -2438,9 +2428,6 @@ void MsckfVio::removeLostFeatures() { // processed_feature_ids.size() << endl; //cout << "jacobian row #: " << jacobian_row_size << endl; - - cout << "sizing" << endl; - // Remove the features that do not have enough measurements. for (const auto& feature_id : invalid_feature_ids) map_server.erase(feature_id); @@ -2479,27 +2466,19 @@ void MsckfVio::removeLostFeatures() { MatrixXd twoH_xj; VectorXd twor_j; - - cout << "measuring" << endl; - + /* if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true); { - cout << "p gating" << endl; - 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(); } - } + }*/ - cout << "donephoto" << endl; featureJacobian(feature.id, cam_state_ids, H_xj, r_j); - cout << "two" << endl; twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); - cout << "gating" << endl; - 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; @@ -2524,7 +2503,6 @@ void MsckfVio::removeLostFeatures() { photometricMeasurementUpdate(pH_x, pr); } - cout << "resizing" << endl; H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2670,26 +2648,21 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - cout << "measuring" << endl; - + /* if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true); { - cout << "p gating" << endl; 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(); } - } + }*/ - cout << "norm" << endl; featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - cout << "two" << endl; twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); - cout << "gating" << endl; 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; @@ -2857,23 +2830,18 @@ void MsckfVio::pruneCamStateBuffer() { } if (involved_cam_state_ids.size() == 0) continue; - cout << "measuring" << endl; + /* if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { - cout << "p gating" << endl; 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(); } - } - cout << "norm" << endl; + }*/ featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - cout << "two" << endl; twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); - - cout << "gating" << endl; 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; From 737c23f32ae92350d365b7a955bc807e34c1c3f2 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Tue, 2 Jul 2019 08:32:56 +0200 Subject: [PATCH 12/19] restructured calcualtion of patches in code --- include/msckf_vio/msckf_vio.h | 61 +++- launch/msckf_vio_tinytum.launch | 4 +- src/image_processor.cpp | 4 +- src/msckf_vio.cpp | 534 +++++++++++++++++++------------- 4 files changed, 365 insertions(+), 238 deletions(-) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 995c5a3..250dfac 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -202,32 +202,56 @@ class MsckfVio { Eigen::Vector4d& r); // This function computes the Jacobian of all measurements viewed // in the given camera states of this feature. - void featureJacobian(const FeatureIDType& feature_id, + void featureJacobian( + const FeatureIDType& feature_id, const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); void twodotMeasurementJacobian( - const StateIDType& cam_state_id, - const FeatureIDType& feature_id, - Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r); + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r); + + bool ConstructJacobians( + Eigen::MatrixXd& H_rho, + Eigen::MatrixXd& H_pl, + Eigen::MatrixXd& H_pA, + const Feature& feature, + const StateIDType& cam_state_id, + Eigen::MatrixXd& H_xl, + Eigen::MatrixXd& H_yl); + + bool PhotometricPatchPointResidual( + const StateIDType& cam_state_id, + const Feature& feature, + Eigen::VectorXd& r); + + bool PhotometricPatchPointJacobian( + const CAMState& cam_state, + const Feature& feature, + Eigen::Vector3d point, + int count, + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj); bool PhotometricMeasurementJacobian( - const StateIDType& cam_state_id, - const FeatureIDType& feature_id, - Eigen::MatrixXd& H_x, - Eigen::MatrixXd& H_y, - Eigen::VectorXd& r); + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Eigen::MatrixXd& H_x, + Eigen::MatrixXd& H_y, + Eigen::VectorXd& r); void twodotFeatureJacobian( - const FeatureIDType& feature_id, - const std::vector& cam_state_ids, - Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + Eigen::MatrixXd& H_x, Eigen::VectorXd& r); bool PhotometricFeatureJacobian( - const FeatureIDType& feature_id, - const std::vector& cam_state_ids, - Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + const FeatureIDType& feature_id, + 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, @@ -263,6 +287,13 @@ class MsckfVio { // Chi squared test table. static std::map chi_squared_test_table; + double eval_time; + + IMUState timed_old_imu_state; + IMUState timed_old_true_state; + + IMUState old_imu_state; + IMUState old_true_state; // change in position Eigen::Vector3d delta_position; diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 28ae944..89112e1 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/image_processor.cpp b/src/image_processor.cpp index 168eebc..6867743 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -223,8 +223,8 @@ void ImageProcessor::stereoCallback( image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); - ROS_INFO("Publishing: %f", - (ros::Time::now()-start_time).toSec()); + //ROS_INFO("Publishing: %f", + // (ros::Time::now()-start_time).toSec()); // Build the image pyramids once since they're used at multiple places createImagePyramids(); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 22b5f4b..51a56b0 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -260,6 +260,7 @@ bool MsckfVio::createRosIO() { // activating bag playing parameter, for debugging nh.setParam("/play_bag", true); + eval_time = 0; odom_pub = nh.advertise("odom", 10); truth_odom_pub = nh.advertise("true_odom", 10); feature_pub = nh.advertise( @@ -412,6 +413,10 @@ void MsckfVio::imageCallback( return; } + old_imu_state = state_server.imu_state; + old_true_state = true_state_server.imu_state; + + // Start the system if the first image is received. // The frame where the first image is received will be // the origin. @@ -419,7 +424,6 @@ void MsckfVio::imageCallback( is_first_img = false; state_server.imu_state.time = feature_msg->header.stamp.toSec(); } - static double max_processing_time = 0.0; static int critical_time_cntr = 0; double processing_start_time = ros::Time::now().toSec(); @@ -427,24 +431,14 @@ void MsckfVio::imageCallback( // Propogate the IMU state. // that are received before the image feature_msg. ros::Time start_time = ros::Time::now(); - - - batchImuProcessing(feature_msg->header.stamp.toSec()); - - if(GROUNDTRUTH) - { - state_server.imu_state.position = true_state_server.imu_state.position; - state_server.imu_state.orientation = true_state_server.imu_state.orientation; - state_server.imu_state.position_null = true_state_server.imu_state.position_null; - state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null; - state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0; - state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu; - } + nh.param("FILTER", FILTER, 0); + + batchImuProcessing(feature_msg->header.stamp.toSec()); + double imu_processing_time = ( ros::Time::now()-start_time).toSec(); - cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); @@ -453,7 +447,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - cout << "2" << endl; + // cout << "2" << endl; // Add new observations for existing features or new // features in the map server. start_time = ros::Time::now(); @@ -462,7 +456,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - cout << "3" << endl; + // cout << "3" << endl; // Add new images to moving window start_time = ros::Time::now(); manageMovingWindow(cam0_img, cam1_img, feature_msg); @@ -470,14 +464,14 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - cout << "4" << endl; + // cout << "4" << endl; // Perform measurement update if necessary. start_time = ros::Time::now(); removeLostFeatures(); double remove_lost_features_time = ( ros::Time::now()-start_time).toSec(); - cout << "5" << endl; + // cout << "5" << endl; start_time = ros::Time::now(); pruneLastCamStateBuffer(); double prune_cam_states_time = ( @@ -487,7 +481,7 @@ void MsckfVio::imageCallback( batchTruthProcessing(feature_msg->header.stamp.toSec()); - cout << "6" << endl; + // cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); publish(feature_msg->header.stamp); @@ -504,18 +498,18 @@ void MsckfVio::imageCallback( ++critical_time_cntr; ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", processing_time, critical_time_cntr); - printf("IMU processing time: %f/%f\n", - imu_processing_time, imu_processing_time/processing_time); - printf("State augmentation time: %f/%f\n", - state_augmentation_time, state_augmentation_time/processing_time); - printf("Add observations time: %f/%f\n", - add_observations_time, add_observations_time/processing_time); - printf("Remove lost features time: %f/%f\n", - remove_lost_features_time, remove_lost_features_time/processing_time); - printf("Remove camera states time: %f/%f\n", - prune_cam_states_time, prune_cam_states_time/processing_time); - printf("Publish time: %f/%f\n", - publish_time, publish_time/processing_time); + //printf("IMU processing time: %f/%f\n", + // imu_processing_time, imu_processing_time/processing_time); + //printf("State augmentation time: %f/%f\n", + // state_augmentation_time, state_augmentation_time/processing_time); + //printf("Add observations time: %f/%f\n", + // add_observations_time, add_observations_time/processing_time); + //printf("Remove lost features time: %f/%f\n", + // remove_lost_features_time, remove_lost_features_time/processing_time); + //printf("Remove camera states time: %f/%f\n", + // prune_cam_states_time, prune_cam_states_time/processing_time); + //printf("Publish time: %f/%f\n", + // publish_time, publish_time/processing_time); } if(STREAMPAUSE) @@ -806,8 +800,6 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { // Counter how many IMU msgs in the buffer are used. int used_truth_msg_cntr = 0; - const IMUState old_true_state = true_state_server.imu_state; - for (const auto& truth_msg : truth_msg_buffer) { double truth_time = truth_msg.header.stamp.toSec(); if (truth_time < true_state_server.imu_state.time) { @@ -839,22 +831,76 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { truth_msg_buffer.erase(truth_msg_buffer.begin(), truth_msg_buffer.begin()+used_truth_msg_cntr); + /* + // calculate change + delta_position = state_server.imu_state.position - old_imu_state.position; + Eigen::Vector4d delta_orientation_quaternion = quaternionMultiplication(quaternionConjugate(state_server.imu_state.orientation), old_imu_state.orientation); + delta_orientation = Eigen::Vector3d(delta_orientation_quaternion[0]*2, delta_orientation_quaternion[1]*2, delta_orientation_quaternion[2]*2); // calculate error in position // calculate error in change - // calculate change Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - old_true_state.position; Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), old_true_state.orientation); Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2); Eigen::Vector3d error_delta_position = delta_true_position - delta_position; Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation - delta_orientation; - - double dx = (error_delta_position[0]/delta_true_position[0]); - double dy = (error_delta_position[1]/delta_true_position[1]); - double dz = (error_delta_position[2]/delta_true_position[2]); - cout << "relative pos error: " << sqrt(dx*dx + dy*dy + dz*dz) * 100 << "%" << endl; + Eigen::Vector3d error_position = true_state_server.imu_state.position - state_server.imu_state.position; + Eigen::Vector4d error_orientation_q = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), state_server.imu_state.orientation); + Eigen::Vector3d error_orientation = Eigen::Vector3d(error_orientation_q[0]*2, error_orientation_q[1]*2, error_orientation_q[2]*2); + + double relerr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + double relOerr = (sqrt(error_delta_orientation[0]*error_delta_orientation[0] + error_delta_orientation[1]*error_delta_orientation[1] + error_delta_orientation[2]*error_delta_orientation[2])/ + sqrt(delta_smallangle_true_orientation[0]*delta_smallangle_true_orientation[0] + delta_smallangle_true_orientation[1]*delta_smallangle_true_orientation[1] + delta_smallangle_true_orientation[2]*delta_smallangle_true_orientation[2])); + + double abserr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + + cout << "relative pos error: " << relerr * 100 << "%" << endl; + cout << "relative ori error: " << relOerr * 100 << "%" << endl; + //cout << "absolute pos error: " << + */ + if (eval_time + 1 < ros::Time::now().toSec()) + { + + // calculate change + delta_position = state_server.imu_state.position - timed_old_imu_state.position; + Eigen::Vector4d delta_orientation_quaternion = quaternionMultiplication(quaternionConjugate(state_server.imu_state.orientation), timed_old_imu_state.orientation); + delta_orientation = Eigen::Vector3d(delta_orientation_quaternion[0]*2, delta_orientation_quaternion[1]*2, delta_orientation_quaternion[2]*2); + // calculate error in position + + // calculate error in change + + Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - timed_old_true_state.position; + Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), timed_old_true_state.orientation); + Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2); + Eigen::Vector3d error_delta_position = delta_true_position - delta_position; + Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation - delta_orientation; + + Eigen::Vector3d error_position = true_state_server.imu_state.position - state_server.imu_state.position; + Eigen::Vector4d error_orientation_q = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), state_server.imu_state.orientation); + Eigen::Vector3d error_orientation = Eigen::Vector3d(error_orientation_q[0]*2, error_orientation_q[1]*2, error_orientation_q[2]*2); + + double relerr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + double relOerr = (sqrt(error_delta_orientation[0]*error_delta_orientation[0] + error_delta_orientation[1]*error_delta_orientation[1] + error_delta_orientation[2]*error_delta_orientation[2])/ + sqrt(delta_smallangle_true_orientation[0]*delta_smallangle_true_orientation[0] + delta_smallangle_true_orientation[1]*delta_smallangle_true_orientation[1] + delta_smallangle_true_orientation[2]*delta_smallangle_true_orientation[2])); + + double abserr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + // cout << "relative pos error: " << relerr * 100 << "%" << endl; + // cout << "relative ori error: " << relOerr * 100 << "%" << endl; + + timed_old_imu_state = state_server.imu_state; + timed_old_true_state = true_state_server.imu_state; + eval_time = ros::Time::now().toSec(); + } } void MsckfVio::processTruthtoIMU(const double& time, @@ -1477,17 +1523,60 @@ void MsckfVio::twodotFeatureJacobian( return; } - - -bool MsckfVio::PhotometricMeasurementJacobian( - const StateIDType& cam_state_id, - const FeatureIDType& feature_id, - MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) +bool MsckfVio::PhotometricPatchPointResidual( + const StateIDType& cam_state_id, + const Feature& feature, + VectorXd& r) { - // Prepare all the required data. + VectorXd r_photo = VectorXd::Zero(N*N); + int count = 0; + auto frame = cam0.moving_window.find(cam_state_id)->second.image; + const CAMState& cam_state = state_server.cam_states[cam_state_id]; - const Feature& feature = map_server[feature_id]; + + //estimate photometric measurement + std::vector estimate_irradiance; + std::vector estimate_photo_z; + std::vector photo_z; + + 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 * + estimated_illumination.frame_gain * estimated_illumination.feature_gain + + estimated_illumination.frame_bias + estimated_illumination.feature_bias); + + + for(auto point : feature.anchorPatch_3d) + { + + + cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); + // test if projection is inside frame + if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) + return false; + //add observation + photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); + + //calculate photom. residual + r_photo(count) = photo_z[count] - estimate_photo_z[count]; + count++; + } + r = r_photo; + return true; +} + +// generates the jacobian of one patch point regarding rho, anchor and current frame +bool MsckfVio::PhotometricPatchPointJacobian( + const CAMState& cam_state, + const Feature& feature, + Eigen::Vector3d point, + int count, + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj) +{ const StateIDType anchor_state_id = feature.observations.begin()->first; const CAMState anchor_state = state_server.cam_states[anchor_state_id]; @@ -1501,10 +1590,7 @@ bool MsckfVio::PhotometricMeasurementJacobian( Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0; Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation(); - - //photometric observation - std::vector photo_z; - VectorXd r_photo = VectorXd::Zero(N*N); + // individual Jacobians Matrix dI_dhj = Matrix::Zero(); Matrix dh_dCpij = Matrix::Zero(); @@ -1517,6 +1603,69 @@ bool MsckfVio::PhotometricMeasurementJacobian( Matrix dCpij_dCGtheta = Matrix::Zero(); Matrix dCpij_dGpC = Matrix::Zero(); + double dx, dy; + + Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); + cv::Point2f p_in_anchor = feature.projectPositionToCamera(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] + dx = feature.cvKernel(p_in_anchor, "Sobel_x"); + dy = feature.cvKernel(p_in_anchor, "Sobel_y"); + + dI_dhj(0, 0) = dx * cam0.intrinsics[0]; + dI_dhj(0, 1) = dy * cam0.intrinsics[1]; + + //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); + + //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; + + // 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(); + + // 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 + + return true; +} + +bool MsckfVio::PhotometricMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) +{ + + // Prepare all the required data. + const CAMState& cam_state = state_server.cam_states[cam_state_id]; + const Feature& feature = map_server[feature_id]; + + //photometric observation + VectorXd r_photo = VectorXd::Zero(N*N); + // one line of the NxN Jacobians Eigen::Matrix H_rhoj; Eigen::Matrix H_plj; @@ -1528,89 +1677,18 @@ bool MsckfVio::PhotometricMeasurementJacobian( Eigen::MatrixXd H_pA(N*N, 6); auto frame = cam0.moving_window.find(cam_state_id)->second.image; - auto anchor_frame = cam0.moving_window.find(anchor_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); + // calcualte residual of patch + PhotometricPatchPointResidual(cam_state_id, feature, r_photo); + // calculate jacobian for patch int count = 0; - double dx, dy; - for (auto point : feature.anchorPatch_3d) { - //cout << "____feature-measurement_____\n" << endl; - 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); - cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); - - if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) - { - cout << "skipped" << endl; - return false; - } - //add observation - photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); - - //calculate photom. residual - r_photo(count) = photo_z[count] - estimate_photo_z[count]; - //cout << "residual: " << photo_r.back() << endl; - - // calculate derivation for anchor frame, use position for derivation calculation - // frame derivative calculated convoluting with kernel [-1, 0, 1] - - dx = feature.cvKernel(p_in_anchor, "Sobel_x"); - dy = feature.cvKernel(p_in_anchor, "Sobel_y"); - - // dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); - // dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); - - dI_dhj(0, 0) = dx * cam0.intrinsics[0]; - dI_dhj(0, 1) = dy * cam0.intrinsics[1]; - - //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); - - //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; - - // 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(); - - // 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 - + // get jacobi of single point in patch + PhotometricPatchPointJacobian(cam_state, feature, point, count, H_rhoj, H_plj, H_pAj); + + // stack point into entire jacobi 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; @@ -1618,48 +1696,31 @@ bool MsckfVio::PhotometricMeasurementJacobian( count++; } - cout << "done" << endl; + // construct the jacobian structure needed for nullspacing MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1); - // set anchor Jakobi - // get position of anchor in cam states + ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); - 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; - - // set frame Jakobi - //get position of current frame in cam states - auto cam_state_iter = state_server.cam_states.find(cam_state_id); - 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, N*N, 1) = -H_rho; + // set to return values H_x = H_xl; H_y = H_yl; - r = r_photo; - std::stringstream ss; - ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; if(PRINTIMAGES) - { + { + // pregenerating information for visualization + std::stringstream ss; + 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); + //get position of current frame in cam states + auto cam_state_iter = state_server.cam_states.find(cam_state_id); + int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); + + ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; + + // visualizing functions feature.MarkerGeneration(marker_pub, state_server.cam_states); feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); @@ -1668,6 +1729,43 @@ bool MsckfVio::PhotometricMeasurementJacobian( return true; } +bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, + Eigen::MatrixXd& H_pl, + Eigen::MatrixXd& H_pA, + const Feature& feature, + const StateIDType& cam_state_id, + Eigen::MatrixXd& H_xl, + Eigen::MatrixXd& H_yl) +{ + // 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); + // set anchor Jakobi + H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA; + + //get position of current frame in cam states + auto cam_state_iter = state_server.cam_states.find(cam_state_id); + // set frame Jakobi + 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); + + + /*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, N*N, 1) = -H_rho; + +} + bool MsckfVio::PhotometricFeatureJacobian( const FeatureIDType& feature_id, @@ -1717,11 +1815,9 @@ bool MsckfVio::PhotometricFeatureJacobian( r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; } - if(stack_cntr == 0) - { - cout << "skip feature" << endl; + if(stack_cntr < 2*N*N) return false; - } + // Project the residual and Jacobians onto the nullspace // of H_yj. @@ -2027,8 +2123,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { VectorXd delta_x = K * r; - 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; + // 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; if(FILTER != 0) return; @@ -2053,9 +2149,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { myfile.close(); } - 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]); - // Update the IMU state. const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -2239,10 +2332,13 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r // Perform QR decompostion on H_sparse. SPQR > spqr_helper; spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); - spqr_helper.compute(H_sparse); + cout << "comp" << endl; + spqr_helper.compute(H_sparse); + cout << "done" << endl; MatrixXd H_temp; VectorXd r_temp; + (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp); (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp); @@ -2261,6 +2357,7 @@ void MsckfVio::photometricMeasurementUpdate(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); + MatrixXd K_transpose = S.ldlt().solve(H_thin*P); MatrixXd K = K_transpose.transpose(); // Compute the error of the state. @@ -2271,28 +2368,27 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r if(PRINTIMAGES) { - //octave - ofstream myfile; - - myfile.open("/home/raphael/dev/octave/measurement2octave"); - myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" - << "# name: K\n" - << "# type: matrix\n" - << "# rows: " << K.rows() << "\n" - << "# columns: " << K.cols() << "\n" - << K << endl; + //octave + ofstream myfile; + + myfile.open("/home/raphael/dev/octave/measurement2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: K\n" + << "# type: matrix\n" + << "# rows: " << K.rows() << "\n" + << "# columns: " << K.cols() << "\n" + << K << endl; - myfile << "# name: r\n" - << "# type: matrix\n" - << "# rows: " << r.rows() << "\n" - << "# columns: " << r.cols() << "\n" - << r << endl; + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << r.cols() << "\n" + << r << endl; - myfile.close(); + myfile.close(); } - 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]); + cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -2349,8 +2445,8 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - MatrixXd P1 = H * state_server.state_cov * H.transpose(); + MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P2 = Feature::observation_noise * MatrixXd::Identity(H.rows(), H.rows()); @@ -2363,10 +2459,10 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) if (chi_squared_test_table[dof] == 0) return false; if (gamma < chi_squared_test_table[dof]) { - //cout << "passed" << endl; + // cout << "passed" << endl; return true; } else { - //cout << "failed" << endl; + // cout << "failed" << endl; return false; } } @@ -2465,16 +2561,15 @@ void MsckfVio::removeLostFeatures() { VectorXd pr_j; MatrixXd twoH_xj; VectorXd twor_j; - - /* - if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true); + + if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true) { - if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + 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(); } - }*/ + } featureJacobian(feature.id, cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2499,10 +2594,11 @@ void MsckfVio::removeLostFeatures() { { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); - + photometricMeasurementUpdate(pH_x, pr); } + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2648,17 +2744,15 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - /* - if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true); - { + 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)) { 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(); } - }*/ - + } featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -2679,26 +2773,26 @@ void MsckfVio::pruneLastCamStateBuffer() feature.observations.erase(cam_id); } - if (pstack_cntr) + + if(pstack_cntr) { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); - photometricMeasurementUpdate(pH_x, pr); + photometricMeasurementUpdate(pH_x, pr); } H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - // Perform measurement update. - + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); - // Perform measurement update. - - + + // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); + //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -2831,7 +2925,7 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; - /* + 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())) { @@ -2839,7 +2933,8 @@ void MsckfVio::pruneCamStateBuffer() { pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - }*/ + } + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -2860,23 +2955,24 @@ void MsckfVio::pruneCamStateBuffer() { } - if(pstack_cntr > 0) + + if(pstack_cntr) { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); - pr.conservativeResize(pstack_cntr); - + pr.conservativeResize(pstack_cntr); photometricMeasurementUpdate(pH_x, pr); } H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); - - // Perform measurement update. + + // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); + //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), From 6bcc72f8265b8faa04f8b3d2c496ae6d71abada6 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Tue, 2 Jul 2019 16:58:03 +0200 Subject: [PATCH 13/19] changed to simple irradiance calcualation with derivation of image per frame - not working --- include/msckf_vio/feature.hpp | 45 +++++++++++ include/msckf_vio/msckf_vio.h | 4 +- launch/msckf_vio_tinytum.launch | 4 +- launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 132 +++++++++++++++++++++----------- 5 files changed, 137 insertions(+), 50 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index b818dc6..c45f6a0 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -184,6 +184,11 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, const CameraCalibration& cam, Eigen::Vector3d& in_p) const; +double CompleteCvKernel( + const cv::Point2f pose, + const cv::Mat& frame, + std::string type) const; + double cvKernel( const cv::Point2f pose, std::string type) const; @@ -488,6 +493,45 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const else return false; } +double Feature::CompleteCvKernel( + const cv::Point2f pose, + const cv::Mat& frame, + 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.; + else if (type == "Sobel_y") + delta = ((double)yder.at(pose.y, pose.x))/255.; + return delta; +} + double Feature::cvKernel( const cv::Point2f pose, std::string type) const @@ -823,6 +867,7 @@ bool Feature::VisualizePatch( // residual grid projection, positive - red, negative - blue colored namer.str(std::string()); namer << "residual"; + std::cout << "-- photo_r -- \n" << photo_r << " -- " << std::endl; cv::putText(irradianceFrame, namer.str() , cvPoint(30+scale*N, scale*N/2-5), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA); diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 250dfac..90a5750 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -229,12 +229,14 @@ class MsckfVio { bool PhotometricPatchPointJacobian( const CAMState& cam_state, + const StateIDType& cam_state_id, const Feature& feature, Eigen::Vector3d point, int count, Eigen::Matrix& H_rhoj, Eigen::Matrix& H_plj, - Eigen::Matrix& H_pAj); + Eigen::Matrix& H_pAj, + Eigen::Matrix& dI_dhj); bool PhotometricMeasurementJacobian( const StateIDType& cam_state_id, diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 89112e1..28ae944 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index a8432b2..b7e9365 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -25,7 +25,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 51a56b0..2ba011b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -496,8 +496,8 @@ void MsckfVio::imageCallback( processing_end_time - processing_start_time; if (processing_time > 1.0/frame_rate) { ++critical_time_cntr; - ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", - processing_time, critical_time_cntr); + //ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", + // processing_time, critical_time_cntr); //printf("IMU processing time: %f/%f\n", // imu_processing_time, imu_processing_time/processing_time); //printf("State augmentation time: %f/%f\n", @@ -1418,8 +1418,7 @@ void MsckfVio::twodotMeasurementJacobian( { std::stringstream ss; ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; - feature.MarkerGeneration(marker_pub, state_server.cam_states); - //feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss); + feature.MarkerGeneration(marker_pub, state_server.cam_states); } return; @@ -1540,27 +1539,40 @@ bool MsckfVio::PhotometricPatchPointResidual( std::vector estimate_photo_z; std::vector photo_z; + // estimate irradiance based on anchor frame 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 * - estimated_illumination.frame_gain * estimated_illumination.feature_gain + - estimated_illumination.frame_bias + estimated_illumination.feature_bias); + estimate_photo_z.push_back (estimate_irradiance_j);// * + //estimated_illumination.frame_gain * estimated_illumination.feature_gain + + //estimated_illumination.frame_bias + estimated_illumination.feature_bias); + + + // irradiance measurement around feature point + std::vector true_irradiance; + cv::Point2f p_f(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1)); + p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs); + cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; + for(int i = 0; i frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) return false; - //add observation + // add observation photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); - //calculate photom. residual - r_photo(count) = photo_z[count] - estimate_photo_z[count]; + // calculate photom. residual acc. to paper + // r_photo(count) = photo_z[count] - estimate_photo_z[count]; + + // calculate alternate photom. residual + r_photo(count) = true_irradiance[count] - photo_z[count]; count++; } r = r_photo; @@ -1570,12 +1582,14 @@ bool MsckfVio::PhotometricPatchPointResidual( // generates the jacobian of one patch point regarding rho, anchor and current frame bool MsckfVio::PhotometricPatchPointJacobian( const CAMState& cam_state, + const StateIDType& cam_state_id, const Feature& feature, Eigen::Vector3d point, int count, Eigen::Matrix& H_rhoj, Eigen::Matrix& H_plj, - Eigen::Matrix& H_pAj) + Eigen::Matrix& H_pAj, + Matrix& dI_dhj) { const StateIDType anchor_state_id = feature.observations.begin()->first; @@ -1592,7 +1606,7 @@ bool MsckfVio::PhotometricPatchPointJacobian( // individual Jacobians - Matrix dI_dhj = Matrix::Zero(); + /*Matrix */dI_dhj = Matrix::Zero(); Matrix dh_dCpij = Matrix::Zero(); Matrix dh_dGpij = Matrix::Zero(); Matrix dh_dXplj = Matrix::Zero(); @@ -1606,13 +1620,17 @@ bool MsckfVio::PhotometricPatchPointJacobian( double dx, dy; 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); cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); + auto frame = cam0.moving_window.find(cam_state_id)->second.image; + // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.cvKernel(p_in_anchor, "Sobel_x"); - dy = feature.cvKernel(p_in_anchor, "Sobel_y"); + dx = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_x"); + dy = feature.CompleteCvKernel(p_in_c0, frame, "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]; dI_dhj(0, 1) = dy * cam0.intrinsics[1]; @@ -1650,7 +1668,11 @@ bool MsckfVio::PhotometricPatchPointJacobian( H_plj = dI_dhj * dh_dXplj; // 1 x 6 H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 - return true; + // check if point nullspaceable + if (H_rhoj(0, 0) != 0) + return true; + + return false; } bool MsckfVio::PhotometricMeasurementJacobian( @@ -1671,6 +1693,8 @@ bool MsckfVio::PhotometricMeasurementJacobian( Eigen::Matrix H_plj; Eigen::Matrix H_pAj; + Eigen::MatrixXd dI_dh(N*N, 2); + // combined Jacobians Eigen::MatrixXd H_rho(N*N, 1); Eigen::MatrixXd H_pl(N*N, 6); @@ -1683,26 +1707,33 @@ bool MsckfVio::PhotometricMeasurementJacobian( // calculate jacobian for patch int count = 0; + bool valid = false; + Matrix dI_dhj;// = Matrix::Zero(); for (auto point : feature.anchorPatch_3d) { // get jacobi of single point in patch - PhotometricPatchPointJacobian(cam_state, feature, point, count, H_rhoj, H_plj, H_pAj); - + if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj)) + valid = true; + // stack point into entire jacobi 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; + dI_dh.block<1, 2>(count, 0) = dI_dhj; + count++; } + //Eigen::Matrix h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo; + //cout << "h photo: \n" << h_photo << endl; + // construct the jacobian structure needed for nullspacing MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1); - + MatrixXd H_yl = MatrixXd::Zero(N*N, 1); + ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); - // set to return values H_x = H_xl; H_y = H_yl; @@ -1726,9 +1757,13 @@ bool MsckfVio::PhotometricMeasurementJacobian( //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } - return true; + if(valid) + return true; + else + return false; } +// uses the calcualted jacobians to construct the final Hx Hy jacobians used for nullspacing bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, Eigen::MatrixXd& H_pl, Eigen::MatrixXd& H_pA, @@ -1741,7 +1776,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, 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); // set anchor Jakobi - 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, N*N, 6) = H_pA; //get position of current frame in cam states auto cam_state_iter = state_server.cam_states.find(cam_state_id); @@ -1749,21 +1784,20 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, 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; + 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); + //H_yl.block(0, 0, N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N); /*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, N*N, 1) = -H_rho; - + H_yl = H_rho; } @@ -1772,7 +1806,6 @@ bool MsckfVio::PhotometricFeatureJacobian( const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) { - const auto& feature = map_server[feature_id]; // Check how many camera states in the provided camera @@ -1792,7 +1825,7 @@ bool MsckfVio::PhotometricFeatureJacobian( 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+1); + MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); // CHANGED N*N+1 to 1 VectorXd r_i = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; @@ -1803,7 +1836,7 @@ bool MsckfVio::PhotometricFeatureJacobian( Eigen::VectorXd r_l = VectorXd::Zero(N*N); if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) - continue; + continue; auto cam_state_iter = state_server.cam_states.find(cam_id); int cam_state_cntr = std::distance( @@ -1815,19 +1848,25 @@ bool MsckfVio::PhotometricFeatureJacobian( r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; } - if(stack_cntr < 2*N*N) - return false; + // if not enough to propper nullspace (in paper implementation) + if(stack_cntr < N*N) + return false; // Project the residual and Jacobians onto the nullspace // of H_yj. // get Nullspace + + bool valid = false; + for(int i = 0; i < H_yi.rows(); i++) + if(H_yi(i,0) != 0) + valid = true; + FullPivLU lu(H_yi.transpose()); MatrixXd A_null_space = lu.kernel(); H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; - //cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; if(PRINTIMAGES) @@ -1956,6 +1995,7 @@ void MsckfVio::measurementJacobian( 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)); + // cout << "h reg: \n" << r(0) << "\n" << r(1) << endl; return; } @@ -2333,9 +2373,8 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r SPQR > spqr_helper; spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); - cout << "comp" << endl; spqr_helper.compute(H_sparse); - cout << "done" << endl; + MatrixXd H_temp; VectorXd r_temp; @@ -2364,6 +2403,9 @@ 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; + + if (FILTER != 1) return; if(PRINTIMAGES) @@ -2387,9 +2429,6 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r myfile.close(); } - - cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; - const VectorXd& delta_x_imu = delta_x.head<21>(); if (//delta_x_imu.segment<3>(0).norm() > 0.15 || @@ -2445,7 +2484,7 @@ void MsckfVio::photometricMeasurementUpdate(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(); MatrixXd P2 = Feature::observation_noise * @@ -2453,8 +2492,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; @@ -2562,7 +2601,7 @@ void MsckfVio::removeLostFeatures() { MatrixXd twoH_xj; VectorXd twor_j; - if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true) + 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)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; @@ -2747,7 +2786,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)) { + 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(); @@ -2769,6 +2808,7 @@ void MsckfVio::pruneLastCamStateBuffer() twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2928,7 +2968,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())) { + 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(); @@ -2937,7 +2977,7 @@ void MsckfVio::pruneCamStateBuffer() { featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_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; @@ -2949,7 +2989,7 @@ void MsckfVio::pruneCamStateBuffer() { twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } From 6ee756941c062d779763903a8a90070bd3b8b602 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Wed, 3 Jul 2019 16:11:23 +0200 Subject: [PATCH 14/19] 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(); From 3873c978dd29ac155e3fc623c6fd9d8b560ec97f Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Wed, 3 Jul 2019 17:48:54 +0200 Subject: [PATCH 15/19] added structure for stereo photometry - diverging --- include/msckf_vio/cam_state.h | 1 + include/msckf_vio/feature.hpp | 40 ++++-- include/msckf_vio/msckf_vio.h | 8 +- launch/msckf_vio_tinytum.launch | 4 +- src/msckf_vio.cpp | 222 ++++++++++++++++++-------------- 5 files changed, 161 insertions(+), 114 deletions(-) diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index 880c70c..9eda6f5 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -41,6 +41,7 @@ struct CameraCalibration{ cv::Vec4d distortion_coeffs; movingWindow moving_window; cv::Mat featureVisu; + int id; }; diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index e30df3c..a2a3d63 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -507,6 +507,7 @@ double Feature::CompleteCvKernel( delta = ((double)cam.moving_window.find(cam_state_id)->second.dximage.at(pose.y, pose.x))/255.; else if (type == "Sobel_y") delta = ((double)cam.moving_window.find(cam_state_id)->second.dyimage.at(pose.y, pose.x))/255.; + return delta; } @@ -990,28 +991,39 @@ cv::Point2f Feature::projectPositionToCamera( cv::Point2f out_p; cv::Point2f my_p; // transfrom position to camera frame + + // cam0 position Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); const Eigen::Vector3d& t_c0_w = cam_state.position; - Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); - out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); - // if(cam_state_id == observations.begin()->first) - //printf("undist:\n \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y); - + // project point according to model + if(cam.id == 0) + { + Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); + out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); + + } + // if camera is one, calcualte the cam1 position from cam0 position first + else if(cam.id == 1) + { + // cam1 position + Eigen::Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); + Eigen::Matrix3d R_w_c1 = R_c0_c1 * R_w_c0; + Eigen::Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation(); + + Eigen::Vector3d p_c1 = R_w_c1 * (in_p-t_c1_w); + out_p = cv::Point2f(p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); + } + + // undistort point according to camera model if (cam.distortion_model.substr(0,3) == "pre-") my_p = cv::Point2f(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]); else my_p = image_handler::distortPoint(out_p, - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs); - - - // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); - // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2)); - // printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y); - + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs); return my_p; } diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 90a5750..06bad0f 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -233,10 +233,10 @@ class MsckfVio { const Feature& feature, Eigen::Vector3d point, int count, - Eigen::Matrix& H_rhoj, - Eigen::Matrix& H_plj, - Eigen::Matrix& H_pAj, - Eigen::Matrix& dI_dhj); + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj, + Eigen::Matrix& dI_dhj); bool PhotometricMeasurementJacobian( const StateIDType& cam_state_id, diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index d4fbc31..28ae944 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 af56991..5e3dd8c 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -184,6 +184,8 @@ bool MsckfVio::loadParameters() { cam1.distortion_coeffs[2] = cam1_distortion_coeffs_temp[2]; cam1.distortion_coeffs[3] = cam1_distortion_coeffs_temp[3]; + cam0.id = 0; + cam1.id = 1; state_server.state_cov = MatrixXd::Zero(21, 21); @@ -551,8 +553,8 @@ void MsckfVio::manageMovingWindow( cv::Mat xder; cv::Mat yder; cv::Mat deeper_frame; - cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); + cam0_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.; @@ -561,6 +563,15 @@ void MsckfVio::manageMovingWindow( cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone(); + 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.; + + cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); + cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone(); + //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) @@ -1540,51 +1551,63 @@ bool MsckfVio::PhotometricPatchPointResidual( VectorXd& r) { - VectorXd r_photo = VectorXd::Zero(N*N); + VectorXd r_photo = VectorXd::Zero(2*N*N); int count = 0; - auto frame = cam0.moving_window.find(cam_state_id)->second.image; - const CAMState& cam_state = state_server.cam_states[cam_state_id]; //estimate photometric measurement std::vector estimate_irradiance; std::vector estimate_photo_z; - std::vector photo_z; + std::vector photo_z_c0, photo_z_c1; // estimate irradiance based on anchor frame + /* 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);// * //estimated_illumination.frame_gain * estimated_illumination.feature_gain + //estimated_illumination.frame_bias + estimated_illumination.feature_bias); + */ + // irradiance measurement around feature point in c0 and c1 + std::vector true_irradiance_c0, true_irradiance_c1; + cv::Point2f p_f_c0(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1)); + cv::Point2f p_f_c1(feature.observations.find(cam_state_id)->second(2), feature.observations.find(cam_state_id)->second(3)); - // irradiance measurement around feature point - std::vector true_irradiance; - cv::Point2f p_f(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1)); - p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs); - cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; - for(int i = 0; isecond.image; + cv::Mat current_image_c1 = cam1.moving_window.find(cam_state_id)->second.image; + for(int i = 0; i frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) + if(p_in_c0.x < 0 or p_in_c0.x > current_image_c0.cols-1 or p_in_c0.y < 0 or p_in_c0.y > current_image_c0.rows-1) + return false; + if(p_in_c1.x < 0 or p_in_c1.x > current_image_c1.cols-1 or p_in_c1.y < 0 or p_in_c1.y > current_image_c1.rows-1) return false; - // add observation - photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); + // add observation + photo_z_c0.push_back(feature.PixelIrradiance(p_in_c0, current_image_c0)); + photo_z_c1.push_back(feature.PixelIrradiance(p_in_c1, current_image_c1)); // calculate photom. residual acc. to paper // r_photo(count) = photo_z[count] - estimate_photo_z[count]; - // calculate alternate photom. residual - r_photo(count) = true_irradiance[count] - photo_z[count]; + // calculate photom. residual alternating between frames + r_photo(count*2) = true_irradiance_c0[count] - photo_z_c0[count]; + r_photo(count*2+1) = true_irradiance_c1[count] - photo_z_c1[count]; count++; } r = r_photo; @@ -1598,10 +1621,10 @@ bool MsckfVio::PhotometricPatchPointJacobian( const Feature& feature, Eigen::Vector3d point, int count, - Eigen::Matrix& H_rhoj, - Eigen::Matrix& H_plj, - Eigen::Matrix& H_pAj, - Matrix& dI_dhj) + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj, + Matrix& dI_dhj) { const StateIDType anchor_state_id = feature.observations.begin()->first; @@ -1618,67 +1641,82 @@ bool MsckfVio::PhotometricPatchPointJacobian( // individual Jacobians - /*Matrix */dI_dhj = Matrix::Zero(); - Matrix dh_dCpij = Matrix::Zero(); - Matrix dh_dGpij = Matrix::Zero(); - Matrix dh_dXplj = Matrix::Zero(); + /*Matrix */dI_dhj = 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(); - double dx, dy; + // irradiance jacobian + double dx_c0, dy_c0, dx_c1, dy_c1; Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); + Eigen::Vector3d p_c1 = R_w_c1 * (point-t_c1_w); + cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); + cv::Point2f p_in_c1 = feature.projectPositionToCamera(cam_state, cam_state_id, cam1, point); + cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); - - auto frame = cam0.moving_window.find(cam_state_id)->second.image; - // 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, 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; + dx_c0 = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_x"); + dy_c0 = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_y"); - dI_dhj(0, 0) = dx * cam0.intrinsics[0]; - dI_dhj(0, 1) = dy * cam0.intrinsics[1]; + dx_c1 = feature.CompleteCvKernel(p_in_c1, cam_state_id, cam1, "Sobel_x"); + dy_c1 = feature.CompleteCvKernel(p_in_c1, cam_state_id, cam1, "Sobel_y"); + + dI_dhj(0, 0) = dx_c0 * cam0.intrinsics[0]; + dI_dhj(0, 1) = dy_c0 * cam0.intrinsics[1]; + dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0]; + dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1]; + + cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; + + // add jacobian + + //dh / d{}^Cp_{ij} + 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)); //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_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)); - dCpij_dGpij = quaternionToRotation(cam_state.orientation); + dC0pij_dGpij = R_w_c0; + dC1pij_dGpij = R_c0_c1 * R_w_c0; - //orientation takes camera frame to world frame, we wa - dh_dGpij = dh_dCpij * dCpij_dGpij; + dC0pij_dXplj.leftCols(3) = skewSymmetric(p_c0); + dC0pij_dXplj.rightCols(3) = -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; // 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)); 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_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_rhoj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_drhoj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_drhoj; // 4 x 1 + H_plj = dI_dhj * dh_dC0pij * dC0pij_dXplj + dI_dhj * dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6 + H_pAj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6 // check if point nullspaceable if (H_rhoj(0, 0) != 0) @@ -1701,26 +1739,25 @@ bool MsckfVio::PhotometricMeasurementJacobian( VectorXd r_photo = VectorXd::Zero(N*N); // one line of the NxN Jacobians - Eigen::Matrix H_rhoj; - Eigen::Matrix H_plj; - Eigen::Matrix H_pAj; + Eigen::Matrix H_rhoj; + Eigen::Matrix H_plj; + Eigen::Matrix H_pAj; - Eigen::MatrixXd dI_dh(N*N, 2); + Eigen::MatrixXd dI_dh(2* N*N, 4); // combined Jacobians - Eigen::MatrixXd H_rho(N*N, 1); - Eigen::MatrixXd H_pl(N*N, 6); - Eigen::MatrixXd H_pA(N*N, 6); - - auto frame = cam0.moving_window.find(cam_state_id)->second.image; + Eigen::MatrixXd H_rho(2 * N*N, 1); + Eigen::MatrixXd H_pl(2 * N*N, 6); + Eigen::MatrixXd H_pA(2 * N*N, 6); // calcualte residual of patch PhotometricPatchPointResidual(cam_state_id, feature, r_photo); + cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0; bool valid = false; - Matrix dI_dhj;// = Matrix::Zero(); + Matrix dI_dhj;// = Matrix::Zero(); for (auto point : feature.anchorPatch_3d) { // get jacobi of single point in patch @@ -1728,11 +1765,11 @@ bool MsckfVio::PhotometricMeasurementJacobian( valid = true; // stack point into entire jacobi - 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; + H_rho.block<2, 1>(count*2, 0) = H_rhoj; + H_pl.block<2, 6>(count*2, 0) = H_plj; + H_pA.block<2, 6>(count*2, 0) = H_pAj; - dI_dh.block<1, 2>(count, 0) = dI_dhj; + dI_dh.block<2, 4>(count*2, 0) = dI_dhj; count++; } @@ -1741,8 +1778,8 @@ bool MsckfVio::PhotometricMeasurementJacobian( //cout << "h photo: \n" << h_photo << endl; // construct the jacobian structure needed for nullspacing - MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(N*N, 1); + MatrixXd H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7); + MatrixXd H_yl = MatrixXd::Zero(2*N*N, 1); ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); @@ -1788,7 +1825,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, 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); // set anchor Jakobi - 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*N*N, 6) = H_pA; //get position of current frame in cam states auto cam_state_iter = state_server.cam_states.find(cam_state_id); @@ -1796,7 +1833,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, 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; + H_xl.block(0, 21+cam_state_cntr*7, 2*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); @@ -1819,7 +1856,6 @@ bool MsckfVio::PhotometricFeatureJacobian( MatrixXd& H_x, VectorXd& r) { - return false; const auto& feature = map_server[feature_id]; // Check how many camera states in the provided camera @@ -1835,7 +1871,7 @@ bool MsckfVio::PhotometricFeatureJacobian( } int jacobian_row_size = 0; - jacobian_row_size = N * N * valid_cam_state_ids.size(); + jacobian_row_size = 2 * N * N * valid_cam_state_ids.size(); MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, 21+state_server.cam_states.size()*7); @@ -1847,7 +1883,7 @@ bool MsckfVio::PhotometricFeatureJacobian( MatrixXd H_xl; MatrixXd H_yl; - Eigen::VectorXd r_l = VectorXd::Zero(N*N); + Eigen::VectorXd r_l = VectorXd::Zero(2*N*N); if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) continue; @@ -1855,15 +1891,16 @@ bool MsckfVio::PhotometricFeatureJacobian( 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_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*N*N) = r_l; + stack_cntr += 2*N*N; } + // if not enough to propper nullspace (in paper implementation) - if(stack_cntr < N*N) + if(stack_cntr < 2*N*N) return false; // Project the residual and Jacobians onto the nullspace @@ -2263,10 +2300,7 @@ 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; @@ -2316,7 +2350,7 @@ 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; + //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]); @@ -2377,8 +2411,11 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { - if (H.rows() == 0 || r.rows() == 0) + 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; @@ -2423,7 +2460,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; @@ -2504,7 +2541,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 * @@ -2571,7 +2608,7 @@ void MsckfVio::removeLostFeatures() { } } - pjacobian_row_size += N*N*feature.observations.size(); + pjacobian_row_size += 2*N*N*feature.observations.size(); twojacobian_row_size += 4*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; @@ -2640,7 +2677,6 @@ 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(); @@ -2769,7 +2805,7 @@ void MsckfVio::pruneLastCamStateBuffer() } } - pjacobian_row_size += N*N*feature.observations.size(); + pjacobian_row_size += 2*N*N*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; twojacobian_row_size += 4*feature.observations.size(); @@ -2827,7 +2863,6 @@ 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(); @@ -2954,7 +2989,7 @@ void MsckfVio::pruneCamStateBuffer() { } twojacobian_row_size += 4*involved_cam_state_ids.size(); - pjacobian_row_size += N*N*involved_cam_state_ids.size(); + pjacobian_row_size += 2*N*N*involved_cam_state_ids.size(); jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } @@ -3010,7 +3045,6 @@ 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(); From 1a07ba3d3c85b5ce84506fb671119c5614128115 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 5 Jul 2019 09:43:35 +0200 Subject: [PATCH 16/19] added larger sobel filter in calculation - converges sometimes for a few seconds --- launch/msckf_vio_tinytum.launch | 4 +- launch/msckf_vio_tum.launch | 4 +- src/msckf_vio.cpp | 99 +++++++++++++++++++++++---------- 3 files changed, 73 insertions(+), 34 deletions(-) diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 28ae944..d5de69a 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -25,7 +25,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index b7e9365..7b1b86a 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 5e3dd8c..016ef8b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -50,7 +50,7 @@ Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity(); // Static member variables in Feature class. FeatureIDType Feature::next_id = 0; -double Feature::observation_noise = 0.01; +double Feature::observation_noise = 0.05; Feature::OptimizationConfig Feature::optimization_config; map MsckfVio::chi_squared_test_table; @@ -312,7 +312,7 @@ bool MsckfVio::initialize() { for (int i = 1; i < 1000; ++i) { boost::math::chi_squared chi_squared_dist(i); chi_squared_test_table[i] = - boost::math::quantile(chi_squared_dist, 0.05); + boost::math::quantile(chi_squared_dist, 0.1); } if (!createRosIO()) return false; @@ -555,19 +555,21 @@ void MsckfVio::manageMovingWindow( cv::Mat deeper_frame; cam0_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.; + cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); + xder/=72.; + yder/=72.; cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone(); 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.; + cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); + xder/=72.; + yder/=72.; + + cvWaitKey(0); cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone(); @@ -1679,7 +1681,7 @@ bool MsckfVio::PhotometricPatchPointJacobian( dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0]; dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1]; - cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; + //cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; // add jacobian @@ -1718,6 +1720,10 @@ bool MsckfVio::PhotometricPatchPointJacobian( H_plj = dI_dhj * dh_dC0pij * dC0pij_dXplj + dI_dhj * dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6 H_pAj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6 + // check if any direction not large enough for eval + if(dI_dhj(0, 0) < 0.01 or dI_dhj(0, 1) < 0.01 or dI_dhj(1, 2) < 0.01 or dI_dhj(1, 3) < 0.01) + return false; + // check if point nullspaceable if (H_rhoj(0, 0) != 0) return true; @@ -1751,9 +1757,11 @@ bool MsckfVio::PhotometricMeasurementJacobian( Eigen::MatrixXd H_pA(2 * N*N, 6); // calcualte residual of patch - PhotometricPatchPointResidual(cam_state_id, feature, r_photo); + if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) + return false; - cout << "r\n" << r_photo << endl; + + //cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0; bool valid = false; @@ -2446,7 +2454,6 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r r_thin = r; } - // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; MatrixXd S = H_thin*P*H_thin.transpose() + @@ -2536,12 +2543,13 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r 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) { - return true; + MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P2 = Feature::observation_noise * @@ -2549,12 +2557,40 @@ 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 << "r" << r << endl; + // cout << "procov" << P1+P2 << endl; + cout << "gate: " << dof << " " << gamma << " " << + chi_squared_test_table[dof] << endl; + + if(gamma > 1000000) + { + cout << " logging " << endl; + ofstream myfile; + myfile.open("/home/raphael/dev/octave/log2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: H\n" + << "# type: matrix\n" + << "# rows: " << H.rows() << "\n" + << "# columns: " << H.cols() << "\n" + << H << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << 1 << "\n" + << r << endl; + + myfile << "# name: C\n" + << "# type: matrix\n" + << "# rows: " << state_server.state_cov.rows() << "\n" + << "# columns: " << state_server.state_cov.cols() << "\n" + << state_server.state_cov << endl; + myfile.close(); + } if (chi_squared_test_table[dof] == 0) return false; - if (gamma < chi_squared_test_table[dof]) { + if (gamma < chi_squared_test_table[dof]*10) { // cout << "passed" << endl; return true; } else { @@ -2661,13 +2697,14 @@ 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; + 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(); } } + /* featureJacobian(feature.id, cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2681,6 +2718,7 @@ void MsckfVio::removeLostFeatures() { twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } + */ // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. @@ -2695,7 +2733,7 @@ void MsckfVio::removeLostFeatures() { photometricMeasurementUpdate(pH_x, pr); } - + /* H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2705,7 +2743,7 @@ void MsckfVio::removeLostFeatures() { // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - + */ // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) map_server.erase(feature_id); @@ -2845,13 +2883,13 @@ 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; + 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(); } } - + /* featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -2867,7 +2905,7 @@ void MsckfVio::pruneLastCamStateBuffer() twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - + */ for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2881,7 +2919,7 @@ void MsckfVio::pruneLastCamStateBuffer() photometricMeasurementUpdate(pH_x, pr); } - + /* H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2891,7 +2929,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - + */ //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -3024,17 +3062,17 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; - 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; + 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(); } } + /* featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -3049,7 +3087,7 @@ void MsckfVio::pruneCamStateBuffer() { twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - + */ for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -3063,6 +3101,7 @@ void MsckfVio::pruneCamStateBuffer() { photometricMeasurementUpdate(pH_x, pr); } + /* H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -3072,7 +3111,7 @@ void MsckfVio::pruneCamStateBuffer() { // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - + */ //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), From a7c296ca3d14b7b639032421fcb6b9fc18244cfe Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 5 Jul 2019 13:51:58 +0200 Subject: [PATCH 17/19] removed dx filter, corrected jacobi calculation with bigger sobel (and correct division), removed scale for mahalanobis --- include/msckf_vio/feature.hpp | 3 +- include/msckf_vio/msckf_vio.h | 2 +- launch/msckf_vio_euroc.launch | 4 +- src/msckf_vio.cpp | 70 +++++++++++++++++++++-------------- 4 files changed, 48 insertions(+), 31 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index a2a3d63..17e0ce1 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -736,6 +736,7 @@ bool Feature::VisualizeKernel( cvWaitKey(0); } + bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, @@ -934,7 +935,7 @@ bool Feature::VisualizePatch( //cv::imwrite(loc.str(), cam0.featureVisu); cv::imshow("patch", cam0.featureVisu); - cvWaitKey(0); + cvWaitKey(1); } float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 06bad0f..3ffc586 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -261,7 +261,7 @@ class MsckfVio { void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); bool gatingTest(const Eigen::MatrixXd& H, - const Eigen::VectorXd&r, const int& dof); + const Eigen::VectorXd&r, const int& dof, int filter=0); void removeLostFeatures(); void findRedundantCamStates( std::vector& rm_cam_state_ids); diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index b818735..b57704e 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 016ef8b..ef8f76e 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -312,7 +312,7 @@ bool MsckfVio::initialize() { for (int i = 1; i < 1000; ++i) { boost::math::chi_squared chi_squared_dist(i); chi_squared_test_table[i] = - boost::math::quantile(chi_squared_dist, 0.1); + boost::math::quantile(chi_squared_dist, 0.4); } if (!createRosIO()) return false; @@ -554,23 +554,34 @@ void MsckfVio::manageMovingWindow( cv::Mat yder; cv::Mat deeper_frame; + // generate derivative matrix for cam0 cam0_img_ptr->image.convertTo(deeper_frame,CV_16S); cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); - xder/=72.; - yder/=72.; + xder/=96.; + yder/=96.; + // save into moving window cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone(); + // generate derivative matrix for cam 1 cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); - xder/=72.; - yder/=72.; + xder/=96.; + yder/=96.; + + /* + cv::Mat norm_abs_xderImage; + cv::convertScaleAbs(xder, norm_abs_xderImage); + cv::normalize(norm_abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1); + + cv::imshow("xder", norm_abs_xderImage); cvWaitKey(0); - + */ + // save into moving window cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone(); @@ -1721,9 +1732,9 @@ bool MsckfVio::PhotometricPatchPointJacobian( H_pAj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6 // check if any direction not large enough for eval - if(dI_dhj(0, 0) < 0.01 or dI_dhj(0, 1) < 0.01 or dI_dhj(1, 2) < 0.01 or dI_dhj(1, 3) < 0.01) + /*if((dI_dhj(0, 0) < 0.1 or dI_dhj(0, 1) < 0.1) and (dI_dhj(1, 2) < 0.1 or dI_dhj(1, 3) < 0.1)) return false; - + */ // check if point nullspaceable if (H_rhoj(0, 0) != 0) return true; @@ -1766,11 +1777,15 @@ bool MsckfVio::PhotometricMeasurementJacobian( int count = 0; bool valid = false; Matrix dI_dhj;// = Matrix::Zero(); + int valid_count = 0; for (auto point : feature.anchorPatch_3d) { // get jacobi of single point in patch if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj)) + { + valid_count++; valid = true; + } // stack point into entire jacobi H_rho.block<2, 1>(count*2, 0) = H_rhoj; @@ -1781,7 +1796,7 @@ bool MsckfVio::PhotometricMeasurementJacobian( count++; } - + cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl; //Eigen::Matrix h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo; //cout << "h photo: \n" << h_photo << endl; @@ -2548,7 +2563,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r } -bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { +bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, int filter) { MatrixXd P1 = H * state_server.state_cov * H.transpose(); @@ -2559,6 +2574,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) // cout << "r" << r << endl; // cout << "procov" << P1+P2 << endl; + if(filter == 1) cout << "gate: " << dof << " " << gamma << " " << chi_squared_test_table[dof] << endl; @@ -2588,9 +2604,9 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) myfile.close(); } - if (chi_squared_test_table[dof] == 0) + if (chi_squared_test_table[dof] == 0) return false; - if (gamma < chi_squared_test_table[dof]*10) { + if (gamma < chi_squared_test_table[dof]) { // cout << "passed" << endl; return true; } else { @@ -2696,7 +2712,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)) { + if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, 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; @@ -2704,7 +2720,7 @@ void MsckfVio::removeLostFeatures() { } } - /* + featureJacobian(feature.id, cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2718,7 +2734,7 @@ void MsckfVio::removeLostFeatures() { twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - */ + // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. @@ -2733,7 +2749,7 @@ void MsckfVio::removeLostFeatures() { photometricMeasurementUpdate(pH_x, pr); } - /* + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2743,7 +2759,7 @@ void MsckfVio::removeLostFeatures() { // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - */ + // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) map_server.erase(feature_id); @@ -2882,14 +2898,14 @@ 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)) { + if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, 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(); } } - /* + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -2905,7 +2921,7 @@ void MsckfVio::pruneLastCamStateBuffer() twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - */ + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2919,7 +2935,7 @@ void MsckfVio::pruneLastCamStateBuffer() photometricMeasurementUpdate(pH_x, pr); } - /* + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2929,7 +2945,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - */ + //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -3064,7 +3080,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())) { + if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// 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; @@ -3072,7 +3088,7 @@ void MsckfVio::pruneCamStateBuffer() { } } - /* + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -3087,7 +3103,7 @@ void MsckfVio::pruneCamStateBuffer() { twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - */ + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -3101,7 +3117,7 @@ void MsckfVio::pruneCamStateBuffer() { photometricMeasurementUpdate(pH_x, pr); } - /* + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -3111,7 +3127,7 @@ void MsckfVio::pruneCamStateBuffer() { // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - */ + //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), From 1380ec426f049ef9d4de5c7bdbffb578e14ae7d1 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Tue, 9 Jul 2019 11:24:25 +0200 Subject: [PATCH 18/19] fixed minor error when not enough samples, edited feature ammount and patch size to make irradiance msckf more stable --- config/camchain-imucam-tum-scaled.yaml | 4 +- include/msckf_vio/feature.hpp | 10 +- launch/image_processor_tinytum.launch | 4 +- launch/msckf_vio_euroc.launch | 2 +- launch/msckf_vio_tinytum.launch | 4 +- src/msckf_vio.cpp | 132 +++++++++++++------------ 6 files changed, 79 insertions(+), 77 deletions(-) diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index f5ea7e8..4facacc 100644 --- a/config/camchain-imucam-tum-scaled.yaml +++ b/config/camchain-imucam-tum-scaled.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182] - distortion_model: pre-equidistant + distortion_model: equidistant intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] resolution: [512, 512] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] - distortion_model: pre-equidistant + distortion_model: equidistant intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] resolution: [512, 512] rostopic: /cam1/image_raw diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 17e0ce1..6c55861 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -208,7 +208,7 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, bool estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, std::vector& anchorPatch_estimate, IlluminationParameter& estimatedIllumination) const; @@ -549,7 +549,7 @@ return delta; bool Feature::estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, std::vector& anchorPatch_estimate, IlluminationParameter& estimated_illumination) const { @@ -558,11 +558,11 @@ bool Feature::estimate_FrameIrradiance( // muliply by a and add b of this frame auto anchor = observations.begin(); - if(cam0.moving_window.find(anchor->first) == cam0.moving_window.end()) + if(cam.moving_window.find(anchor->first) == cam.moving_window.end()) return false; - double anchorExposureTime_ms = cam0.moving_window.find(anchor->first)->second.exposureTime_ms; - double frameExposureTime_ms = cam0.moving_window.find(cam_state_id)->second.exposureTime_ms; + double anchorExposureTime_ms = cam.moving_window.find(anchor->first)->second.exposureTime_ms; + double frameExposureTime_ms = cam.moving_window.find(cam_state_id)->second.exposureTime_ms; double a_A = anchorExposureTime_ms; diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index 53dd99a..0e5985c 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -14,8 +14,8 @@ - - + + diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index b57704e..6237037 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -18,7 +18,7 @@ output="screen"> - + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index d5de69a..28ae944 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -25,7 +25,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index ef8f76e..172fda2 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -277,9 +277,9 @@ bool MsckfVio::createRosIO() { truth_sub = nh.subscribe("ground_truth", 100, &MsckfVio::truthCallback, this); - cam0_img_sub.subscribe(nh, "cam0_image", 10); - cam1_img_sub.subscribe(nh, "cam1_image", 10); - feature_sub.subscribe(nh, "features", 10); + cam0_img_sub.subscribe(nh, "cam0_image", 100); + cam1_img_sub.subscribe(nh, "cam1_image", 100); + feature_sub.subscribe(nh, "features", 100); image_sub.connectInput(cam0_img_sub, cam1_img_sub, feature_sub); image_sub.registerCallback(&MsckfVio::imageCallback, this); @@ -312,7 +312,7 @@ bool MsckfVio::initialize() { for (int i = 1; i < 1000; ++i) { boost::math::chi_squared chi_squared_dist(i); chi_squared_test_table[i] = - boost::math::quantile(chi_squared_dist, 0.4); + boost::math::quantile(chi_squared_dist, 0.05); } if (!createRosIO()) return false; @@ -703,7 +703,7 @@ bool MsckfVio::resetCallback( is_first_img = true; // Restart the subscribers. - imu_sub = nh.subscribe("imu", 100, + imu_sub = nh.subscribe("imu", 1000, &MsckfVio::imuCallback, this); truth_sub = nh.subscribe("ground_truth", 100, @@ -1563,25 +1563,28 @@ bool MsckfVio::PhotometricPatchPointResidual( const Feature& feature, VectorXd& r) { - VectorXd r_photo = VectorXd::Zero(2*N*N); int count = 0; const CAMState& cam_state = state_server.cam_states[cam_state_id]; //estimate photometric measurement std::vector estimate_irradiance; - std::vector estimate_photo_z; + IlluminationParameter estimated_illumination; + std::vector estimate_photo_z_c0, estimate_photo_z_c1; std::vector photo_z_c0, photo_z_c1; // estimate irradiance based on anchor frame - /* - 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_c0.push_back (estimate_irradiance_j);// * + //estimated_illumination.frame_gain * estimated_illumination.feature_gain + + //estimated_illumination.frame_bias + estimated_illumination.feature_bias); + feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam1, estimate_irradiance, estimated_illumination); + for (auto& estimate_irradiance_j : estimate_irradiance) + estimate_photo_z_c1.push_back (estimate_irradiance_j);// * //estimated_illumination.frame_gain * estimated_illumination.feature_gain + //estimated_illumination.frame_bias + estimated_illumination.feature_bias); - */ // irradiance measurement around feature point in c0 and c1 std::vector true_irradiance_c0, true_irradiance_c1; @@ -1615,9 +1618,11 @@ bool MsckfVio::PhotometricPatchPointResidual( // add observation photo_z_c0.push_back(feature.PixelIrradiance(p_in_c0, current_image_c0)); photo_z_c1.push_back(feature.PixelIrradiance(p_in_c1, current_image_c1)); + // calculate photom. residual acc. to paper - // r_photo(count) = photo_z[count] - estimate_photo_z[count]; - + //r_photo(count*2) = photo_z_c0[count] - estimate_photo_z_c0[count]; + //r_photo(count*2+1) = photo_z_c1[count] - estimate_photo_z_c1[count]; + // calculate photom. residual alternating between frames r_photo(count*2) = true_irradiance_c0[count] - photo_z_c0[count]; r_photo(count*2+1) = true_irradiance_c1[count] - photo_z_c1[count]; @@ -1747,13 +1752,12 @@ bool MsckfVio::PhotometricMeasurementJacobian( const FeatureIDType& feature_id, MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) { - // Prepare all the required data. const CAMState& cam_state = state_server.cam_states[cam_state_id]; const Feature& feature = map_server[feature_id]; //photometric observation - VectorXd r_photo = VectorXd::Zero(N*N); + VectorXd r_photo; // one line of the NxN Jacobians Eigen::Matrix H_rhoj; @@ -1771,7 +1775,6 @@ bool MsckfVio::PhotometricMeasurementJacobian( if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) return false; - //cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0; @@ -1796,13 +1799,13 @@ bool MsckfVio::PhotometricMeasurementJacobian( count++; } - cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl; - //Eigen::Matrix h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo; - //cout << "h photo: \n" << h_photo << endl; + // cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl; + // Eigen::Matrix h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo; + // cout << "h photo: \n" << h_photo << endl; // construct the jacobian structure needed for nullspacing - MatrixXd H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(2*N*N, 1); + MatrixXd H_xl; + MatrixXd H_yl; ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); @@ -1844,6 +1847,9 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, Eigen::MatrixXd& H_xl, Eigen::MatrixXd& H_yl) { + H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7); + H_yl = MatrixXd::Zero(2*N*N, 1); + // 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); @@ -1867,9 +1873,9 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, /*for(int i = 0; i< N*N; i++) H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; - */ + */ - H_yl = H_rho; + H_yl.block(0, 0, H_rho.rows(), H_rho.cols()) = H_rho; } @@ -1894,23 +1900,35 @@ bool MsckfVio::PhotometricFeatureJacobian( } int jacobian_row_size = 0; - jacobian_row_size = 2 * N * N * valid_cam_state_ids.size(); + + // stacked feature jacobians + MatrixXd H_xi; + MatrixXd H_yi; + VectorXd r_i; - MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, - 21+state_server.cam_states.size()*7); - MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); // CHANGED N*N+1 to 1 - VectorXd r_i = VectorXd::Zero(jacobian_row_size); + // temporary measurement jacobians + MatrixXd H_xl; + MatrixXd H_yl; + Eigen::VectorXd r_l; int stack_cntr = 0; + bool first = true; + // go through every valid state the feature was observed in for (const auto& cam_id : valid_cam_state_ids) { - MatrixXd H_xl; - MatrixXd H_yl; - Eigen::VectorXd r_l = VectorXd::Zero(2*N*N); - + // skip observation if measurement is not valid if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) continue; + // set size of stacking jacobians, once the returned jacobians are known + if(first) + { + first = not first; + jacobian_row_size = r_l.size() * valid_cam_state_ids.size(); + H_xi = MatrixXd::Zero(jacobian_row_size, H_xl.cols()); + H_yi = MatrixXd::Zero(jacobian_row_size, H_yl.cols()); // CHANGED N*N+1 to 1 + r_i = VectorXd::Zero(jacobian_row_size); + } 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); @@ -1918,12 +1936,12 @@ bool MsckfVio::PhotometricFeatureJacobian( // 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*N*N) = r_l; - stack_cntr += 2*N*N; + r_i.segment(stack_cntr, r_l.size()) = r_l; + stack_cntr += r_l.size(); } // if not enough to propper nullspace (in paper implementation) - if(stack_cntr < 2*N*N) + if(stack_cntr < r_l.size()) return false; // Project the residual and Jacobians onto the nullspace @@ -1936,6 +1954,7 @@ bool MsckfVio::PhotometricFeatureJacobian( if(H_yi(i,0) != 0) valid = true; + FullPivLU lu(H_yi.transpose()); MatrixXd A_null_space = lu.kernel(); @@ -2004,7 +2023,9 @@ bool MsckfVio::PhotometricFeatureJacobian( cout << "---------- LOGGED -------- " << endl; } - return true; + if(valid) + return true; + return false; } void MsckfVio::measurementJacobian( @@ -2435,10 +2456,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { if (H.rows() == 0 || r.rows() == 0) - { - cout << "zero" << endl; - return; - } + return; // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -2572,12 +2590,6 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - // cout << "r" << r << endl; - // cout << "procov" << P1+P2 << endl; - if(filter == 1) - cout << "gate: " << dof << " " << gamma << " " << - chi_squared_test_table[dof] << endl; - if(gamma > 1000000) { cout << " logging " << endl; @@ -2608,6 +2620,9 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, return false; if (gamma < chi_squared_test_table[dof]) { // cout << "passed" << endl; + if(filter == 1) + cout << "gate: " << dof << " " << gamma << " " << + chi_squared_test_table[dof] << endl; return true; } else { // cout << "failed" << endl; @@ -2713,13 +2728,11 @@ void MsckfVio::removeLostFeatures() { if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) { if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, 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(); } } - featureJacobian(feature.id, cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2735,7 +2748,6 @@ void MsckfVio::removeLostFeatures() { twostack_cntr += twoH_xj.rows(); } - // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. //if (stack_cntr > 1500) break; @@ -2745,11 +2757,8 @@ void MsckfVio::removeLostFeatures() { { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); - photometricMeasurementUpdate(pH_x, pr); } - - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2898,8 +2907,8 @@ 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(), 1)) { //, 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(); @@ -2921,27 +2930,24 @@ void MsckfVio::pruneLastCamStateBuffer() twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } - if(pstack_cntr) { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); - photometricMeasurementUpdate(pH_x, pr); } - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); - + // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); @@ -3081,17 +3087,14 @@ 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(), 1)) {// 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(); } } - - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_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; @@ -3104,11 +3107,11 @@ void MsckfVio::pruneCamStateBuffer() { twostack_cntr += twoH_xj.rows(); } + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); + } - - if(pstack_cntr) { @@ -3117,7 +3120,6 @@ void MsckfVio::pruneCamStateBuffer() { photometricMeasurementUpdate(pH_x, pr); } - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); From 02156bd89afd444071fb1b2a4ba01dd8db80c8c7 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 12 Jul 2019 14:01:11 +0200 Subject: [PATCH 19/19] changed visualization --- config/camchain-imucam-tum-scaled.yaml | 4 +-- include/msckf_vio/feature.hpp | 34 +++++++++++++------------- launch/image_processor_tinytum.launch | 10 ++++---- src/msckf_vio.cpp | 2 +- 4 files changed, 25 insertions(+), 25 deletions(-) diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index 4facacc..f5ea7e8 100644 --- a/config/camchain-imucam-tum-scaled.yaml +++ b/config/camchain-imucam-tum-scaled.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] resolution: [512, 512] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] resolution: [512, 512] rostopic: /cam1/image_raw diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 6c55861..8d5c9c1 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -224,7 +224,7 @@ bool VisualizeKernel( bool VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, const Eigen::VectorXd& photo_r, std::stringstream& ss) const; /* @@ -740,7 +740,7 @@ bool Feature::VisualizeKernel( bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, const Eigen::VectorXd& photo_r, std::stringstream& ss) const { @@ -749,7 +749,7 @@ bool Feature::VisualizePatch( //visu - anchor auto anchor = observations.begin(); - cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image; + cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image; cv::Mat dottedFrame(anchorImage.size(), CV_8UC3); cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB); @@ -761,10 +761,10 @@ bool Feature::VisualizePatch( cv::Point ys(point.x, point.y); cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255)); } - cam0.featureVisu = dottedFrame.clone(); + cam.featureVisu = dottedFrame.clone(); // visu - feature - cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; + cv::Mat current_image = cam.moving_window.find(cam_state_id)->second.image; cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB); // set position in frame @@ -772,7 +772,7 @@ bool Feature::VisualizePatch( std::vector projectionPatch; for(auto point : anchorPatch_3d) { - cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); + cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point); projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image)); // visu - feature cv::Point xs(p_in_c0.x, p_in_c0.y); @@ -780,7 +780,7 @@ bool Feature::VisualizePatch( cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0)); } - cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu); + cv::hconcat(cam.featureVisu, dottedFrame, cam.featureVisu); // patches visualization @@ -829,7 +829,7 @@ bool Feature::VisualizePatch( cv::Point2f p_f(observations.find(cam_state_id)->second(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); + p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs); for(int i = 0; i0) + if(photo_r(2*(i*N+j))>0) cv::rectangle(irradianceFrame, 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::Scalar(255 - photo_r(2*(i*N+j)+1)*255, 255 - photo_r(2*(i*N+j)+1)*255, 255), CV_FILLED); else cv::rectangle(irradianceFrame, 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::Scalar(255, 255 + photo_r(2*(i*N+j)+1)*255, 255 + photo_r(2*(i*N+j)+1)*255), CV_FILLED); // gradient arrow @@ -884,7 +884,7 @@ bool Feature::VisualizePatch( 3); */ - cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); + cv::hconcat(cam.featureVisu, irradianceFrame, cam.featureVisu); /* // visualize position of used observations and resulting feature position @@ -916,15 +916,15 @@ bool Feature::VisualizePatch( // draw, x y position and arrow with direction - write z next to it - cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale); + cv::resize(cam.featureVisu, cam.featureVisu, cv::Size(), rescale, rescale); - cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu); + cv::hconcat(cam.featureVisu, positionFrame, cam.featureVisu); */ // write feature position std::stringstream pos_s; pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1); - cv::putText(cam0.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), + cv::putText(cam.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA); // create line? @@ -932,9 +932,9 @@ bool Feature::VisualizePatch( std::stringstream loc; // loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; - //cv::imwrite(loc.str(), cam0.featureVisu); + //cv::imwrite(loc.str(), cam.featureVisu); - cv::imshow("patch", cam0.featureVisu); + cv::imshow("patch", cam.featureVisu); cvWaitKey(1); } diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index 0e5985c..f862755 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -14,8 +14,8 @@ - - + + @@ -24,9 +24,9 @@ - - - + + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 172fda2..00b2c99 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1828,7 +1828,7 @@ bool MsckfVio::PhotometricMeasurementJacobian( // visualizing functions feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); + feature.VisualizePatch(cam_state, cam_state_id, cam1, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); }