triangulate points using epipolar geometry - opencv

I'm using in OpenCV the method
triangulatePoints(P1,P2,x1,x2)
to get the 3D coordinates of a point by its image points x1/x2 in the left/right image and the projection matrices P1/P2.
I've already studied epipolar geometry and know most of the maths behind it. But what how does this algorithm get mathematically the 3D Coordinates?

Here are just some ideas, to the best of my knowledge, should at least work theoretically.
Using the camera equation ax = PX, we can express the two image point correspondences as
ap = PX
bq = QX
where p = [p1 p2 1]' and q = [q1 q2 1]' are the matching image points to the 3D point X = [X Y Z 1]' and P and Q are the two projection matrices.
We can expand these two equations and rearrange the terms to form an Ax = b system as shown below
p11.X + p12.Y + p13.Z - a.p1 + b.0 = -p14
p21.X + p22.Y + p23.Z - a.p2 + b.0 = -p24
p31.X + p32.Y + p33.Z - a.1 + b.0 = -p34
q11.X + q12.Y + q13.Z + a.0 - b.q1 = -q14
q21.X + q22.Y + q23.Z + a.0 - b.q2 = -q24
q31.X + q32.Y + q33.Z + a.0 - b.1 = -q34
from which we get
A = [p11 p12 p13 -p1 0; p21 p22 p23 -p2 0; p31 p32 p33 -1 0; q11 q12 q13 0 -q1; q21 q22 q23 0 -q2; q31 q32 q33 0 -1], x = [X Y Z a b]' and b = -[p14 p24 p34 q14 q24 q34]'. Now we can solve for x to find the 3D coordinates.
Another approach is to use the fact, from camera equation ax = PX, that x and PX are parallel. Therefore, their cross product must be a 0 vector. So using,
p x PX = 0
q x QX = 0
we can construct a system of the form Ax = 0 and solve for x.

Related

Intrinsic parameters OpenCV explanation

I measured intrinsic camera parameters with the OpenCv algorithm. The imager has an resolution of (752,480)
The values are :
Focal Length X 1021.477666;
Focal Length Y 1021.146888
Principal Point X 376.2966754
Principal Point Y 253.7513058
K1 -0.263658008
K2 -0.206264024
P1 -0.001102641
P2 -0.000980475
Error 0.122991565
Now I want to calculate the distorsion of one pixel.
The coordinates of the "old" pixel are
(x = 680,y = 390).
I´m using the following formula to calculate the coordinates of the corrected pixel
x_{corrected} = x( 1 + k_1 r^2 + k_2 r^4)
y_{corrected} = y( 1 + k_1 r^2 + k_2 r^4)
For r I´m using the distance
r = sqrt((x_shifted^2 - x_principalPoint^2)+(y_shifted^2 - y_principalPoint^2))
The calculation I did :
x_corrected = 680 * ( 1 + (-0.26365 * 333^2) + (-0.2062 + 333^2 ) )
= 55524658.09
This value is incorrect I think.
Does anyone have an idea what I´m doing wrong?
You need to express your undistorted points in normalised coordinates. So you need to convert it first:
x_norm = (x-px)/px = 0.8071
and
y_norm = (y-py)/py = 0.5369
Then calculate the radius:
r = sqrt(x_norm^2+y_norm^2)
With these values you get your corrrected normalizes coordinates :
x_corr_norm = x_norm*(1+k1*r^2+k2*r^4) = 0.4601
y_corr_norm = y_norm*(1+k1*r^2+k2*r^4) = 0.3061
and you have to convert them back again:
x_corr = (x_corr_norm+1)*px = 549.4409
y_corr = (y_corr_norm+1)*py = 331.4280
For a more accurate result, you should consider the non-radial distortion too:

Formula for CGPointApplyAffineTransform function in iOS

I am trying to affine rotate matrix in Android based on iOS Code
iOS has two functions CGAffineTransformMakeRotation and CGPointApplyAffineTransform used for calculation
Step 1: CGAffineTransformMakeRotation();
Input:
2.2860321998596191
Result:
a = -0.65579550461444569,
b = 0.75493857771840255,
c = -0.75493857771840255,
d = -0.65579550461444569,
tx = 0, ty = 0
Formula:
double A = Math.cos(RadiansRotated);
double B = -Math.sin(RadiansRotated);
double C = Math.sin(RadiansRotated);
double D = Math.cos(RadiansRotated);
I am able to calculate a,b,c,d for step 1 using above formula
Step 2: CGPointApplyAffineTransform()
Input :
x = 612.55191924649432,
y = -391.95960729287646
And Matrix return from Step 1
Result:
x = -105.80336653205421,
y = 719.48442314773808
Does anyone know the formula used in ApplyAffineTransform?
I need help for Step 2
I have tried with Android's Matrix class - Not Working
I have also tried with Java's AffineTransform - Not working
The math behind the CGAffineTransform functions is described in “The Math Behind the Matrices” in the Quartz 2D Programming Guide.
The formulae for transforming a point using an affine transform are given as:
x' = ax + cy + tx
y' = bx + dy + ty
Incidentally, in your step 1, you have reversed the signs of b and c, which has the effect of reversing the direction of rotation.

