diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 5a756c2..8215cad 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -127,9 +127,7 @@ struct Feature { bool initializeAnchor( const movingWindow& cam0_moving_window, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs); + const CameraCalibration& cam); /* @@ -150,9 +148,7 @@ struct Feature { inline cv::Point2f projectPositionToCamera( const CAMState& cam_state, const StateIDType& cam_state_id, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs, + const CameraCalibration& cam, Eigen::Vector3d& in_p) const; /* @@ -164,9 +160,7 @@ struct Feature { bool IrradianceOfAnchorPatch( const CAMState& cam_state, const StateIDType& cam_state_id, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs, + const CameraCalibration& cam, const movingWindow& cam0_moving_window, std::vector& anchorPatch_measurement) const; @@ -174,11 +168,8 @@ struct Feature { * @brief projectPixelToPosition uses the calcualted pixels * of the anchor patch to generate 3D positions of all of em */ - inline bool projectPixelToPosition(cv::Point2f in_p, - Eigen::Vector3d& out_p, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs); +inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, + const CameraCalibration& cam); /* * @brief Irradiance returns irradiance value of a pixel @@ -362,16 +353,14 @@ bool Feature::checkMotion( bool Feature::IrradianceOfAnchorPatch( const CAMState& cam_state, const StateIDType& cam_state_id, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs, + const CameraCalibration& cam, const movingWindow& cam0_moving_window, std::vector& anchorPatch_measurement) const { //project every point in anchorPatch_3d. for (auto point : anchorPatch_3d) { - cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, intrinsics, distortion_model, distortion_coeffs, point); + cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point); uint8_t irradiance = Irradiance(p_in_c0 , cam0_moving_window.find(cam_state_id)->second); anchorPatch_measurement.push_back(irradiance); } @@ -385,9 +374,7 @@ uint8_t Feature::Irradiance(cv::Point2f pose, cv::Mat image) const cv::Point2f Feature::projectPositionToCamera( const CAMState& cam_state, const StateIDType& cam_state_id, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs, + const CameraCalibration& cam, Eigen::Vector3d& in_p) const { Eigen::Isometry3d T_c0_w; @@ -402,23 +389,17 @@ cv::Point2f Feature::projectPositionToCamera( out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); std::vector out_v; out_v.push_back(out_p); - std::vector my_p = image_handler::distortPoints( out_v, - intrinsics, - distortion_model, - distortion_coeffs); + std::vector my_p = image_handler::distortPoints(out_v, 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); - return out_p; + return my_p[0]; } -bool Feature::projectPixelToPosition(cv::Point2f in_p, - Eigen::Vector3d& out_p, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs) +Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, + const CameraCalibration& cam) { // use undistorted position of point of interest // project it back into 3D space using pinhole model @@ -426,7 +407,7 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p, Eigen::Vector3d PositionInCamera(in_p.x/rho, in_p.y/rho, 1/rho); Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation(); - anchorPatch_3d.push_back(PositionInWorld); + return PositionInWorld; //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); } @@ -434,9 +415,7 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p, //@test center projection must always be initial feature projection bool Feature::initializeAnchor( const movingWindow& cam0_moving_window, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs) + const CameraCalibration& cam) { int N = 3; @@ -447,8 +426,8 @@ bool Feature::initializeAnchor( return false; cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second; - auto u = anchor->second(0)*intrinsics[0] + intrinsics[2]; - auto v = anchor->second(1)*intrinsics[1] + intrinsics[3]; + auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2]; + auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3]; int count = 0; //go through surrounding pixels @@ -457,12 +436,9 @@ bool Feature::initializeAnchor( for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1) { anchorPatch.push_back(anchorImage.at((int)u_run,(int)v_run)); - Eigen::Vector3d Npose; - projectPixelToPosition(cv::Point2f((u_run-intrinsics[2])/intrinsics[0], (v_run-intrinsics[3])/intrinsics[1]), - Npose, - intrinsics, - distortion_model, - distortion_coeffs); + cv::Point2f currentPoint((u_run-cam.intrinsics[2])/cam.intrinsics[0], (v_run-cam.intrinsics[3])/cam.intrinsics[1]); + Eigen::Vector3d Npose = projectPixelToPosition(currentPoint, cam); + anchorPatch_3d.push_back(Npose); } } diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 589c2bc..095b71b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -904,8 +904,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //photometric observation std::vector photo_z; - feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs, cam0_moving_window, photo_z); - + feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z); // Convert the feature position from the world frame to // the cam0 and cam1 frame. @@ -1316,7 +1315,7 @@ void MsckfVio::removeLostFeatures() { } } - if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs)) + if(!feature.initializeAnchor(cam0_moving_window, cam0)) { invalid_feature_ids.push_back(feature.id); continue; @@ -1470,7 +1469,7 @@ void MsckfVio::pruneCamStateBuffer() { } } - if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs)) + if(!feature.initializeAnchor(cam0_moving_window, cam0)) { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id);