I need to convert a panorama in equirectangular projection to 6 cubic faces and then to spherical projection and back, however I need to keep a track of how each point is mapped in each projection like
Equirectangular Point(x,y) <---> Cubic face Point (x, y) <---> Sphere Point(x, y, z)
How can I accomplish this in C++ and OpenCV?
These transformations are required because I need to find out the good matching key-points between two such images by comparing angles between keypoints when the two panoramas, projected on a sphere, are placed side by side.
Here is the panorama:
Solved, below is the function to convert 2d Panoramic to 3d spherical coordinates.
vector<int> getSphericalPoint3D(int x, int y, int cols, int rows)
{
//introduce a radius to
static const int radius = 128;
vector<int> point3D;
// the center
double c_x = (double)cols / 2;
double c_y = (double)rows / 2;
double X = (((double)x - c_x) * CV_PI) / c_x;
double Y = (((double)y - c_y) * CV_PI) / c_y;
int x3D = round(radius * cos(X) * cos(Y)) + radius;
int y3D = round(radius * cos(X) * sin(Y)) + radius;
int z3D = round(radius * sin(X)) + radius;
point3D = { x3D, y3D, z3D };
return point3D;
}
The idea is to normalize 2d pixels from -Pi to Pi on both X and Y axis
Use the following relationship to get spherical co-ordinates
x = R * cos(x)cos(y) + R (this R is added to avoid the negative values)
y = R * cos(x)sin(y) + R
z = R * sin(x) + R
A similar transformation is used for Cubic transformation
I'm trying to calculate the values shown in the picture in red i.e. the interior angles.
I've got an array of the points where lines intersect and have tried using the dot-product but it only returns the smallest angles. I need the full range of internal angles (0-359) but can't seem to find much that meets this criteria.
Assuming your angles are in standard counterclockwise format, the following should work:
void angles(double points[][2], double angles[], int npoints){
for(int i = 0; i < npoints; i++){
int last = (i - 1 + npoints) % npoints;
int next = (i + 1) % npoints;
double x1 = points[i][0] - points[last][0];
double y1 = points[i][1] - points[last][1];
double x2 = points[next][0] - points[i][0];
double y2 = points[next][1] - points[i][1];
double theta1 = atan2(y1, x1)*180/3.1415926358979323;
double theta2 = atan2(y2, x2)*180/3.1415926358979323;
angles[i] = (180 + theta1 - theta2 + 360);
while(angles[i]>360)angles[i]-=360;
}
}
Obviously, if you are using some sort of data structure for your points, you will want to replace double points[][2] and references to it with references to your data structure.
You can obtain full angle range (-Pi..Pi) with atan2 function:
atan2(crossproduct, dotproduct)
I found several post about this subject, but none of the solutions are using opencv.
I am wondering if OpenCv has any function, or class that could help on this subject?
I have a 4*4 affine transformation in opencv and I am looking to find the rotation, translation assuming that scaling is 1 and there is no other transformation in matrix.
Is there any function in OpenCV to help finding these parameters?
The problem you're facing is known as a matrix decomposition problem.
You can retrieve your desired matrices following these steps:
Compute the scaling factors as the magnitudes of the first three basis vectors (columns or rows) of the matrix
Divide the first three basis vectors by these values (thus normalizing them)
The upper-left 3x3 part of the matrix now represents the rotation (you can use this as is, or convert it to quaternion form)
The translation is the fourth basis vector of the matrix (in homogeneous coordinates - it'll be the first three elements that you're interested in)
In your case, being your scaling factor 1, you can skip the first two steps.
To retrieve the rotation matrix axis and angle (in radians) I suggest you to port the following Java algorithm in OpenCV (source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/).
/**
This requires a pure rotation matrix 'm' as input.
*/
public axisAngle toAxisAngle(matrix m) {
double angle,x,y,z; // variables for result
double epsilon = 0.01; // margin to allow for rounding errors
double epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees
// optional check that input is pure rotation, 'isRotationMatrix' is defined at:
// http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/
assert isRotationMatrix(m) : "not valid rotation matrix" ;// for debugging
if ((Math.abs(m[0][1]-m[1][0])< epsilon)
&& (Math.abs(m[0][2]-m[2][0])< epsilon)
&& (Math.abs(m[1][2]-m[2][1])< epsilon)) {
// singularity found
// first check for identity matrix which must have +1 for all terms
// in leading diagonaland zero in other terms
if ((Math.abs(m[0][1]+m[1][0]) < epsilon2)
&& (Math.abs(m[0][2]+m[2][0]) < epsilon2)
&& (Math.abs(m[1][2]+m[2][1]) < epsilon2)
&& (Math.abs(m[0][0]+m[1][1]+m[2][2]-3) < epsilon2)) {
// this singularity is identity matrix so angle = 0
return new axisAngle(0,1,0,0); // zero angle, arbitrary axis
}
// otherwise this singularity is angle = 180
angle = Math.PI;
double xx = (m[0][0]+1)/2;
double yy = (m[1][1]+1)/2;
double zz = (m[2][2]+1)/2;
double xy = (m[0][1]+m[1][0])/4;
double xz = (m[0][2]+m[2][0])/4;
double yz = (m[1][2]+m[2][1])/4;
if ((xx > yy) && (xx > zz)) { // m[0][0] is the largest diagonal term
if (xx< epsilon) {
x = 0;
y = 0.7071;
z = 0.7071;
} else {
x = Math.sqrt(xx);
y = xy/x;
z = xz/x;
}
} else if (yy > zz) { // m[1][1] is the largest diagonal term
if (yy< epsilon) {
x = 0.7071;
y = 0;
z = 0.7071;
} else {
y = Math.sqrt(yy);
x = xy/y;
z = yz/y;
}
} else { // m[2][2] is the largest diagonal term so base result on this
if (zz< epsilon) {
x = 0.7071;
y = 0.7071;
z = 0;
} else {
z = Math.sqrt(zz);
x = xz/z;
y = yz/z;
}
}
return new axisAngle(angle,x,y,z); // return 180 deg rotation
}
// as we have reached here there are no singularities so we can handle normally
double s = Math.sqrt((m[2][1] - m[1][2])*(m[2][1] - m[1][2])
+(m[0][2] - m[2][0])*(m[0][2] - m[2][0])
+(m[1][0] - m[0][1])*(m[1][0] - m[0][1])); // used to normalise
if (Math.abs(s) < 0.001) s=1;
// prevent divide by zero, should not happen if matrix is orthogonal and should be
// caught by singularity test above, but I've left it in just in case
angle = Math.acos(( m[0][0] + m[1][1] + m[2][2] - 1)/2);
x = (m[2][1] - m[1][2])/s;
y = (m[0][2] - m[2][0])/s;
z = (m[1][0] - m[0][1])/s;
return new axisAngle(angle,x,y,z);
}
I have an ellipse, defined by Center Point, radiusX and radiusY, and I have a Point. I want to find the point on the ellipse that is closest to the given point. In the illustration below, that would be S1.
Now I already have code, but there is a logical error somewhere in it, and I seem to be unable to find it. I broke the problem down to the following code example:
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <math.h>
using namespace std;
void dostuff();
int main()
{
dostuff();
return 0;
}
typedef std::vector<cv::Point> vectorOfCvPoints;
void dostuff()
{
const double ellipseCenterX = 250;
const double ellipseCenterY = 250;
const double ellipseRadiusX = 150;
const double ellipseRadiusY = 100;
vectorOfCvPoints datapoints;
for (int i = 0; i < 360; i+=5)
{
double angle = i / 180.0 * CV_PI;
double x = ellipseRadiusX * cos(angle);
double y = ellipseRadiusY * sin(angle);
x *= 1.4;
y *= 1.4;
x += ellipseCenterX;
y += ellipseCenterY;
datapoints.push_back(cv::Point(x,y));
}
cv::Mat drawing = cv::Mat::zeros( 500, 500, CV_8UC1 );
for (int i = 0; i < datapoints.size(); i++)
{
const cv::Point & curPoint = datapoints[i];
const double curPointX = curPoint.x;
const double curPointY = curPoint.y * -1; //transform from image coordinates to geometric coordinates
double angleToEllipseCenter = atan2(curPointY - ellipseCenterY * -1, curPointX - ellipseCenterX); //ellipseCenterY * -1 for transformation to geometric coords (from image coords)
double nearestEllipseX = ellipseCenterX + ellipseRadiusX * cos(angleToEllipseCenter);
double nearestEllipseY = ellipseCenterY * -1 + ellipseRadiusY * sin(angleToEllipseCenter); //ellipseCenterY * -1 for transformation to geometric coords (from image coords)
cv::Point center(ellipseCenterX, ellipseCenterY);
cv::Size axes(ellipseRadiusX, ellipseRadiusY);
cv::ellipse(drawing, center, axes, 0, 0, 360, cv::Scalar(255));
cv::line(drawing, curPoint, cv::Point(nearestEllipseX,nearestEllipseY*-1), cv::Scalar(180));
}
cv::namedWindow( "ellipse", CV_WINDOW_AUTOSIZE );
cv::imshow( "ellipse", drawing );
cv::waitKey(0);
}
It produces the following image:
You can see that it actually finds "near" points on the ellipse, but it are not the "nearest" points. What I intentionally want is this: (excuse my poor drawing)
would you extent the lines in the last image, they would cross the center of the ellipse, but this is not the case for the lines in the previous image.
I hope you get the picture. Can anyone tell me what I am doing wrong?
Consider a bounding circle around the given point (c, d), which passes through the nearest point on the ellipse. From the diagram it is clear that the closest point is such that a line drawn from it to the given point must be perpendicular to the shared tangent of the ellipse and circle. Any other points would be outside the circle and so must be further away from the given point.
So the point you are looking for is not the intersection between the line and the ellipse, but the point (x, y) in the diagram.
Gradient of tangent:
Gradient of line:
Condition for perpedicular lines - product of gradients = -1:
When rearranged and substituted into the equation of your ellipse...
...this will give two nasty quartic (4th-degree polynomial) equations in terms of either x or y. AFAIK there are no general analytical (exact algebraic) methods to solve them. You could try an iterative method - look up the Newton-Raphson iterative root-finding algorithm.
Take a look at this very good paper on the subject:
http://www.spaceroots.org/documents/distance/distance-to-ellipse.pdf
Sorry for the incomplete answer - I totally blame the laws of mathematics and nature...
EDIT: oops, i seem to have a and b the wrong way round in the diagram xD
There is a relatively simple numerical method with better convergence than Newtons Method. I have a blog post about why it works http://wet-robots.ghost.io/simple-method-for-distance-to-ellipse/
This implementation works without any trig functions:
def solve(semi_major, semi_minor, p):
px = abs(p[0])
py = abs(p[1])
tx = 0.707
ty = 0.707
a = semi_major
b = semi_minor
for x in range(0, 3):
x = a * tx
y = b * ty
ex = (a*a - b*b) * tx**3 / a
ey = (b*b - a*a) * ty**3 / b
rx = x - ex
ry = y - ey
qx = px - ex
qy = py - ey
r = math.hypot(ry, rx)
q = math.hypot(qy, qx)
tx = min(1, max(0, (qx * r / q + ex) / a))
ty = min(1, max(0, (qy * r / q + ey) / b))
t = math.hypot(ty, tx)
tx /= t
ty /= t
return (math.copysign(a * tx, p[0]), math.copysign(b * ty, p[1]))
Credit to Adrian Stephens for the Trig-Free Optimization.
Here is the code translated to C# implemented from this paper to solve for the ellipse:
http://www.geometrictools.com/Documentation/DistancePointEllipseEllipsoid.pdf
Note that this code is untested - if you find any errors let me know.
//Pseudocode for robustly computing the closest ellipse point and distance to a query point. It
//is required that e0 >= e1 > 0, y0 >= 0, and y1 >= 0.
//e0,e1 = ellipse dimension 0 and 1, where 0 is greater and both are positive.
//y0,y1 = initial point on ellipse axis (center of ellipse is 0,0)
//x0,x1 = intersection point
double GetRoot ( double r0 , double z0 , double z1 , double g )
{
double n0 = r0*z0;
double s0 = z1 - 1;
double s1 = ( g < 0 ? 0 : Math.Sqrt(n0*n0+z1*z1) - 1 ) ;
double s = 0;
for ( int i = 0; i < maxIter; ++i ){
s = ( s0 + s1 ) / 2 ;
if ( s == s0 || s == s1 ) {break; }
double ratio0 = n0 /( s + r0 );
double ratio1 = z1 /( s + 1 );
g = ratio0*ratio0 + ratio1*ratio1 - 1 ;
if (g > 0) {s0 = s;} else if (g < 0) {s1 = s ;} else {break ;}
}
return s;
}
double DistancePointEllipse( double e0 , double e1 , double y0 , double y1 , out double x0 , out double x1)
{
double distance;
if ( y1 > 0){
if ( y0 > 0){
double z0 = y0 / e0;
double z1 = y1 / e1;
double g = z0*z0+z1*z1 - 1;
if ( g != 0){
double r0 = (e0/e1)*(e0/e1);
double sbar = GetRoot(r0 , z0 , z1 , g);
x0 = r0 * y0 /( sbar + r0 );
x1 = y1 /( sbar + 1 );
distance = Math.Sqrt( (x0-y0)*(x0-y0) + (x1-y1)*(x1-y1) );
}else{
x0 = y0;
x1 = y1;
distance = 0;
}
}
else // y0 == 0
x0 = 0 ; x1 = e1 ; distance = Math.Abs( y1 - e1 );
}else{ // y1 == 0
double numer0 = e0*y0 , denom0 = e0*e0 - e1*e1;
if ( numer0 < denom0 ){
double xde0 = numer0/denom0;
x0 = e0*xde0 ; x1 = e1*Math.Sqrt(1 - xde0*xde0 );
distance = Math.Sqrt( (x0-y0)*(x0-y0) + x1*x1 );
}else{
x0 = e0;
x1 = 0;
distance = Math.Abs( y0 - e0 );
}
}
return distance;
}
The following python code implements the equations described at "Distance from a Point to an Ellipse" and uses newton's method to find the roots and from that the closest point on the ellipse to the point.
Unfortunately, as can be seen from the example, it seems to only be accurate outside the ellipse. Within the ellipse weird things happen.
from math import sin, cos, atan2, pi, fabs
def ellipe_tan_dot(rx, ry, px, py, theta):
'''Dot product of the equation of the line formed by the point
with another point on the ellipse's boundary and the tangent of the ellipse
at that point on the boundary.
'''
return ((rx ** 2 - ry ** 2) * cos(theta) * sin(theta) -
px * rx * sin(theta) + py * ry * cos(theta))
def ellipe_tan_dot_derivative(rx, ry, px, py, theta):
'''The derivative of ellipe_tan_dot.
'''
return ((rx ** 2 - ry ** 2) * (cos(theta) ** 2 - sin(theta) ** 2) -
px * rx * cos(theta) - py * ry * sin(theta))
def estimate_distance(x, y, rx, ry, x0=0, y0=0, angle=0, error=1e-5):
'''Given a point (x, y), and an ellipse with major - minor axis (rx, ry),
its center at (x0, y0), and with a counter clockwise rotation of
`angle` degrees, will return the distance between the ellipse and the
closest point on the ellipses boundary.
'''
x -= x0
y -= y0
if angle:
# rotate the points onto an ellipse whose rx, and ry lay on the x, y
# axis
angle = -pi / 180. * angle
x, y = x * cos(angle) - y * sin(angle), x * sin(angle) + y * cos(angle)
theta = atan2(rx * y, ry * x)
while fabs(ellipe_tan_dot(rx, ry, x, y, theta)) > error:
theta -= ellipe_tan_dot(
rx, ry, x, y, theta) / \
ellipe_tan_dot_derivative(rx, ry, x, y, theta)
px, py = rx * cos(theta), ry * sin(theta)
return ((x - px) ** 2 + (y - py) ** 2) ** .5
Here's an example:
rx, ry = 12, 35 # major, minor ellipse axis
x0 = y0 = 50 # center point of the ellipse
angle = 45 # ellipse's rotation counter clockwise
sx, sy = s = 100, 100 # size of the canvas background
dist = np.zeros(s)
for x in range(sx):
for y in range(sy):
dist[x, y] = estimate_distance(x, y, rx, ry, x0, y0, angle)
plt.imshow(dist.T, extent=(0, sx, 0, sy), origin="lower")
plt.colorbar()
ax = plt.gca()
ellipse = Ellipse(xy=(x0, y0), width=2 * rx, height=2 * ry, angle=angle,
edgecolor='r', fc='None', linestyle='dashed')
ax.add_patch(ellipse)
plt.show()
Which generates an ellipse and the distance from the boundary of the ellipse as a heat map. As can be seen, at the boundary the distance is zero (deep blue).
Given an ellipse E in parametric form and a point P
the square of the distance between P and E(t) is
The minimum must satisfy
Using the trigonometric identities
and substituting
yields the following quartic equation:
Here's an example C function that solves the quartic directly and computes sin(t) and cos(t) for the nearest point on the ellipse:
void nearest(double a, double b, double x, double y, double *ecos_ret, double *esin_ret) {
double ax = fabs(a*x);
double by = fabs(b*y);
double r = b*b - a*a;
double c, d;
int switched = 0;
if (ax <= by) {
if (by == 0) {
if (r >= 0) { *ecos_ret = 1; *esin_ret = 0; }
else { *ecos_ret = 0; *esin_ret = 1; }
return;
}
c = (ax - r) / by;
d = (ax + r) / by;
} else {
c = (by + r) / ax;
d = (by - r) / ax;
switched = 1;
}
double cc = c*c;
double D0 = 12*(c*d + 1); // *-4
double D1 = 54*(d*d - cc); // *4
double D = D1*D1 + D0*D0*D0; // *16
double St;
if (D < 0) {
double t = sqrt(-D0); // *2
double phi = acos(D1 / (t*t*t));
St = 2*t*cos((1.0/3)*phi); // *2
} else {
double Q = cbrt(D1 + sqrt(D)); // *2
St = Q - D0 / Q; // *2
}
double p = 3*cc; // *-2
double SS = (1.0/3)*(p + St); // *4
double S = sqrt(SS); // *2
double q = 2*cc*c + 4*d; // *2
double l = sqrt(p - SS + q / S) - S - c; // *2
double ll = l*l; // *4
double ll4 = ll + 4; // *4
double esin = (4*l) / ll4;
double ecos = (4 - ll) / ll4;
if (switched) {
double t = esin;
esin = ecos;
ecos = t;
}
*ecos_ret = copysign(ecos, a*x);
*esin_ret = copysign(esin, b*y);
}
Try it online!
You just need to calculate the intersection of the line [P1,P0] to your elipse which is S1.
If the line equeation is:
and the elipse equesion is:
than the values of S1 will be:
Now you just need to calculate the distance between S1 to P1 , the formula (for A,B points) is:
I've solved the distance issue via focal points.
For every point on the ellipse
r1 + r2 = 2*a0
where
r1 - Euclidean distance from the given point to focal point 1
r2 - Euclidean distance from the given point to focal point 2
a0 - semimajor axis length
I can also calculate the r1 and r2 for any given point which gives me another ellipse that this point lies on that is concentric to the given ellipse. So the distance is
d = Abs((r1 + r2) / 2 - a0)
As propposed by user3235832
you shall solve quartic equation to find the normal to the ellipse (https://www.mathpages.com/home/kmath505/kmath505.htm). With good initial value only few iterations are needed (I use it myself). As an initial value I use S1 from your picture.
The fastest method I guess is
http://wwwf.imperial.ac.uk/~rn/distance2ellipse.pdf
Which has been mentioned also by Matt but as he found out the method doesn't work very well inside of ellipse.
The problem is the theta initialization.
I proposed an stable initialization:
Find the intersection of ellipse and horizontal line passing the point.
Find the other intersection using vertical line.
Choose the one that is closer the point.
Calculate the initial angle based on that point.
I got good results with no issue inside and outside:
As you can see in the following image it just iterated about 3 times to reach 1e-8. Close to axis it is 1 iteration.
The C++ code is here:
double initialAngle(double a, double b, double x, double y) {
auto abs_x = fabs(x);
auto abs_y = fabs(y);
bool isOutside = false;
if (abs_x > a || abs_y > b) isOutside = true;
double xd, yd;
if (!isOutside) {
xd = sqrt((1.0 - y * y / (b * b)) * (a * a));
if (abs_x > xd)
isOutside = true;
else {
yd = sqrt((1.0 - x * x / (a * a)) * (b * b));
if (abs_y > yd)
isOutside = true;
}
}
double t;
if (isOutside)
t = atan2(a * y, b * x); //The point is outside of ellipse
else {
//The point is inside
if (xd < yd) {
if (x < 0) xd = -xd;
t = atan2(y, xd);
}
else {
if (y < 0) yd = -yd;
t = atan2(yd, x);
}
}
return t;
}
double distanceToElipse(double a, double b, double x, double y, int maxIter = 10, double maxError = 1e-5) {
//std::cout <<"p="<< x << "," << y << std::endl;
auto a2mb2 = a * a - b * b;
double t = initialAngle(a, b, x, y);
auto ct = cos(t);
auto st = sin(t);
int i;
double err;
for (i = 0; i < maxIter; i++) {
auto f = a2mb2 * ct * st - x * a * st + y * b * ct;
auto fp = a2mb2 * (ct * ct - st * st) - x * a * ct - y * b * st;
auto t2 = t - f / fp;
err = fabs(t2 - t);
//std::cout << i + 1 << " " << err << std::endl;
t = t2;
ct = cos(t);
st = sin(t);
if (err < maxError) break;
}
auto dx = a * ct - x;
auto dy = b * st - y;
//std::cout << a * ct << "," << b * st << std::endl;
return sqrt(dx * dx + dy * dy);
}
I have been testing collision between two circles using the method:
Circle A = (x1,y1) Circle b = (x2,y2)
Radius A Radius b
x1 - x2 = x' * x'
y1 - y2 = y' * y'
x' + y' = distance
square root of distance - Radius A + Radius B
and if the resulting answer is a negative number it is intersecting.
I have used this method in a test but it doesn't seem to be very accurate at all.
bool circle::intersects(circle & test)
{
Vector temp;
temp.setX(centre.getX() - test.centre.getX());
temp.setY(centre.getY() - test.centre.getY());
float distance;
float temp2;
float xt;
xt = temp.getX();
temp2 = xt * xt;
temp.setX(temp2);
xt = temp.getY();
temp2 = xt * xt;
temp.setY(temp2);
xt = temp.getX() + temp.getY();
distance = sqrt(xt);
xt = radius + test.radius;
if( distance - xt < test.radius)
{
return true;
}
else return false;
}
This is the function using this method maybe I'm wrong here. I just wondered what other methods I could use. I know separating axis theorem is better , but I wouldn't know where to start.
if( distance - xt < test.radius)
{
return true;
}
distance - xt will evaluate to the blue line, the distance between the two disks. It also meets the condition of being less than the test radius, but there is no collision going on.
The solution:
if(distance <= (radius + test.radius) )
return true;
Where distance is the distance from the centres.
Given: xt = radius + test.radius;
The correct test is: if( distance < xt)
Here is an attempt to re-write the body for you: (no compiler, so may be errors)
bool circle::intersects(circle & test)
{
float x = this->centre.getX() - test.centre.getX()
float y = this->centre.getY() - test.centre.getY()
float distance = sqrt(x*x+y*y);
return distance < (this->radius + test.radius);
}
Based on Richard solution but comparing the squared distance. This reduce the computation errors and the computation time.
bool circle::intersects(circle & test)
{
float x = this->centre.getX() - test.centre.getX()
float y = this->centre.getY() - test.centre.getY()
float distance2 = x * x + y * y;
float intersect_distance2 = (this->radius + test.radius) * (this->radius + test.radius);
return distance <= intersect_distance2;
}
Use Pythagoras theorem to compute the distance between the centres
That is a straight line
If they have collided then that distance is shorter that the sum of the two radiuses