From b3e86b3e64daab8827cdd70130c153ab009a1039 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 15:57:24 +0200 Subject: [PATCH] changed structure for image undistort into image_handler --- include/msckf_vio/feature.hpp | 187 ++++++++---------------------- include/msckf_vio/image_handler.h | 7 ++ launch/msckf_vio_tum.launch | 6 +- src/image_handler.cpp | 18 +++ src/image_processor.cpp | 22 ++-- src/msckf_vio.cpp | 74 +++++++----- 6 files changed, 128 insertions(+), 186 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 54157e7..839f564 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -138,8 +138,6 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, const CamStateServer& cam_states) const; - bool initializeAnchorUndistort(const CameraCalibration& cam, int N); - /* * @brief InitializeAnchor generates the NxN patch around the * feature in the Anchor image @@ -174,13 +172,6 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, Eigen::Vector3d& in_p) const; -cv::Point2f projectPositionToCameraUndistorted( - const CAMState& cam_state, - const StateIDType& cam_state_id, - const CameraCalibration& cam, - Eigen::Vector3d& in_p) const; - - /* * @brief project PositionToCamera Takes a 3d position in a world frame * and projects it into the passed camera frame using pinhole projection @@ -757,7 +748,7 @@ bool Feature::VisualizePatch( std::vector projectionPatch; for(auto point : anchorPatch_3d) { - cv::Point2f p_in_c0 = projectPositionToCameraUndistorted(cam_state, cam_state_id, cam0, point); + cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image)); // visu - feature cv::Point xs(p_in_c0.x, p_in_c0.y); @@ -964,35 +955,6 @@ cv::Point2f Feature::pixelDistanceAt( return distance; } -cv::Point2f Feature::projectPositionToCameraUndistorted( - const CAMState& cam_state, - const StateIDType& cam_state_id, - const CameraCalibration& cam, - Eigen::Vector3d& in_p) const -{ - Eigen::Isometry3d T_c0_w; - - cv::Point2f out_p; - - // transfrom position to camera frame - 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); - - cv::Point2f my_p(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]); - - // 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); - - return my_p; -} - cv::Point2f Feature::projectPositionToCamera( const CAMState& cam_state, const StateIDType& cam_state_id, @@ -1009,15 +971,17 @@ cv::Point2f Feature::projectPositionToCamera( 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)); + cv::Point2f my_p; - // 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, + // test is prewarped or not + 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); @@ -1068,121 +1032,64 @@ 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)) - 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; -} - -//@test center projection must always be initial feature projection -bool Feature::initializeAnchorUndistort(const CameraCalibration& cam, int N) -{ - - //initialize patch Size - int n = (int)(N-1)/2; - - auto anchor = observations.begin(); - if(cam.moving_window.find(anchor->first) == cam.moving_window.end()) - return false; - - cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image; - cv::Mat anchorImage_deeper; - anchorImage.convertTo(anchorImage_deeper,CV_16S); - //TODO remove this? - - - cv::Mat hen = cv::Mat::zeros(cv::Size(3, 3), CV_16S); - hen.at(1,0) = 1; - - //sobel test - /* - cv::Mat newhen; - cv::Sobel(hen, newhen, -1, 1, 0, 3); - std::cout << "newhen" << newhen << std::endl; - */ - - cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3); - cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3); - - xderImage/=4; - yderImage/=4; - - cv::convertScaleAbs(xderImage, abs_xderImage); - cv::convertScaleAbs(yderImage, abs_yderImage); - - cvWaitKey(0); - - auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2]; - auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3]; - - //project onto pixel plane - undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]); - - // 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])); - - - // 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)-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; } diff --git a/include/msckf_vio/image_handler.h b/include/msckf_vio/image_handler.h index eae27c3..3cf2ed2 100644 --- a/include/msckf_vio/image_handler.h +++ b/include/msckf_vio/image_handler.h @@ -19,6 +19,13 @@ 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/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index a5b65c7..1c2c689 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,11 +18,11 @@ output="screen"> - + - - + + diff --git a/src/image_handler.cpp b/src/image_handler.cpp index bd2a699..324ffdb 100644 --- a/src/image_handler.cpp +++ b/src/image_handler.cpp @@ -24,6 +24,24 @@ cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsic 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, diff --git a/src/image_processor.cpp b/src/image_processor.cpp index da8494e..0e01957 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -215,8 +215,8 @@ void ImageProcessor::stereoCallback( const sensor_msgs::ImageConstPtr& cam1_img) { // stop playing bagfile if printing images - if(STREAMPAUSE) - nh.setParam("/play_bag_image", false); + //if(STREAMPAUSE) + // nh.setParam("/play_bag_image", false); // Get the current image. cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img, @@ -225,17 +225,13 @@ void ImageProcessor::stereoCallback( sensor_msgs::image_encodings::MONO8); - const cv::Matx33d K0(cam0.intrinsics[0], 0.0, cam0.intrinsics[2], - 0.0, cam0.intrinsics[1], cam0.intrinsics[3], - 0.0, 0.0, 1.0); - const cv::Matx33d K1(cam1.intrinsics[0], 0.0, cam1.intrinsics[2], - 0.0, cam1.intrinsics[1], cam1.intrinsics[3], - 0.0, 0.0, 1.0); + cv::Mat undistortedCam0; + cv::Mat undistortedCam1; - cv::fisheye::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, K0, cam0.distortion_coeffs, K0); - cv::fisheye::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, K1, cam1.distortion_coeffs, K1); - + //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(); @@ -306,8 +302,8 @@ void ImageProcessor::stereoCallback( } // stop playing bagfile if printing images - if(STREAMPAUSE) - nh.setParam("/play_bag_image", true); + //if(STREAMPAUSE) + // nh.setParam("/play_bag_image", true); return; } diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index e5f5b3f..9a31f42 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -450,7 +450,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()); @@ -554,17 +554,15 @@ void MsckfVio::manageMovingWindow( // undistort Images - - const cv::Matx33d K(cam0.intrinsics[0], 0.0, cam0.intrinsics[2], - 0.0, cam0.intrinsics[1], cam0.intrinsics[3], - 0.0, 0.0, 1.0); - cv::Mat undistortedCam0; - cv::fisheye::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, K, cam0.distortion_coeffs, K); - //cv::imshow("anchor", undistortedCam0); - //cvWaitKey(0); - cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone(); - cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone(); + //cv::Mat undistortedCam0; + //cv::Mat undistortedCam1; + + //image_handler::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + //image_handler::undistortImage(cam1.moving_window[state_server.imu_state.id].image, undistortedCam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + + //cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone(); + //cam1.moving_window[state_server.imu_state.id].image = undistortedCam1.clone(); //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) while(cam0.moving_window.size() > 100) @@ -1322,8 +1320,8 @@ bool MsckfVio::PhotometricMeasurementJacobian( //cout << "____feature-measurement_____\n" << endl; Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); - cv::Point2f p_in_c0 = feature.projectPositionToCameraUndistorted(cam_state, cam_state_id, cam0, point); - cv::Point2f p_in_anchor = feature.projectPositionToCameraUndistorted(anchor_state, anchor_state_id, cam0, point); + 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 || p_in_c0.x >= cam0.resolution(0) || p_in_c0.y < 0 || p_in_c0.y >= cam0.resolution(1) ) { @@ -1509,7 +1507,7 @@ void MsckfVio::PhotometricFeatureJacobian( H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; - //cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; + Eigen::MatrixXd xres = H_x.colPivHouseholderQr().solve(r); if(PRINTIMAGES) { @@ -1556,6 +1554,13 @@ void MsckfVio::PhotometricFeatureJacobian( << "# columns: " << 1 << "\n" << r_i << endl; + + myfile << "# name: xres\n" + << "# type: matrix\n" + << "# rows: " << xres.rows() << "\n" + << "# columns: " << xres.cols() << "\n" + << xres << endl; + myfile.close(); cout << "---------- LOGGED -------- " << endl; cvWaitKey(0); @@ -1693,9 +1698,10 @@ void MsckfVio::featureJacobian( FullPivLU lu(H_fj.transpose()); MatrixXd A = lu.kernel(); - H_x = A.transpose() * H_xj; - r = A.transpose() * r_j; + H_x = A.transpose() * H_xj; + r = A.transpose() * r_j; + Eigen::MatrixXd xres = H_x.colPivHouseholderQr().solve(r); // stop playing bagfile if printing images if(PRINTIMAGES) { @@ -1715,13 +1721,19 @@ void MsckfVio::featureJacobian( << "# 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: xres\n" + << "# type: matrix\n" + << "# rows: " << xres.rows() << "\n" + << "# columns: " << 1 << "\n" + << xres << endl; + myfile.close(); cout << "---------- LOGGED ORG -------- " << endl; } @@ -2002,7 +2014,7 @@ void MsckfVio::removeLostFeatures() { } if(!feature.is_anchored) { - if(!feature.initializeAnchorUndistort(cam0, N)) + if(!feature.initializeAnchor(cam0, N)) { invalid_feature_ids.push_back(feature.id); continue; @@ -2049,20 +2061,20 @@ void MsckfVio::removeLostFeatures() { MatrixXd pH_xj; VectorXd pr_j; - PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); + //PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j); 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; 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(); } - + */ // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. //if (stack_cntr > 1500) break; @@ -2075,7 +2087,7 @@ void MsckfVio::removeLostFeatures() { // Perform the measurement update step. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); + //photometricMeasurementUpdate(pH_x, pr); // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) @@ -2168,7 +2180,7 @@ void MsckfVio::pruneLastCamStateBuffer() } if(!feature.is_anchored) { - if(!feature.initializeAnchorUndistort(cam0, N)) + if(!feature.initializeAnchor(cam0, N)) { feature.observations.erase(rm_cam_state_id); continue; @@ -2205,7 +2217,7 @@ 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); + //PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { @@ -2214,11 +2226,13 @@ 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(); } + */ for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2231,7 +2245,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Perform measurement update. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); + //photometricMeasurementUpdate(pH_x, pr); //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), @@ -2320,7 +2334,7 @@ void MsckfVio::pruneCamStateBuffer() { } if(!feature.is_anchored) { - if(!feature.initializeAnchorUndistort(cam0, N)) + if(!feature.initializeAnchor(cam0, N)) { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); @@ -2358,7 +2372,7 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + //PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { @@ -2366,13 +2380,13 @@ void MsckfVio::pruneCamStateBuffer() { 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(); } - + */ for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2385,7 +2399,7 @@ void MsckfVio::pruneCamStateBuffer() { // Perform measurement update. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); + //photometricMeasurementUpdate(pH_x, pr); //reduction for (const auto& cam_id : rm_cam_state_ids) {