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

added fov camera model

Browse Source
This commit is contained in:
Raphael Maenle 2019-06-25 19:49:04 +02:00
parent 1d6100ed13
commit 273bbf8a94
1 changed files with 68 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
src/image_handler.cpp
View File

@ -45,7 +45,22 @@ void undistortPoint(
} 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 {
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 +94,26 @@ 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 {
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 +141,23 @@ 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 {
ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
distortion_model.c_str());
std::vector<cv::Point3f> homogenous_pts;
@ -143,7 +190,23 @@ 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 {
ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
distortion_model.c_str());
std::vector<cv::Point3f> homogenous_pts;