distortPoints

/****************************************************************************
 *
 *   Copyright (c) 2018 Mohammed Kabir. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name Mohammed Kabir nor the names of their contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 
 void Utils::distortPoint(const cv::Point2f &src, const cv::Mat &camera_matrix_K,
			 const std::vector<double> &distortion_matrix, cv::Point2f &dst)
{

	double fx_K = camera_matrix_K.at<double>(0, 0);
	double fy_K = camera_matrix_K.at<double>(1, 1);
	double cx_K = camera_matrix_K.at<double>(0, 2);
	double cy_K = camera_matrix_K.at<double>(1, 2);

	double k1 = distortion_matrix[0];
	double k2 = distortion_matrix[1];
	double p1 = distortion_matrix[2];
	double p2 = distortion_matrix[3];
	double k3 = distortion_matrix[4];

	// Project the points into the world
	const cv::Point2d &p = src;
	double x = (p.x - cx_K) / fx_K;
	double y = (p.y - cy_K) / fy_K;
	double xCorrected, yCorrected;

	// Correct distortion
	double r2 = x * x + y * y;

	// Radial
	xCorrected = x * (1. + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
	yCorrected = y * (1. + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);

	// Tangential
	xCorrected = xCorrected + (2. * p1 * x * y + p2 * (r2 + 2. * x * x));
	yCorrected = yCorrected + (p1 * (r2 + 2. * y * y) + 2. * p2 * x * y);

	// Project coordinates onto image plane
	xCorrected = xCorrected * fx_K + cx_K;
	yCorrected = yCorrected * fy_K + cy_K;

	dst = cv::Point2f(xCorrected, yCorrected);

}