I want to solve a set of non linear equations in matlab. I mean lets say I have two points defined by (lat1,lon1) and (lat2,lon2). Now I want to find a point lat3,lon3 such that it is at a distance of 20km from both of the points. It is given by the intersection of the circles with radius 20km drawn with points (lat1,lon1) and (lat2,lon2) as center.
However, I am a bit confused about how to solve this equation.
I have the function to calculate the distance between two points in matlab
function [ distance ] = calculateDistance( latitude1,longitude1,latitude2,longitude2 )
radius = 6371;
dLat = degtorad(latitude2-latitude1);
dLon = degtorad(longitude2-longitude1);
a = sin(dLat/2) * sin(dLat/2) + cos(degtorad(latitude1)) * cos(degtorad(latitude2)) * sin(dLon/2) * sin(dLon/2);
c = 2 * atan2(sqrt(a), sqrt(1-a));
distance = radius * c;
end
and I am trying to use the solve function of matlab available at
http://www.mathworks.com/help/toolbox/symbolic/solve.html
However when I define
syms lat3 lon3
and try to get the equations to pass to the solve function it throws the error that
atan2 only accepts arguments of type sym.
How can I over this?
If you have to solve only that particular question, you do not need any equation solving functions of Matlab. You can simply use Pythagoras' formula:
If your points are (0,0) and (1,0) and the radius is x, then the two points which are x away from both (0,0) and (1,0) are
(0.5, sqrt (x^2 - 0.25) ) and (0.5, - sqrt (x^2 - 0.25)).
Now if your points are (a,b) and (c,d), then the distance of the two points is
dist = sqrt ( (c-a)^2 + (d-b)^2 ).
Ok, now we take a coordinate system where the origin is (a,b) and the unit is dist and the horizontal axis goes through (c,d). In this coordinate system, the points in question are
(0.5, +/- sqrt ( (r/dist)^2 - 0.25 ) ).
Now, to get to the original coordinate system, we have to multiply by dist, getting
(0.5 * dist, +/- sqrt ( r^2 - 0.25 * dist^2 ) ),
then rotate with the matrix rotating (dist, 0) to (c-a, d-b), which is
cos alpha -sin alpha
sin alpha cos alpha
where alpha = arccos ( (d-b) / dist), i.e. the matrix
(d-b) / dist -(c-a) / dist
(c-a) / dist (d-b) / dist
which gives
(0.5 (d-b) -/+ (c-a) sqrt (r^2 / dist^2 - 0.25),
0.5 (c-a) +/- (d-b) sqrt (r^2 / dist^2 - 0.25)
and finally add (a,b), yielding
(a + 0.5 (d-b) -/+ (c-a) sqrt (r^2 /dist^2 - 0.25),
b + 0.5 (c-a) +/- (d-b) sqrt (r^2 /dist^2 - 0.25) )
These are the points you are looking for. Probably I have made a mistake somewhere, but the direction should be clear, I hope.
Related
I have been struggeling with this quiz question. This was part of FSG 2022 registration quiz and I can't figure out how to solve it
At first I thought that I can use extrinsic and intrinsic parameters to calculate 3D coordinates using equations described by Mathworks or in this article. Later I realized that the distance to the object is provided in camera frame, which means that this could be treat as a depth camera and convert depth info into 3d space as described in medium.com article
this article is using formula show below to calculate x and y coordinates and is very similar to this question, yet I can't get the correct solution.
One of my Matlab scripts attempting to solve it:
rot = eul2rotm(deg2rad([102 0 90]));
trans = [500 160 1140]' / 1000; % mm to m
t = [rot trans];
u = 795; % here was typo as pointed out by solstad.
v = 467;
cx = 636;
cy = 548;
fx = 241;
fy = 238;
z = 2100 / 1000 % mm to m
tmp_x = (u - cx) * z / fx;
tmp_y = (v - cy) * z / fy;
% attempt 1
tmp_cords = [tmp_x; tmp_y; z; 1]
linsolve(t', tmp_cords)'
% result is: 1.8913 1.8319 -0.4292
% attempt 2
tmp_cords = [tmp_x; tmp_y; z]
rot * tmp_cords + trans
% result is: 2.2661 1.9518 0.4253
If possible I would like to see the calculation process not any kind of a python code.
Correct answer is under the image.
Correct solution provided by the organisers were 2.030, 1.272, 0.228 m
The task states that the object's euclidean (straight-line) distance is 2.1 m. That doesn't mean its distance along z is 2.1 m. Those two only coincide if there is no x or y component in the object's translation to the camera frame.
The z component of the object's translation will be less than 2.1 meters.
You need to take a ray/vector for the screen space coordinates (normalized) and multiply that by the euclidean distance.
v_x = (u - cx) / fx;
v_y = (v - cy) / fy;
v_z = 1;
v = [v_x; v_y; v_z];
dist = 2.1;
tmp = v / norm(v) * dist;
The rotation may be an issue. Roll happens around X, then pitch happens around Y, and then yaw happens around Z. These operations are applied in that order, i.e. inner to outer.
R_Z * R_Y * R_X * v
My rotation matrix is
[[ 0. 0.20791 0.97815]
[ 1. 0. 0. ]
[ 0. 0.97815 -0.20791]]
That camera, taking the usual (X right, Y down, Z far) frame, would be looking, upside down, out the windshield, and slightly down.
Make sure that eul2rotm() does the right thing (specify axis order as 'XYZ') or that you use something else.
You can use rotvec2mat3d() to build individual rotation matrices from an axis-angle encoding.
Perhaps also review different MATLAB conventions regarding matrix multiplication: https://www.mathworks.com/help/images/migrate-geometric-transformations-to-premultiply-convention.html
I used Python and scipy.spatial.transform.Rotation.from_euler('xyz', [R_roll, R_pitch, R_yaw], degrees=True).as_matrix() to arrive at the sample solution.
Properly, the task should have specified a frame conversion step between vehicle and camera because the differing views are quite confusing, with a car having +X being forward and a camera having +Z being forward...
In addition to Christoph Rackwitz answer, which is correct and should get all the credited, here is a working Matlab script:
rot = eul2rotm(deg2rad([90 0 102]));
trans = [500 160 1140]' / 1000; % mm to m
u = 795;
v = 467;
cx = 636;
cy = 548;
fx = 241;
fy = 238;
v_x = (u - cx) / fx;
v_y = (v - cy) / fy;
v_z = 1;
v = [v_x; v_y; v_z];
dist = 2.1;
tmp = v / norm(v) * dist;
rot * tmp + trans
I’m searching for a method that returns a CGPoint and Angle for each of n items around a rounded rect / squircle (I’m aware those shapes are different but suspect they don’t make a relevant visual difference in my case. Therefore I’m searching for the easiest solution).
Something like this:
func getCoordinates(of numberOfPoints: Int, in roundedRect: CGRect, with cornerRadius: CGFloat) -> [(CGPoint, Angle)] {
// ... NO IDEA HOW TO COMPUTE THIS
}
My ultimate goal is to draw something like this (points distributed with equal angles):
Unfortunately my math skills are not sufficient.
Pseudocode. Used center as cx, cy, w and h as half-width and half-height, r as corner radius.
Calculate angle in side for-loop, add phase to start from needed direction (0 from OX axis, Pi/2 from OY axis)
for (i = 0..n-1):
angle = i * 2 * math.pi / n + phase
Get unit vector components for this direction and absolute values
dx = cos(angle)
dy = sin(angle)
ax = abs(dx)
ay = abs(dy)
Find vertical or horizontal for this direction and calculate point relative to center (we work in the first quadrant at this moment):
if ax * h > ay * w:
x = w
y = w * ay / ax
else:
y = h
x = ax * h / ay
Now we have to correct result if point is in rounded corner:
if (x > w - r) and (y > h - r):
recalculate x and y as below
Here we have to find intersection of the ray with circle arc.
Circle equation
(x - (w-r))^2 + (y - (h-r))^2 = r^2
(x - wr)^2 + (y - hr)^2 = r^2 //wr = w - r, hr = h - r
Ray equation (t is parameter)
x = ax * t
y = ay * t
Substitute in circle eq:
(ax*t - wr)^2 + (ay*t - hr)^2 = r^2
ax^2*t^2 - 2*ax*t*wr + wr^2 + ay^2*t^2 -2*ay*t*hr + hr^2 -r^2 = 0
t^2*(ax^2+ay^2) + t*(-2*ax*wr - 2*ay*hr) + (wr^2 +hr^2 - r^2) = 0
t^2* a + t* b + c = 0
Solve this quadratic equation for unknown t, get larger root, and find intersection point substituting t into ray equation.
Now we want to put result into correct quadrant:
if dx < 0:
x = -x
if dy < 0:
y = -y
and shift them by center coordinates
dx += cx
dy += cy
That's all.
The known factors are O (center of the circle), R (radius), and X's position(and the distance between X and O naturally).
What I need to get here are the intersecting points of the straight line which always is perfectly vertical or horizontal. The circle is not a collider but just visual representation to show. Another fact in this is that X is always inside the range(within the radius) of the circle which means both x1 and x2 will always be at the radius distance from O.
Time to time I get really bummed out when trying to utilize known mathematical equations with coding and this is no different.
If I understand your problem correctly,
X is a "random" point inside your circle whose position is known : X = (X.x, X.y).
Supposing you want to know X1 and X2 on the circle so that (X1X2) is a vertical line. You know the absicca of those points which is X.x.
You surely know that the coordinates of a point on a circle can be defined as : A = (O.x + r * cos θ, O.y + r * sin θ) where :
O is the origin of the circle
r is the radius of your circle
θ is an angle in radians.
Thus,
X1 = (O.x + r * cos θ1, O.y + r * sin θ1) = (X.x, O.y + r * sin θ1)
which means
X.x = O.x + r * cos θ1
<=> cos θ1 = (X.x - O.x) / r <=> θ1 = arcos( (X.x - O.x) / r )
Once you know θ1, computing sin θ1 and X1.y is a piece of cake.
Computing X2 is very easy, you just have to invert the ordinate.
If (X1X2) is an horizontal line, you know the ordinates of X1 and X2 :
X1 = (O.x + r * cos θ1, O.y + r * sin θ1) = (O.x + r * cos θ1, X.y)
X.y = O.y + r * sin θ1
<=> sin θ1 = (X.y - O.y) / r <=> θ1 = arcsin( (X.y - O.y) / r )
Here, to get X2 you have to invert the absicca.
I have some Matlab code of the following form:
syms theta x
theta = acos(x)
This returns a single solution for theta. However, I want to return the complete solution (between some limits).
For example,
x = cos(theta)
would give x=0.5 for theta = 60 degrees, 120 degrees, 420 degrees, etc. Therefore, in my code above, I want theta to return all these possible values.
Does anyone know how to do this? I have been searching google for hours but I can't find how to do this!
Many thanks!
Here's a numerical solution; like Benoit_11 I don't see the point of doing it symbolically in this context.
There are two solutions within the interval [-pi, pi], the larger one being returned by acos:
% solution within [0, pi]
theta1 = acos(x);
% solution within [-pi, 0]
theta2 = -acos(x);
These solutions repeat at steps of 2 pi. The number of possible steps downwards and upwards can be determined by the integer part of the distance between the basic solution and the respective limit (lower and upper), in units of 2 pi. For theta1:
% repetitions in 2 pi intervals within limits
sd = floor((theta1 - lower) / (2 * pi));
su = floor((upper - theta1) / (2 * pi));
theta1 = (-sd : su) * 2 * pi + theta1
And the same for theta2:
% repetitions in 2 pi intervals within limits
sd = floor((theta2 - lower) / (2 * pi));
su = floor((upper - theta2) / (2 * pi));
theta2 = (-sd : su) * 2 * pi + theta2
If you'd like one combined list of solutions, excluding possible duplicates:
theta = unique([theta1, theta2])
and in degrees:
theta = theta / pi * 180;
Example:
x = 0.5;
lower = -10;
upper = 30;
gives
theta =
-7.3304 -5.2360 -1.0472 1.0472 5.2360 7.3304 11.5192 13.6136 17.8024 19.8968 24.0855 26.1799
You just have to use a loop:
for x = limit_inf:step:limit_sup
theta(x) = acos(x);
end
And also define your limits and the step between them.
I have tried several different solutions but no luck so far.
- (CGPoint)contractLineTemp:(CGPoint)point :(CGPoint)circle :(float)circleRadius {
CGFloat x,y;
x = point.x - circle.x;
y = point.y - circle.y;
CGFloat theta = atan2(x, y);
CGPoint newPoint;
newPoint.x = circle.x + circleRadius * sin(theta);
newPoint.y = circle.y + circleRadius * cos(theta);
return newPoint;
}
- (CGPoint)contractLineTemp:(CGPoint)startPoint :(CGPoint)endPoint :(float)scaleBy {
float dx = endPoint.x - startPoint.x;
float dy = endPoint.y - startPoint.y;
float scale = scaleBy * Q_rsqrt(dx * dx + dy * dy);
return CGPointMake (endPoint.x - dx * scale, endPoint.y - dy * scale);
}
Both of these solutions kind of work. If I draw the line to the center of the circle you can see that it intersects the circle exactly where it should.
http://www.freeimagehosting.net/le5pi
If I use either of the solutions above and draw to the circumference of the circle depending on the angle it is no longer going towards the center of the circle. In the second image the line should be in the middle of the right edge of the circle and going straight right.
http://www.freeimagehosting.net/53ovs
http://www.freeimagehosting.net/sb3b2
Sorry for the links. I am to new to currently post images.
Thanks for you help.
It's easier to treat this as a vector problem. Your second approach is close, but you don't correctly scale the vector between the two points. It's easier to work with a normalized vector in this case, although you have to assume that the distance between the two points on the line is non-zero.
Given:
double x0 = CIRC_X0; /* x-coord of center of circle */
double y0 = CIRC_Y0; /* y-coord of center of circle */
double x1 = LINE_X1; /* x-coord of other point on the line */
double y1 = LINE_Y1; /* y-coord of other point on the line */
Then the vector between the two points is (vx,vy):
double vx = x1 - x0;
double vy = y1 - y0;
It's easier to work with a unit vector, which we can get by normalizing (vx,vy):
double vmag = sqrt(vx*vx + vy*vy);
vx /= vmag; /* Assumption is vmag > 0 */
vy /= vmag;
Now, any point along the line can be described as:
x0 + dist * vx
y0 + dist * vy
where dist is the distance from the center. The intersection of the circle and the line must be a distance of CIRC_RADIUS from the center, so:
double x_intersect = x0 + CIRC_RADIUS * vx;
double y_intersect = y0 + CIRC_RADIUS * vy;
I think that there may be a convention conflict on what theta, x and y are. The atan2 function yields values in the range -pi..pi, by taking the convention of theta as the angle growing from the X axis towards Y. However you are considering theta as the angle from Y to X.
Try changing the code:
CGFloat theta = atan2(y, x);
CGPoint newPoint;
newPoint.x = circle.x + circleRadius * cos(theta);
newPoint.y = circle.y + circleRadius * sin(theta);
Although your formulae are consistent within a coordinate system, it may have conflict with the screen/display device coordinate system.