raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added image_handler undistorted image

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-27 16:38:02 +02:00
parent 010db87e4b
commit 655416a490
2 changed files with 141 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

139
src/image_handler.cpp
View File

@@ -14,6 +14,34 @@ namespace msckf_vio {
namespace image_handler {
cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics)
{
return cv::Point2f(p_in.x * intrinsics[0] + intrinsics[2], p_in.y * intrinsics[1] + intrinsics[3]);
}
cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics)
{
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,
@@ -42,10 +70,36 @@ void undistortPoint(
if (distortion_model == "radtan") {
cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
rectification_matrix, K_new);
} else if (distortion_model == "equidistant") {
}
// equidistant
else if (distortion_model == "equidistant") {
cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
rectification_matrix, K_new);
} else {
}
// fov
else if (distortion_model == "fov") {
for(int i = 0; i < pts_in.size(); i++)
{
float omega = distortion_coeffs[0];
float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
float ru = tan(rd * omega)/(2 * tan(omega / 2));
cv::Point2f newPoint(
((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd),
((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd));
pts_out.push_back(newPoint);
}
}
else if (distortion_model == "pre-equidistant")
{
std::vector<cv::Point2f> temp_pts_out;
for(int i = 0; i < pts_in.size(); i++)
temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics));
pts_out = temp_pts_out;
}
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,
@@ -79,10 +133,35 @@ void undistortPoints(
if (distortion_model == "radtan") {
cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
rectification_matrix, K_new);
} else if (distortion_model == "equidistant") {
}
else if (distortion_model == "equidistant") {
cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
rectification_matrix, K_new);
} else {
}
else if (distortion_model == "fov") {
for(int i = 0; i < pts_in.size(); i++)
{
float omega = distortion_coeffs[0];
float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
float ru = tan(rd * omega)/(2 * tan(omega / 2));
cv::Point2f newPoint(
((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd),
((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd));
pts_out.push_back(newPoint);
}
}
else if (distortion_model == "pre-equidistant")
{
std::vector<cv::Point2f> temp_pts_out;
for(int i = 0; i < pts_in.size(); i++)
temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics));
pts_out = temp_pts_out;
}
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,
@@ -110,7 +189,31 @@ std::vector<cv::Point2f> distortPoints(
distortion_coeffs, pts_out);
} else if (distortion_model == "equidistant") {
cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
} else {
}
else if (distortion_model == "fov") {
for(int i = 0; i < pts_in.size(); i++)
{
// based on 'straight lines have to be straight'
float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
float omega = distortion_coeffs[0];
float rd = 1 / (omega)*atan(2*ru*tan(omega / 2));
cv::Point2f newPoint(
pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2],
pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]);
pts_out.push_back(newPoint);
}
}
else if (distortion_model == "pre-equidistant")
{
std::vector<cv::Point2f> temp_pts_out;
for(int i = 0; i < pts_in.size(); i++)
temp_pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics));
pts_out = temp_pts_out;
}
else {
ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
distortion_model.c_str());
std::vector<cv::Point3f> homogenous_pts;
@@ -143,7 +246,31 @@ cv::Point2f distortPoint(
distortion_coeffs, pts_out);
} else if (distortion_model == "equidistant") {
cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
} else {
}
else if (distortion_model == "fov") {
for(int i = 0; i < pts_in.size(); i++)
{
// based on 'straight lines have to be straight'
float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
float omega = distortion_coeffs[0];
float rd = 1 / (omega)*atan(2*ru*tan(omega / 2));
cv::Point2f newPoint(
pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2],
pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]);
pts_out.push_back(newPoint);
}
}
else if (distortion_model == "pre-equidistant")
{
std::vector<cv::Point2f> temp_pts_out;
for(int i = 0; i < pts_in.size(); i++)
pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics));
pts_out = temp_pts_out;
}
else {
ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
distortion_model.c_str());
std::vector<cv::Point3f> homogenous_pts;

6
src/image_processor.cpp
View File

@@ -219,6 +219,12 @@ void ImageProcessor::stereoCallback(
cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img,
sensor_msgs::image_encodings::MONO8);
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();