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) {