It is my first post on Stack so I'm sorry in advance for my clumsiness. Please let me know if I can improve my question anyway.
► What I want to achieve (in a long term):
I try to manipulate my Unity3d presentation with a laser pointer using OpenCV fo Unity.
I believe one picture is worth more than a thousand words, so this should tell the most:
► What is the problem:
I try to make a simple 4-point calibration (projection) from camera view (some kind of trapezium) into plane space.
I thought it will be something very basic and easy, but I have no experience with OpenCV and I can't make it work.
► Sample:
I made a much less complicated example, without any laser detection and all other stuff. Only 4-points trapezium that I try to reproject into the plane space.
Link to the whole sample project: https://1drv.ms/u/s!AiDsGecSyzmuujXGQUapcYrIvP7b
The core script from my example:
using OpenCVForUnity;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.UI;
using System;
public class TestCalib : MonoBehaviour
{
public RawImage displayDummy;
public RectTransform[] handlers;
public RectTransform dummyCross;
public RectTransform dummyResult;
public Vector2 webcamSize = new Vector2(640, 480);
public Vector2 objectSize = new Vector2(1024, 768);
private Texture2D texture;
Mat cameraMatrix;
MatOfDouble distCoeffs;
MatOfPoint3f objectPoints;
MatOfPoint2f imagePoints;
Mat rvec;
Mat tvec;
Mat rotationMatrix;
Mat imgMat;
void Start()
{
texture = new Texture2D((int)webcamSize.x, (int)webcamSize.y, TextureFormat.RGB24, false);
if (displayDummy) displayDummy.texture = texture;
imgMat = new Mat(texture.height, texture.width, CvType.CV_8UC3);
}
void Update()
{
imgMat = new Mat(texture.height, texture.width, CvType.CV_8UC3);
Test();
DrawImagePoints();
Utils.matToTexture2D(imgMat, texture);
}
void DrawImagePoints()
{
Point[] pointsArray = imagePoints.toArray();
for (int i = 0; i < pointsArray.Length; i++)
{
Point p0 = pointsArray[i];
int j = (i < pointsArray.Length - 1) ? i + 1 : 0;
Point p1 = pointsArray[j];
Imgproc.circle(imgMat, p0, 5, new Scalar(0, 255, 0, 150), 1);
Imgproc.line(imgMat, p0, p1, new Scalar(255, 255, 0, 150), 1);
}
}
private void DrawResults(MatOfPoint2f resultPoints)
{
Point[] pointsArray = resultPoints.toArray();
for (int i = 0; i < pointsArray.Length; i++)
{
Point p = pointsArray[i];
Imgproc.circle(imgMat, p, 5, new Scalar(255, 155, 0, 150), 1);
}
}
public void Test()
{
float w2 = objectSize.x / 2F;
float h2 = objectSize.y / 2F;
/*
objectPoints = new MatOfPoint3f(
new Point3(-w2, -h2, 0),
new Point3(w2, -h2, 0),
new Point3(-w2, h2, 0),
new Point3(w2, h2, 0)
);
*/
objectPoints = new MatOfPoint3f(
new Point3(0, 0, 0),
new Point3(objectSize.x, 0, 0),
new Point3(objectSize.x, objectSize.y, 0),
new Point3(0, objectSize.y, 0)
);
imagePoints = GetImagePointsFromHandlers();
rvec = new Mat(1, 3, CvType.CV_64FC1);
tvec = new Mat(1, 3, CvType.CV_64FC1);
rotationMatrix = new Mat(3, 3, CvType.CV_64FC1);
double fx = webcamSize.x / objectSize.x;
double fy = webcamSize.y / objectSize.y;
double cx = 0; // webcamSize.x / 2.0f;
double cy = 0; // webcamSize.y / 2.0f;
cameraMatrix = new Mat(3, 3, CvType.CV_64FC1);
cameraMatrix.put(0, 0, fx);
cameraMatrix.put(0, 1, 0);
cameraMatrix.put(0, 2, cx);
cameraMatrix.put(1, 0, 0);
cameraMatrix.put(1, 1, fy);
cameraMatrix.put(1, 2, cy);
cameraMatrix.put(2, 0, 0);
cameraMatrix.put(2, 1, 0);
cameraMatrix.put(2, 2, 1.0f);
distCoeffs = new MatOfDouble(0, 0, 0, 0);
Calib3d.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
Mat uv = new Mat(3, 1, CvType.CV_64FC1);
uv.put(0, 0, dummyCross.anchoredPosition.x);
uv.put(1, 0, dummyCross.anchoredPosition.y);
uv.put(2, 0, 0);
Calib3d.Rodrigues(rvec, rotationMatrix);
Mat P = rotationMatrix.inv() * (cameraMatrix.inv() * uv - tvec);
Vector2 v = new Vector2((float)P.get(0, 0)[0], (float)P.get(1, 0)[0]);
dummyResult.anchoredPosition = v;
}
private MatOfPoint2f GetImagePointsFromHandlers()
{
MatOfPoint2f m = new MatOfPoint2f();
List<Point> points = new List<Point>();
foreach (RectTransform handler in handlers)
{
Point p = new Point(handler.anchoredPosition.x, handler.anchoredPosition.y);
points.Add(p);
}
m.fromList(points);
return m;
}
}
Thanks in advance for any help.
This question is not opencv specific but heavily math-based and more often seen in the realm of computer graphics. What you are looking for is called a Projective Transformation.
A Projective Transformation takes a set of coordinates and projects them onto something. In your case you want to project a 2D point in the camera view to a 2D point on a flat plane.
So we want a projection transform for 2D-Space. To perform a projection transform we need to find the projection matrix for the transformation we want to apply. In this case we need a matrix that expresses the projective deformation of the camera in relation to a flat plane.
To work with projections we first need to convert our points into homogeneous coordinates. To do so we simply add a new component to our vectors with value 1. So (x,y) becomes (x,y,1). And we will do that with all our five available points.
Now we start with the actual math. First some definitions: The camera's point of view and respective coordinates shall be the camera space, coordinates in relation to a flat plane are in flat space. Let c₁ to c₄ be the corner points of the plane in relation to camera space as homogeneous vectors. Let p be the point that we have found in camera space and p' the point we want to find in flat space, both as homogeneous vectors again.
Mathematically speaking, we are looking for a Matrix C that will allow us to calculate p' by giving it p.
p' = C * p
Now we obviously need to find C. To find a projection matrix for two dimensional space, we need four points (how convenient..) I will assume that c₁ will go to (0,0), c₂ will go to (0,1), c₃ to (1,0) and c₄ to (1,1). You need to solve two matrix equations using e.g. the gaussian row elimination or an LR Decomposition algorithm. OpenCV should contain functions to do those tasks for you, but be aware of matrix conditioning and their impact on a usable solution.
Now back to the matrices. You need to calculate two basis change matrices as they are called. They are used to change the frame of reference of your coordinates (exactly what we want to do). The first matrix will transform our coordinates to three dimensional basis vectors and the second one will transform our 2D plane into three dimensional basis vectors.
For the coordinate one you'll need to calculate λ, μ and r in the following equation:
⌈ c₁.x c₂.x c₃.x ⌉ ⌈ λ ⌉ ⌈ c₄.x ⌉
c₁.y c₂.y c₃.y * μ = c₄.y
⌊ 1 1 1 ⌋ ⌊ r ⌋ ⌊ 1 ⌋
this will lead you to your first Matrix, A
⌈ λ*c₁.x μ*c₂.x r*c₃.x ⌉
A = λ*c₁.y μ*c₂.y r*c₃.y
⌊ λ μ r ⌋
A will now map the points c₁ to c₄ to the basis coordinates (1,0,0), (0,1,0), (0,0,1) and (1,1,1). We do the same thing for our plane now. First solve
⌈ 0 0 1 ⌉ ⌈ λ ⌉ ⌈ 1 ⌉
0 1 0 * μ = 1
⌊ 1 1 1 ⌋ ⌊ r ⌋ ⌊ 1 ⌋
and get B
⌈ 0 0 r ⌉
B = 0 μ 0
⌊ λ μ r ⌋
A and B will now map from those three dimensional basis vectors into your respective spaces. But that is not quite what we want. We want camera space -> basis -> flat space, so only matrix B manipulates in the right direction. But that is easily fixable by inverting A. That will give us matrix C = B * A⁻¹ (watch the order of B and A⁻¹ it is not interchangeable). This leaves us with a formula to calculate p' out of p.
p' = C * p
p' = B * A⁻¹ * p
Read it from left to right like: take p, transform p from camera space into basis vectors and transform those into flat space.
If you remember correctly, p' still has three components, so we need to dehomogenize p' first before we can use it. This will yield
x' = p'.x / p'.z
y' = p'.y / p'.z
and viola we have successfully transformed a laser point from a camera view onto a flat piece of paper. Totally not overly complicated or so...
I Develop Code. MouseUp Call this Function. And Resolution Edit;
void Cal()
{
// Webcam Resolution 1280*720
MatOfPoint2f pts_src = new MatOfPoint2f(
new Point(Double.Parse(imagePoints.get(0,0).GetValue(0).ToString()), Double.Parse(imagePoints.get(0, 0).GetValue(1).ToString())),
new Point(Double.Parse(imagePoints.get(1,0).GetValue(0).ToString()), Double.Parse(imagePoints.get(1, 0).GetValue(1).ToString())),
new Point(Double.Parse(imagePoints.get(2,0).GetValue(0).ToString()), Double.Parse(imagePoints.get(2, 0).GetValue(1).ToString())),
new Point(Double.Parse(imagePoints.get(3,0).GetValue(0).ToString()), Double.Parse(imagePoints.get(3, 0).GetValue(1).ToString()))
);
//Resolution 1920*1080
MatOfPoint2f pts_dst = new MatOfPoint2f(
new Point(0, 0),
new Point(1920, 0),
new Point(1920, 1080),
new Point(0, 1080)
);
// 1. Calculate Homography
Mat h = Calib3d.findHomography((pts_src), (pts_dst));
// Pick Point (WebcamDummy Cavas : 1280*0.5f / 720*0.5f)
MatOfPoint2f srcPointMat = new MatOfPoint2f(
new Point(dummyCross.anchoredPosition.x*2.0f, dummyCross.anchoredPosition.y*2.0f)
);
MatOfPoint2f dstPointMat = new MatOfPoint2f();
{
//2. h Mat Mul srcPoint to dstPoint
Core.perspectiveTransform(srcPointMat, dstPointMat, h);
Vector2 v = new Vector2((float)dstPointMat.get(0, 0)[0], (float)dstPointMat.get(0, 0)[1]);
//(ResultDummy Cavas: 1920 * 0.5f / 1080 * 0.5f)
dummyResult.anchoredPosition = v*0.5f;
Debug.Log(dummyCross.anchoredPosition.ToString() + "\n" + dummyResult.anchoredPosition.ToString());
}
}
The Project Tango C API documentation says that the TANGO_CALIBRATION_POLYNOMIAL_3_PARAMETERS lens distortion is modeled as:
x_corr_px = x_px (1 + k1 * r2 + k2 * r4 + k3 * r6) y_corr_px = y_px (1
+ k1 * r2 + k2 * r4 + k3 * r6)
That is, the undistorted coordinates are a power series function of the distorted coordinates. There is another definition in the Java API, but that description isn't detailed enough to tell which direction the function maps.
I've had a lot of trouble getting things to register properly, and I suspect that the mapping may actually go in the opposite direction, i.e. the distorted coordinates are a power series of the undistorted coordinates. If the camera calibration was produced using OpenCV, then the cause of the problem may be that the OpenCV documentation contradicts itself. The easiest description to find and understand is the OpenCV camera calibration tutorial, which does agree with the Project Tango docs:
But on the other hand, the OpenCV API documentation specifies that the mapping goes the other way:
My experiments with OpenCV show that its API documentation appears correct and the tutorial is wrong. A positive k1 (with all other distortion parameters set to zero) means pincushion distortion, and a negative k1 means barrel distortion. This matches what Wikipedia says about the Brown-Conrady model and will be opposite from the Tsai model. Note that distortion can be modeled either way depending on what makes the math more convenient. I opened a bug against OpenCV for this mismatch.
So my question: Is the Project Tango lens distortion model the same as the one implemented in OpenCV (documentation notwithstanding)?
Here's an image I captured from the color camera (slight pincushioning is visible):
And here's the camera calibration reported by the Tango service:
distortion = {double[5]#3402}
[0] = 0.23019999265670776
[1] = -0.6723999977111816
[2] = 0.6520439982414246
[3] = 0.0
[4] = 0.0
calibrationType = 3
cx = 638.603
cy = 354.906
fx = 1043.08
fy = 1043.1
cameraId = 0
height = 720
width = 1280
Here's how to undistort with OpenCV in python:
>>> import cv2
>>> src = cv2.imread('tango00042.png')
>>> d = numpy.array([0.2302, -0.6724, 0, 0, 0.652044])
>>> m = numpy.array([[1043.08, 0, 638.603], [0, 1043.1, 354.906], [0, 0, 1]])
>>> h,w = src.shape[:2]
>>> mDst, roi = cv2.getOptimalNewCameraMatrix(m, d, (w,h), 1, (w,h))
>>> dst = cv2.undistort(src, m, d, None, mDst)
>>> cv2.imwrite('foo.png', dst)
And that produces this, which is maybe a bit overcorrected at the top edge but much better than my attempts with the reverse model:
The Tango C-API Docs state that (x_corr_px, y_corr_px) is the "corrected output position". This corrected output position needs to then be scaled by focal length and offset by center of projection to correspond to a distorted pixel coordinates.
So, to project a point onto an image, you would have to:
Transform the 3D point so that it is in the frame of the camera
Convert the point into normalized image coordinates (x, y)
Calculate r2, r4, r6 for the normalized image coordinates (r2 = x*x + y*y)
Compute (x_corr_px, y_corr_px) based on the mentioned equations:
x_corr_px = x (1 + k1 * r2 + k2 * r4 + k3 * r6)
y_corr_px = y (1 + k1 * r2 + k2 * r4 + k3 * r6)
Compute distorted coordinates
x_dist_px = x_corr_px * fx + cx
y_dist_px = y_corr_px * fy + cy
Draw (x_dist_px, y_dist_px) on the original, distorted image buffer.
This also means that the corrected coordinates are the normalized coordinates scaled by a power series of the normalized image coordinates' magnitude. (this is the opposite of what the question suggests)
Looking at the implementation of cvProjectPoints2 in OpenCV (see [opencv]/modules/calib3d/src/calibration.cpp), the "Poly3" distortion in OpenCV is being applied the same direction as in Tango. All 3 versions (Tango Docs, OpenCV Tutorials, OpenCV API) are consistent and correct.
Good luck, and hopefully this helps!
(Update: Taking a closer look at a the code, it looks like the corrected coordinates and distorted coordinates are not the same. I've removed the incorrect parts of my response, and the remaining parts of this answer are still correct.)
Maybe it's not the right place to post, but I really want to share the readable version of code used in OpenCV to actually correct the distortion.
I'm sure that I'm not the only one who needs x_corrected and y_corrected and fails to find an easy and understandable formula.
I've rewritten the essential part of cv2.undistortPoints in Python and you may notice that the correction is performed iteratively. This is important, because the solution for polynom of 9-th power does not exist and all we can do is to apply its the reveresed version several times to get the numerical solution.
def myUndistortPoint((x0, y0), CM, DC):
[[k1, k2, p1, p2, k3, k4, k5, k6]] = DC
fx, _, cx = CM[0]
_, fy, cy = CM[1]
x = x_src = (x0 - cx) / fx
y = y_src = (y0 - cy) / fy
for _ in range(5):
r2 = x**2 + y**2
r4 = r2**2
r6 = r2 * r4
rad_dist = (1 + k4*r2 + k5*r4 + k6*r6) / (1 + k1*r2 + k2*r4 + k3*r6)
tang_dist_x = 2*p1 * x*y + p2*(r2 + 2*x**2)
tang_dist_y = 2*p2 * x*y + p1*(r2 + 2*y**2)
x = (x_src - tang_dist_x) * rad_dist
y = (y_src - tang_dist_y) * rad_dist
x = x * fx + cx
y = y * fy + cy
return x, y
To speed up, you can use only three iterations, on most cameras this will give enough precision to fit the pixels.
I use openCV function projectPoints() to rotate, translate and project a set of 3D points and solvePnp() to find this rotation and translation. This works well when the lens distortion coefficients are all zero but fails otherwise. It takes as little distortion as this to fail completely:
distCoeffs << 0.0, 0.01, 0.0, 0.0, 0.0;
The code is below:
#include <iostream>
#include "opencv.hpp"
using namespace std;
using namespace cv;
#define DEG2RAD (3.1415293/180.0)
#define RAD2DEG (1.0/DEG2RAD)
int main() {
const int npoints = 10; // number of points
// extrinsic
const Point3f tvec(10, 20, 30);
Point3f rvec(3, 5, 7);
cout << "Finding extrinsic parameters (PnP)" << endl;
cout<<"Test transformations: ";
cout<<"Rotation: "<<rvec<<"; translation: "<<tvec<<endl;
rvec*=DEG2RAD;
// intrinsic
Mat_ <double>cameraMatrix(3, 3);
cameraMatrix << 300., 0., 200., 0, 300., 100., 0., 0., 1.;
Mat_ <double>distCoeffs(1, 5); // (k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]]) of 4, 5, or 8 elements.
//distCoeffs << 1.2, 0.2, 0., 0., 0.; // non-zero distortion
distCoeffs << 0.0, 0.0, 0.0, 0.0, 0.0; // zero distortion
cout<<"distrotion coeff: "<<distCoeffs<<endl;
cout<<"============= Running PnP..."<<endl;
vector<Point3f> objPts(npoints);
vector<Point2f> imagePoints(npoints);
Mat rvec_est, tvec_est;
randu(Mat(objPts), 0.0f, 100.0f);
// project
projectPoints(Mat(objPts), Mat(rvec), Mat(tvec), cameraMatrix, distCoeffs, Mat(imagePoints));
// extrinsic
solvePnP(objPts, imagePoints, cameraMatrix, distCoeffs, rvec_est, tvec_est);
cout<<"Rotation: "<<rvec_est*RAD2DEG<<endl;
cout<<"Translation "<<tvec_est<<endl;
return 0;
}
When all distortion coefficients are 0 the result is OK:
Finding extrinsic parameters (PnP)
Test transformations: Rotation: [3, 5, 7]; translation: [10, 20, 30]
distrotion coeff: [0, 0, 0, 0, 0]
============= Running PnP...
Rotation: [2.999999581709123; 4.999997813985293; 6.999999826089725]
Translation [9.999999792663072; 19.99999648222693; 29.99999699621362]
However when they aren't zero the result is totally wrong:
Finding extrinsic parameters (PnP)
Test transformations: Rotation: [3, 5, 7]; translation: [10, 20, 30]
distrotion coeff: [1.2, 0.2, 0, 0, 0]
============= Running PnP...
Rotation: [-91.56479629305277; -124.3631985067845; -74.46486950666471]
Translation [-69.72473511009439; -117.7463271636532; -87.27777166027946]
Since people asked, I am adding intermediate input - some 3D points and their projections for non-zero distortion coefficients. My camera matrix was
cameraMatrix << 300., 0., 200., 0, 300., 100., 0., 0., 1.;
3d points [53.0283, 19.9259, 40.1059]; 2D projection [1060.34, 700.59]
3d points [81.4385, 43.7133, 24.879]; 2D projection [6553.88, 5344.22]
3d points [77.3105, 76.2094, 30.7794]; 2D projection [5143.32, 6497.12]
3d points [70.2432, 47.8447, 79.219]; 2D projection [771.497, 611.726]
Another interesting observation: applying undistort when distCoeff are non zero doesn’t really works (but it does produce identical 2D points when distortion coefficients are all 0):
cout<<"applying undistort..."<<endl;
vector<Point2f> imagePointsUndistort(npoints);
undistortPoints(Mat(imagePoints), Mat(imagePointsUndistort), cameraMatrix, distCoeffs);
for (int i=0; i<4; i++)
cout<<"2d original "<<imagePoints[i]<<"; 2d undistort "<<imagePointsUndistort[i]<<endl;
applying undistort...
2d original [1060.34, 700.59]; 2d undistort [0, 0]
2d original [6553.88, 5344.22]; 2d undistort [0, 0]
2d original [5143.32, 6497.12]; 2d undistort [0, 0]
2d original [771.497, 611.726]; 2d undistort [0, 0]
The reason why I tried undistort() is because if one undoes the effect of known intrinsic parameters PnP becomes just a minimum direction problem of the form Ax=0. It needs min. 6 points for an approximate linear solution which is probably further improved with LMA (flags=CV_ITERATIVE). Technically there are only 6DOF and thus 3 points required so other methods (flags=CV_P3P, CV_EPNP) take less points. Anyways, regardless of a method or number of points the result is still invalid with non-zero distortion coefficients. The last thing I will try is to put all points on a 3D plane. It still fails:
for (int i=0; i<npoints; i++)
objPts[i].z=0.0f;
Finding extrinsic parameters (PnP)
Test transformations: Rotation: [3, 5, 7]; translation: [10, 20, 30]
distrotion coeff: [1.2, 0.2, 0, 0, 0]
============= Running PnP...
Rotation: [-1830.321574903016; 2542.206083947917; 2532.255948350521]
Translation [1407.918216894239; 1391.373407846455; 556.7108606094299]
How to make your code work?
I am able to reproduce the described behavior using the code you provided, however, either one of the two following options solve the problem:
Replace const Point3f tvec(10, 20, 30); by const Point3f tvec(10, 20, N); where N is much lower than 0 (e.g. -300) or much larger than 100 (e.g. 300).
Replace your call to solvePnP by a call to solvePnPRansac.
Why does each of these changes fix the undesired behavior?
First, consider what your original code requests from the solvePnP function. You are using a rotation of rather small magnitude, hence for simplicity of the explanation, I will assume that the rotation is identity. Then, the camera is positionned at world coordinates X=10, Y=20 and Z=30 and you generate object points randomly with world coordinates (X,Y,Z) uniformly drawn in [0,100]3. Hence, the camera is in the middle of the possible range for the object points, as illustrated on the following picture:
This means that object points may be generated very close to the focal plane (i.e. the plane going through the optical center and perpendicularly with respect to the optical axis). The projection in the camera image for such object points is undefined. However, in practice the non-linear optimization algorithm for undistortPoints is unstable even for object points close to the focal plane. This unstability causes the iterative algorithm for undistortPoints to diverge, except when the coefficients are all zero since in that case the initial values remain strictly constant during the estimation.
Hence, the two possible solutions to avoid this behavior are the following:
Avoid generating object points near the focal plane of the camera, i.e. change the translation vector or the range of the coordinates of the object points.
Eliminate the object points too close to the focal plane of the camera, whose undistorted estimation diverged (outliers), before the PnP estimation for example using solvePnPRansac.
Details about why undistortPoints fails:
NB: As we know the 3D world points, I used the following call to obtain the true undistorted coordinates, independently from the result of undistortPoints:
cv::projectPoints(obj_pts, rvec, tvec, cv::Mat_<double>::eye(3,3), cv::Mat_<double>::zeros(5,1), true_norm_pts);
The following function is a simplified version of what undistortPoints is doing:
void simple_undistort_point(const cv::Mat &img_pt,
const cv::Mat_<double> &K,
const cv::Mat_<double> &D,
cv::Mat &norm_pt)
{
// Define temporary variables
double k[8]={D.at<double>(0),
D.at<double>(1),
D.at<double>(2),
D.at<double>(3),
D.at<double>(4)},
fx, fy, ifx, ify, cx, cy;
fx = K.at<double>(0,0);
fy = K.at<double>(1,1);
ifx = 1./fx;
ify = 1./fy;
cx = K.at<double>(0,2);
cy = K.at<double>(1,2);
// Cancel distortion iteratively
const int iters = 5;
double x, y, x0, y0;
x0=x=(img_pt.at<double>(0)-cx)*ifx;
y0=y=(img_pt.at<double>(1)-cy)*ify;
for(int j = 0; j < iters; ++j)
{
double r2 = x*x + y*y;
double icdist = 1/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
x = (x0 - deltaX)*icdist;
y = (y0 - deltaY)*icdist;
}
// Store result
norm_pt.create(1,2,CV_64F);
norm_pt.at<double>(0) = x;
norm_pt.at<double>(1) = y;
}
If you add code to check how x and y change with each iteration, you'll see that the iterative optimization diverges due to r2 being very large at the beginning. Here is a log example:
#0: [2.6383300, 1.7651500] r2=10.0766000, icdist=0.0299408, deltaX=0, deltaY=0
#1: [0.0789937, 0.0528501] r2=0.00903313, icdist=0.9892610, deltaX=0, deltaY=0
#2: [2.6100000, 1.7462000] r2=9.86128000, icdist=0.0309765, deltaX=0, deltaY=0
#3: [0.0817263, 0.0546783] r2=0.00966890, icdist=0.9885120, deltaX=0, deltaY=0
#4: [2.6080200, 1.7448800] r2=9.84637000, icdist=0.0310503, deltaX=0, deltaY=0
end: [0.0819209, 0.0548085]
true: [0.9327440, 0.6240440]
When r2 is large, r2*r2*r2 is huge hence icdist is very small, hence the next iteration starts with a very small r2. When r2 is very small, icdist is close to 1, hence x and y are respectively set to x0 and y0 and we are back with a large r2, etc.
So why is r2 so large in the first place? Because the points may be generated close to the focal plane, in which case they are far from the optical axis (hence a very large r2). See the following log example:
img_pt#0=[991.4992804037340, 629.5460091483255], r2=10.07660, norm(cv_undist-true)=1.0236800
img_pt#1=[5802.666489402056, 4402.387472311543], r2=554.4490, norm(cv_undist-true)=2.1568300
img_pt#2=[5040.551339386630, 5943.173381042060], r2=639.7070, norm(cv_undist-true)=2.1998700
img_pt#3=[741.9742544382640, 572.9513930063181], r2=5.749100, norm(cv_undist-true)=0.8158670
img_pt#4=[406.9101658356062, 403.0152736214052], r2=1.495890, norm(cv_undist-true)=0.1792810
img_pt#5=[516.2079583447821, 1038.026553216831], r2=10.88760, norm(cv_undist-true)=1.0494500
img_pt#6=[1876.220394606081, 8129.280202695572], r2=747.5450, norm(cv_undist-true)=2.2472900
img_pt#7=[236.9935231831764, 329.3418854620716], r2=0.599625, norm(cv_undist-true)=0.0147487
img_pt#8=[1037.586015858139, 1346.494838992490], r2=25.05890, norm(cv_undist-true)=1.2998400
img_pt#9=[499.9808133105154, 715.6213031242644], r2=5.210870, norm(cv_undist-true)=0.7747020
You can see that for most points, r2 is very large, except for a few (#3, #4 & #7) which are also those associated with the best undistortion accuracy.
This problem is due to the particular undistortion algorithm implemented in OpenCV, which has been chosen for its efficiency. Other non-linear optimization algorithm (e.g. Levenberg-Marquardt) would be more accurate but also much slower, and would definitely be an overkill in most applications.
Let me go through opencv sources. But first I present "pure" opencv function that works as in the sources (please read below how I got this point) merged with your code to show it works as the library one:
#include <iostream>
#include <opencv2\opencv.hpp>
using namespace std;
using namespace cv;
#define DEG2RAD (3.1415293/180.0)
#define RAD2DEG (1.0/DEG2RAD)
Point2f Project(Point3f p, double R[], double t[], double k[], double fx, double fy, double cx, double cy) {
double X = p.x, Y = p.y, Z = p.z;
double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
double r2, r4, r6, a1, a2, a3, cdist, icdist2;
double xd, yd;
z = z ? 1./z : 1;
x *= z; y *= z;
r2 = x*x + y*y;
r4 = r2*r2;
r6 = r4*r2;
a1 = 2*x*y;
a2 = r2 + 2*x*x;
a3 = r2 + 2*y*y;
cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
xd = x*cdist*icdist2 + k[2]*a1 + k[3]*a2;
yd = y*cdist*icdist2 + k[2]*a3 + k[3]*a1;
double xRet = xd*fx + cx;
double yRet = yd*fy + cy;
return Point2f(xRet, yRet);
}
int main() {
const int npoints = 10; // number of points
// extrinsic
const Point3f tvec(10, 20, 30);
Point3f rvec(3, 5, 7);
cout << "Finding extrinsic parameters (PnP)" << endl;
cout<<"Test transformations: ";
cout<<"Rotation: "<<rvec<<"; translation: "<<tvec<<endl;
rvec*=DEG2RAD;
// intrinsic
Mat_ <double>cameraMatrix(3, 3);
cameraMatrix << 300., 0., 200., 0, 300., 100., 0., 0., 1.;
Mat_ <double>distCoeffs(1, 5); // (k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]]) of 4, 5, or 8 elements.
distCoeffs << 1.2, 0.2, 0., 0., 0.; // non-zero distortion
//distCoeffs << 0.0, 0.0, 0.0, 0.0, 0.0; // zero distortion
//distCoeffs << 1.8130418031666484e+000, -1.3285019729932657e+001, -1.6921715019797313e-002, -1.3327183367510961e-001, -5.2725832482783389e+001;
cout<<"distrotion coeff: "<<distCoeffs<<endl;
cout<<"============= Running PnP..."<<endl;
vector<Point3f> objPts(npoints);
vector<Point2f> imagePoints(npoints);
Mat rvec_est, tvec_est;
randu(Mat(objPts), 0.0f, 100.0f);
// project
projectPoints(Mat(objPts), Mat(rvec), Mat(tvec), cameraMatrix, distCoeffs, Mat(imagePoints));
std::cout << objPts << std::endl;
std::cout << imagePoints << std::endl;
double R[9];
Mat matR( 3, 3, CV_64F, R);
Mat_<double> m(1,3);
m << (double)rvec.x, (double)rvec.y, (double)rvec.z;
Rodrigues(m, matR);
std::cout << matR << std::endl;
double t[3] = {tvec.x, tvec.y, tvec.z};
double k[8] = {1.2, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
double fx = 300, fy = 300, cx = 200, cy = 100;
for(int i=0;i<objPts.size();i++)
std::cout << Project(objPts[i], R, t, k, fx, fy, cx, cy) << "; ";
std::cout << std::endl;
// extrinsic
solvePnP(objPts, imagePoints, cameraMatrix, distCoeffs, rvec_est, tvec_est);
cout<<"Rotation: "<<rvec_est*RAD2DEG<<endl;
cout<<"Translation "<<tvec_est<<endl;
return 0;
}
R is rotation, t translation, k distortion. Look at the 'r2' computation - it is x*x + y*y, but x,y is the position (scaled by z though) just after applying translation and rotation. And this r stands for (as wikpedia says) for "square distance in image projected by ideal pinhole model". We can say projectPoints implementation is OK.
How I got this result:
I'm digging up version 2.4.8. If you go to the calibration.cpp in the calib3d module, start with
void cv::projectPoints( InputArray _opoints,
InputArray _rvec,
InputArray _tvec,
InputArray _cameraMatrix,
InputArray _distCoeffs,
OutputArray _ipoints,
OutputArray _jacobian,
double aspectRatio )
{
Mat opoints = _opoints.getMat();
int npoints = opoints.checkVector(3), depth = opoints.depth();
CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
CvMat *pdpdrot=0, *pdpdt=0, *pdpdf=0, *pdpdc=0, *pdpddist=0;
_ipoints.create(npoints, 1, CV_MAKETYPE(depth, 2), -1, true);
CvMat c_imagePoints = _ipoints.getMat();
CvMat c_objectPoints = opoints;
Mat cameraMatrix = _cameraMatrix.getMat();
Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
CvMat c_cameraMatrix = cameraMatrix;
CvMat c_rvec = rvec, c_tvec = tvec;
double dc0buf[5]={0};
Mat dc0(5,1,CV_64F,dc0buf);
Mat distCoeffs = _distCoeffs.getMat();
if( distCoeffs.empty() )
distCoeffs = dc0;
CvMat c_distCoeffs = distCoeffs;
int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
if( _jacobian.needed() )
{
// cut out, we dont use this part
}
cvProjectPoints2( &c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
&c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio );
}
Nothing special, right? No content manipulation at all. Let's go deeper:
CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
const CvMat* r_vec,
const CvMat* t_vec,
const CvMat* A,
const CvMat* distCoeffs,
CvMat* imagePoints, CvMat* dpdr,
CvMat* dpdt, CvMat* dpdf,
CvMat* dpdc, CvMat* dpdk,
double aspectRatio )
{
Ptr<CvMat> matM, _m;
Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
int i, j, count;
int calc_derivatives;
const CvPoint3D64f* M;
CvPoint2D64f* m;
double r[3], R[9], dRdr[27], t[3], a[9], k[8] = {0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
// some code not important ...
if( r_vec->rows == 3 && r_vec->cols == 3 )
{
_r = cvMat( 3, 1, CV_64FC1, r );
cvRodrigues2( r_vec, &_r );
cvRodrigues2( &_r, &matR, &_dRdr );
cvCopy( r_vec, &matR );
}
else
{
_r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
cvConvert( r_vec, &_r );
cvRodrigues2( &_r, &matR, &_dRdr );
}
Last part is important, because we use cv::Rodriguez to create an rotation matrix from rotation vector. And later in the function we also create translation matrix, but still no data manipulation. Going further in the ProjectPoints2:
fx = a[0]; fy = a[4];
cx = a[2]; cy = a[5];
if( fixedAspectRatio )
fx = fy*aspectRatio;
if( distCoeffs )
{
if( !CV_IS_MAT(distCoeffs) ||
(CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
(distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
(distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8) )
CV_Error( CV_StsBadArg, cvDistCoeffErr );
_k = cvMat( distCoeffs->rows, distCoeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
cvConvert( distCoeffs, &_k );
}
Here we set focal lengths from camera matrix and principal point coords. Also we set array k which contains distortion coefs. Now we finished setting up variables. Let's go to the computations:
double X = M[i].x, Y = M[i].y, Z = M[i].z;
double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
double r2, r4, r6, a1, a2, a3, cdist, icdist2;
double xd, yd;
z = z ? 1./z : 1;
x *= z; y *= z;
r2 = x*x + y*y;
r4 = r2*r2;
r6 = r4*r2;
a1 = 2*x*y;
a2 = r2 + 2*x*x;
a3 = r2 + 2*y*y;
cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
xd = x*cdist*icdist2 + k[2]*a1 + k[3]*a2;
yd = y*cdist*icdist2 + k[2]*a3 + k[3]*a1;
m[i].x = xd*fx + cx; // here projection
m[i].y = yd*fy + cy;
And we have the function exactly as the one I presented on the top/