75 lines
2.5 KiB
C++
75 lines
2.5 KiB
C++
|
#include <msckf_vio/image_handler.h>
|
||
|
|
|
||
|
|
namespace msckf_vio {
|
||
|
|
namespace image_handler {
|
||
|
|
|
||
|
|
|
||
|
|
void undistortPoints(
|
||
|
|
const std::vector<cv::Point2f>& pts_in,
|
||
|
|
const cv::Vec4d& intrinsics,
|
||
|
|
const std::string& distortion_model,
|
||
|
|
const cv::Vec4d& distortion_coeffs,
|
||
|
|
std::vector<cv::Point2f>& pts_out,
|
||
|
|
const cv::Matx33d &rectification_matrix,
|
||
|
|
const cv::Vec4d &new_intrinsics) {
|
||
|
|
|
||
|
|
if (pts_in.size() == 0) return;
|
||
|
|
|
||
|
|
const cv::Matx33d K(
|
||
|
|
intrinsics[0], 0.0, intrinsics[2],
|
||
|
|
0.0, intrinsics[1], intrinsics[3],
|
||
|
|
0.0, 0.0, 1.0);
|
||
|
|
|
||
|
|
const cv::Matx33d K_new(
|
||
|
|
new_intrinsics[0], 0.0, new_intrinsics[2],
|
||
|
|
0.0, new_intrinsics[1], new_intrinsics[3],
|
||
|
|
0.0, 0.0, 1.0);
|
||
|
|
|
||
|
|
if (distortion_model == "radtan") {
|
||
|
|
cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
|
||
|
|
rectification_matrix, K_new);
|
||
|
|
} else if (distortion_model == "equidistant") {
|
||
|
|
cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
|
||
|
|
rectification_matrix, K_new);
|
||
|
|
} 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,
|
||
|
|
rectification_matrix, K_new);
|
||
|
|
}
|
||
|
|
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
std::vector<cv::Point2f> distortPoints(
|
||
|
|
const std::vector<cv::Point2f>& pts_in,
|
||
|
|
const cv::Vec4d& intrinsics,
|
||
|
|
const std::string& distortion_model,
|
||
|
|
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);
|
||
|
|
|
||
|
|
std::vector<cv::Point2f> pts_out;
|
||
|
|
if (distortion_model == "radtan") {
|
||
|
|
std::vector<cv::Point3f> homogenous_pts;
|
||
|
|
cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
|
||
|
|
cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
|
||
|
|
distortion_coeffs, pts_out);
|
||
|
|
} else if (distortion_model == "equidistant") {
|
||
|
|
cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
|
||
|
|
} else {
|
||
|
|
ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
|
||
|
|
distortion_model.c_str());
|
||
|
|
std::vector<cv::Point3f> homogenous_pts;
|
||
|
|
cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
|
||
|
|
cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
|
||
|
|
distortion_coeffs, pts_out);
|
||
|
|
}
|
||
|
|
|
||
|
|
return pts_out;
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace image_handler
|
||
|
|
} // namespace msckf_vio
|