What is the Project Tango lens distortion model?

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.

Draw arc through three point opencv

I have three Point A(a1,a2) , B (b1, b2) , C (c1, c2). How to draw arc through three point and calculate arc angle.
Thanks all.
[HERE] http://photo.ssc.vn/view.php?filename=374df.png
In the event that you choose a quadratic you will have
y = ax*x + bx + c
Three points A(x1, y1) B(x2, y2) C(x3, y3)
This gives a Linear system
y1 = ax1*x + bx1 + c
y2 = ax2*x + bx2 + c
y3 = ax3*x + bx3 + c
Which can be solved for a, b and c
In the event that you are using a circle, use
Emgu.CV.PointCollection.MinEnclosingCircle
This will give you an object of type CircleF, which has a property Center of type PointF.
Find the vectors between the points and the center.
Va = A - Center
Vb = B - Center
Vc = C - Center
Find the angles between these vectors. You are looking for the largest acute angle.
You can use dot product to calculate the angle.

Describing nonlinear transformation between two images, using homography

A one to one point matching has already been established
between the blue dots on the two images.
The image2 is the distorted version of the image1. The distortion model seems to be
eyefish lens distortion. The question is:
Is there any way to compute a transformation matrix which describes this transition.
In fact a matrix which transforms the blue
dots on the first image to their corresponding blue dots on the second image?
The problem here is that we don’t know the focal length(means images are uncalibrated), however we do have
perfect matching between around 200 points on the two images.
the distorted image:
I think what you're trying to do can be treated as a distortion correction problem, without the need of the rest of a classic camera calibration.
A matrix transformation is a linear one and linear transformations map always straight lines into straight lines (http://en.wikipedia.org/wiki/Linear_map). It is apparent from the picture that the transformation is nonlinear so you cannot describe it with a matrix operation.
That said, you can use a lens distortion model like the one used by OpenCV (http://docs.opencv.org/doc/tutorials/calib3d/camera_calibration/camera_calibration.html) and obtaining the coefficients shouldn't be very difficult. Here is what you can do in Matlab:
Call (x, y) the coordinates of an original point (top picture) and (xp, yp) the coordinates of a distorted point (bottom picture), both shifted to the center of the image and divided by a scaling factor (same for x and y) so they lie more or less in the [-1, 1] interval. The distortion model is:
x = ( xp*(1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p1*xp*yp + p2*(r^2 + 2*xp^2));
y = ( yp*(1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p2*xp*yp + p1*(r^2 + 2*yp^2));
Where
r = sqrt(x^2 + y^2);
You have 5 parameters: k1, k2, k3, p1, p2 for radial and tangential distortion and 200 pairs of points, so you can solve the nonlinear system.
Be sure the x, y, xp and yp arrays exist in the workspace and declare them global:
global x y xp yp
Write a function to evaluate the mean square error given a set of arbitrary distortion coefficients, say it's called 'dist':
function val = dist(var)
global x y xp yp
val = zeros(size(xp));
k1 = var(1);
k2 = var(2);
k3 = var(3);
p1 = var(4);
p2 = var(5);
r = sqrt(xp.*xp + yp.*yp);
temp1 = x - ( xp.*(1 + k1*r.^2 + k2*r.^4 + k3*r.^6) + 2*p1*xp.*yp + p2*(r.^2 + 2*xp.^2));
temp2 = y - ( yp.*(1 + k1*r.^2 + k2*r.^4 + k3*r.^6) + 2*p2*xp.*yp + p1*(r.^2 + 2*yp.^2));
val = sqrt(temp1.*temp1 + temp2.*temp2);
Solve the system with 'fsolve":
[coef, fval] = fsolve(#dist, zeros(5,1));
The values in 'coef' are the distortion coefficients you're looking for. To correct the distortion of new points (xp, yp) not present in the original set, use the equations:
r = sqrt(xp.*xp + yp.*yp);
x_corr = xp.*(1 + k1*r.^2 + k2*r.^4 + k3*r.^6) + 2*p1*xp.*yp + p2*(r.^2 + 2*xp.^2);
y_corr = yp.*(1 + k1*r.^2 + k2*r.^4 + k3*r.^6) + 2*p2*xp.*yp + p1*(r.^2 + 2*yp.^2);
Results will be shifted to the center of the image and scaled by the factor you used above.
Notes:
Coordinates must be shifted to the center of the image as the distortion is symmetric with respect to it.
It should't be necessary to normalize to the interval [-1, 1] but it is comon to do it so the distortion coefficients obtained are more or less of the same order of magnitude (working with powers 2, 4 and 6 of pixel coordinates would need very small coefficients).
This method doesn't require the points in the image to be in an uniform grid.

Resources