From 0ef71b9220e77e5e5dfedab3c3c83a3aa39707fe Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 12 Jul 2019 18:02:29 +0200 Subject: [PATCH] fixed anchor initialization and added image resizing for incomming into msckf --- include/msckf_vio/feature.hpp | 16 ++++++++++------ launch/msckf_vio_euroc.launch | 4 ++-- src/msckf_vio.cpp | 36 +++++++++++++++++++++++++---------- 3 files changed, 38 insertions(+), 18 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 44f2a9a..0014809 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -823,7 +823,11 @@ bool Feature::VisualizePatch( cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA); - cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1)); + cv::Point2f p_f; + if(cam.id == 0) + p_f = cv::Point2f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1)); + else if(cam.id == 1) + p_f = cv::Point2f(observations.find(cam_state_id)->second(2),observations.find(cam_state_id)->second(3)); // move to real pixels p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs); @@ -1042,8 +1046,9 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) 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); @@ -1066,17 +1071,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) // check if image has been pre-undistorted - if(cam.distortion_model.substr(0,3) == "pre-") + 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]); - // 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])); @@ -1112,7 +1116,7 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) 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)); diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index 8f59c97..9e8ef7a 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -19,11 +19,11 @@ - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index c9cb9e9..eb33e53 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -425,6 +425,10 @@ void MsckfVio::imageCallback( // the origin. if (is_first_img) { is_first_img = false; + cam0.intrinsics *=0.5; + cam1.intrinsics *=0.5; + cam0.resolution *=0.5; + cam1.resolution *=0.5; state_server.imu_state.time = feature_msg->header.stamp.toSec(); } static double max_processing_time = 0.0; @@ -546,12 +550,26 @@ void MsckfVio::manageMovingWindow( cv::Mat new_cam0; cv::Mat new_cam1; + cam0.intrinsics *=2; + cam1.intrinsics *=2; + cam0.resolution *=2; + cam1.resolution *=2; image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); image_handler::undistortImage(cam1_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + cam0.intrinsics *=0.5; + cam1.intrinsics *=0.5; + cam0.resolution *=0.5; + cam1.resolution *=0.5; + //resize + + cv::Mat new_cam02; + cv::Mat new_cam12; + cv::resize(new_cam0, new_cam02, cv::Size(), 0.5, 0.5); + cv::resize(new_cam1, new_cam12, cv::Size(), 0.5, 0.5); // save image information into moving window - cam0.moving_window[state_server.imu_state.id].image = new_cam0.clone(); - cam1.moving_window[state_server.imu_state.id].image = new_cam1.clone(); + cam0.moving_window[state_server.imu_state.id].image = new_cam02.clone(); + cam1.moving_window[state_server.imu_state.id].image = new_cam12.clone(); cv::Mat xder; cv::Mat yder; @@ -576,14 +594,15 @@ void MsckfVio::manageMovingWindow( 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(); @@ -1705,9 +1724,6 @@ 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(1, 2) << ", " << dI_dhj(1, 3) << endl; - // add jacobian //dh / d{}^Cp_{ij} @@ -1784,8 +1800,6 @@ bool MsckfVio::PhotometricMeasurementJacobian( if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) return false; - cout << "-\n" << r_photo << "-" << endl; - //cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0; @@ -1929,7 +1943,9 @@ bool MsckfVio::PhotometricFeatureJacobian( // skip observation if measurement is not valid if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) - continue; + { + continue; + } // set size of stacking jacobians, once the returned jacobians are known if(first)