Program Listing for File undistort-helpers.h¶
↰ Return to documentation for file (aslam_cv2/aslam_cv_common/include/aslam/common/undistort-helpers.h
)
#ifndef ASLAM_UNDISTORT_HELPERS_H_
#define ASLAM_UNDISTORT_HELPERS_H_
#include <algorithm>
#include <Eigen/Dense>
#include <glog/logging.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
// This file contains modified opencv routines which use aslam's distort and project functionality.
// Original functions can be found here:
// getUndistortRectangles: https://github.com/Itseez/opencv/blob/5f590ebed084a5002c9013e11c519dcb139d47e9/modules/calib3d/src/calibration.cpp#L2094
// getOptimalNewCameraMatrix: https://github.com/Itseez/opencv/blob/0ffc53bafea9052d1fac17e6fc6f0a07ddf9f789/modules/imgproc/src/undistort.cpp#L62
// buildUndistortMap: https://github.com/Itseez/opencv/blob/5f590ebed084a5002c9013e11c519dcb139d47e9/modules/calib3d/src/calibration.cpp#L3389
namespace aslam {
namespace common {
template<typename DerivedCameraType>
static void getUndistortRectangles(const DerivedCameraType& input_camera, bool undistort_to_pinhole,
cv::Rect_<float>& inner, cv::Rect_<float>& outer) {
const int N = 9;
int x, y, k;
cv::Ptr<CvMat> _pts(cvCreateMat(1, N * N, CV_32FC2));
CvPoint2D32f* pts = (CvPoint2D32f*) (_pts->data.ptr);
for (y = k = 0; y < N; y++) {
for (x = 0; x < N; x++) {
Eigen::Vector2d keypoint(x * input_camera.imageWidth()/ (N - 1),
y * input_camera.imageHeight() / (N - 1));
// Transform keypoint from image to normalized image plane. (incl. projection effects)
Eigen::Vector2d keypoint_normalized;
Eigen::Matrix3d camera_matrix;
switch(input_camera.getType()) {
case Camera::Type::kPinhole: {
const aslam::PinholeCamera *const cam =
dynamic_cast<const aslam::PinholeCamera*>(&input_camera);
CHECK(cam != nullptr);
camera_matrix = cam->getCameraMatrix();
break;
}
case Camera::Type::kUnifiedProjection: {
const aslam::UnifiedProjectionCamera *const cam =
dynamic_cast<const aslam::UnifiedProjectionCamera*>(&input_camera);
CHECK(cam != nullptr);
camera_matrix = cam->getCameraMatrix();
break;
}
default: {
LOG(FATAL) << "Unknown camera model: "
<< static_cast<std::underlying_type<Camera::Type>::type>(
input_camera.getType());
}
}
if (undistort_to_pinhole) {
// Transform keypoint from image to normalized image plane. (incl. projection effects)
Eigen::Vector3d point_3d;
input_camera.backProject3(keypoint, &point_3d);
point_3d /= point_3d[2];
keypoint_normalized[0] = point_3d[0];
keypoint_normalized[1] = point_3d[1];
} else {
// Transform keypoint from image to normalized image plane.
keypoint_normalized[0] = 1.0 / camera_matrix(0,0) * (keypoint[0] - camera_matrix(0,2));
keypoint_normalized[1] = 1.0 / camera_matrix(1,1) * (keypoint[1] - camera_matrix(1,2));
input_camera.getDistortion().undistort(&keypoint_normalized);
}
pts[k++] = cvPoint2D32f((float) keypoint_normalized[0], (float) keypoint_normalized[1]);
}
}
float iX0 = -FLT_MAX, iX1 = FLT_MAX, iY0 = -FLT_MAX, iY1 = FLT_MAX;
float oX0 = FLT_MAX, oX1 = -FLT_MAX, oY0 = FLT_MAX, oY1 = -FLT_MAX;
// find the inscribed rectangle.
// the code will likely not work with extreme rotation matrices (R) (>45%)
for (y = k = 0; y < N; y++)
{
for (x = 0; x < N; x++) {
CvPoint2D32f p = pts[k++];
oX0 = MIN(oX0, p.x);
oX1 = MAX(oX1, p.x);
oY0 = MIN(oY0, p.y);
oY1 = MAX(oY1, p.y);
if (x == 0) iX0 = std::max(iX0, p.x);
if (x == N - 1) iX1 = std::min(iX1, p.x);
if (y == 0) iY0 = std::max(iY0, p.y);
if (y == N - 1) iY1 = std::min(iY1, p.y);
}
}
inner = cv::Rect_<float>(iX0, iY0, iX1 - iX0, iY1 - iY0);
outer = cv::Rect_<float>(oX0, oY0, oX1 - oX0, oY1 - oY0);
}
template<typename DerivedCameraType>
Eigen::Matrix3d getOptimalNewCameraMatrix(const DerivedCameraType& input_camera,
double alpha, double scale,
bool undistort_to_pinhole) {
CHECK_GE(alpha, 0.0); CHECK_LE(alpha, 1.0);
CHECK_GT(scale, 0.0);
cv::Size output_size(static_cast<int>(scale * input_camera.imageWidth()),
static_cast<int>(scale * input_camera.imageHeight()));
// Get inscribed and circumscribed rectangles in normalized
cv::Rect_<float> inner, outer;
getUndistortRectangles(input_camera, undistort_to_pinhole, inner, outer);
// Projection mapping inner rectangle to viewport
double fx0 = (output_size.width - 1) / inner.width;
double fy0 = (output_size.height - 1) / inner.height;
double cx0 = -fx0 * inner.x;
double cy0 = -fy0 * inner.y;
// Projection mapping outer rectangle to viewport
double fx1 = (output_size.width - 1) / outer.width;
double fy1 = (output_size.height - 1) / outer.height;
double cx1 = -fx1 * outer.x;
double cy1 = -fy1 * outer.y;
// Interpolate between the two extremal projections
Eigen::Matrix3d output_camera_matrix = Eigen::Matrix3d::Zero();
output_camera_matrix(0, 0) = fx0 * (1.0 - alpha) + fx1 * alpha;
output_camera_matrix(1, 1) = fy0 * (1.0 - alpha) + fy1 * alpha;
output_camera_matrix(0, 2) = cx0 * (1.0 - alpha) + cx1 * alpha;
output_camera_matrix(1, 2) = cy0 * (1.0 - alpha) + cy1 * alpha;
output_camera_matrix(2, 2) = 1.0;
return output_camera_matrix;
}
template<typename InputDerivedCameraType, typename OutputDerivedCameraType>
void buildUndistortMap(const InputDerivedCameraType& input_camera,
const OutputDerivedCameraType& output_camera, int map_type,
cv::OutputArray map_u, cv::OutputArray map_v) {
// Output image size
cv::Size output_size(output_camera.imageWidth(), output_camera.imageHeight());
// Allocate the outputs maps
CHECK(map_type == CV_16SC2 || map_type == CV_32FC1 || map_type == CV_32FC2);
map_u.create(output_size, map_type);
cv::Mat map1 = map_u.getMat(), map2;
if (map_type != CV_32FC2) {
map_v.create(output_size, map_type == CV_16SC2 ? CV_16UC1 : CV_32FC1);
map2 = map_v.getMat();
} else
map_v.release();
// Build the maps.
for (int i = 0; i < output_size.height; i++) {
float* m1f = (float*) (map1.data + map1.step * i);
float* m2f = (float*) (map2.data + map2.step * i);
short* m1 = (short*) m1f;
ushort* m2 = (ushort*) m2f;
for (int j = 0; j < output_size.width; j++) {
// Convert point on normalized image plane to keypoints. (projection and distortion)
const Eigen::Vector2d keypoint(j, i);
Eigen::Vector2d keypoint_dist;
Eigen::Vector3d point_3d;
output_camera.backProject3(keypoint, &point_3d);
point_3d /= point_3d[2];
input_camera.project3(point_3d, &keypoint_dist);
const double& u = keypoint_dist[0];
const double& v = keypoint_dist[1];
// Store in output format
if (map_type == CV_16SC2) {
int iu = cv::saturate_cast<int>(u * cv::INTER_TAB_SIZE);
int iv = cv::saturate_cast<int>(v * cv::INTER_TAB_SIZE);
m1[j * 2] = (short) (iu >> cv::INTER_BITS);
m1[j * 2 + 1] = (short) (iv >> cv::INTER_BITS);
m2[j] = (ushort) ((iv & (cv::INTER_TAB_SIZE - 1)) * cv::INTER_TAB_SIZE
+ (iu & (cv::INTER_TAB_SIZE - 1)));
} else if (map_type == CV_32FC1) {
m1f[j] = (float) u;
m2f[j] = (float) v;
} else {
m1f[j * 2] = (float) u;
m1f[j * 2 + 1] = (float) v;
}
}
}
}
} //namespace common
} //namespace aslam
#endif // ASLAM_UNDISTORT_HELPERS_H_