I have a 3D point in the world coordinates, (-140,-500,0) where z is the upwards vector, x is the depth and y is the horizontal
Now I want to convert this point to camera coordinates
I know that I need to calculate rotation matrix and translation
I have roll pitch and yaw and the camera position
I want to know if I am calculating the point in the camera coordinates correctly
//ax, ay and az are the position of the point in the real world
//cx, cy and cz are the position of the camera
//67.362312316894531 is pitch
//89.7135009765625 is roll
//0.033716827630996704 is yaw
double x = ax - cx;
double y = ay -cy;
double z = az - cz;
double cosx = cos(67.362312316894531);
double sinx = sin(67.362312316894531);
double cosy = cos(89.7135009765625);
double siny = sin(89.7135009765625);
double cosz = cos(0.033716827630996704);
double sinz = sin(0.033716827630996704);
dx = cosy * (sinz * y + cosz * x) - siny * z;
dy = sinx * (cosy * z + siny * (sinz * y + cosz * x)) + cosx * (cosz * y - sinz * x);
dz = cosx * (cosy * z + siny * (sinz * y + cosz * x)) - sinx * (cosz * y - sinz * x);
I know that the other method is to calculate the rotation matrix
//where yp is pitch, thet is roll and k is yaw
double rotxm[9] = { 1,0,0,0,cos(yp),-sin(yp),0,sin(yp),cos(yp) };
double rotym[9] = { cos(thet),0,sin(thet),0,1,0,-sin(thet),0,cos(thet) };
double rotzm[9] = { cos(k),-sin(k),0,sin(k),cos(k),0,0,0,1};
cv::Mat rotx = Mat{ 3,3,CV_64F,rotxm };
cv::Mat roty = Mat{ 3,3,CV_64F,rotym };
cv::Mat rotz = Mat{ 3,3,CV_64F,rotzm };
cv::Mat rotationm = rotz * roty * rotx;
my question are these two methods correct? or at least on of them.. how can I make sure of that
Related
I have a line with two points in latitude and longitude
A: 3.222895, 101.719751
B: 3.227511, 101.724318
and 1 point
C: 3.224972, 101.722932
How can I calculate minimum distance between point C and a line consists of point A and B?
It will be convenient if you can provide the calculation and objective-c code too. The distance is around 89 meters (using ruler in Google Earth).
Thanks to mimi and this great article http://www.movable-type.co.uk/scripts/latlong.html but they don't give the whole picture. Here is a detail one. All this points are collected using Google Earth using Placemark to mark the locations. Make sure lat/long are set to decimal degrees in Preferences.
lat A = 3.222895
lon A = 101.719751
lat B = 3.222895
lon B = 101.719751
lat C = 3.224972
lon C = 101.722932
Earth radius, R = 6371
1. First you have to find the bearing from A to C and A to B.
Bearing formula
bearingAC = atan2( sin(Δλ)*cos(φ₂), cos(φ₁)*sin(φ₂) − sin(φ₁)*cos(φ₂)*cos(Δλ) )
bearingAB = atan2( sin(Δλ)*cos(φ₂), cos(φ₁)*sin(φ₂) − sin(φ₁)*cos(φ₂)*cos(Δλ) )
φ is latitude, λ is longitude, R is earth radius
2. Find A to C distance using spherical law of cosines
distanceAC = acos( sin(φ₁)*sin(φ₂) + cos(φ₁)*cos(φ₂)*cos(Δλ) )*R
3. Find cross-track distance
distance = asin(sin(distanceAC/ R) * sin(bearingAC − bearing AB)) * R
Objective-C code
double lat1 = 3.227511;
double lon1 = 101.724318;
double lat2 = 3.222895;
double lon2 = 101.719751;
double lat3 = 3.224972;
double lon3 = 101.722932;
double y = sin(lon3 - lon1) * cos(lat3);
double x = cos(lat1) * sin(lat3) - sin(lat1) * cos(lat3) * cos(lat3 - lat1);
double bearing1 = radiansToDegrees(atan2(y, x));
bearing1 = 360 - ((bearing1 + 360) % 360);
double y2 = sin(lon2 - lon1) * cos(lat2);
double x2 = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lat2 - lat1);
double bearing2 = radiansToDegrees(atan2(y2, x2));
bearing2 = 360 - ((bearing2 + 360) % 360);
double lat1Rads = degreesToRadians(lat1);
double lat3Rads = degreesToRadians(lat3);
double dLon = degreesToRadians(lon3 - lon1);
double distanceAC = acos(sin(lat1Rads) * sin(lat3Rads)+cos(lat1Rads)*cos(lat3Rads)*cos(dLon)) * 6371;
double min_distance = fabs(asin(sin(distanceAC/6371)*sin(degreesToRadians(bearing1)-degreesToRadians(bearing2))) * 6371);
NSLog(#"bearing 1: %g", bearing1);
NSLog(#"bearing 2: %g", bearing2);
NSLog(#"distance AC: %g", distanceAC);
NSLog(#"min distance: %g", min_distance);
Actually there's a library for this. You can find it here https://github.com/100grams/CoreLocationUtils
Calculate bearing for each: C to A , and C to B:
var y = Math.sin(dLon) * Math.cos(lat2);
var x = Math.cos(lat1)*Math.sin(lat2) -
Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
var brng = Math.atan2(y, x).toDeg();
dLon= lon2-lon1;
Calculate cross-track distance:
var dXt = Math.asin(Math.sin(distance_CB/R)*Math.sin(bearing_CA-bearing_CB)) * R;
R is the radius of earth, dXt is the minimum distance you wanted to calculate.
Code to carry out this calculation is posted at here.
This implements an accurate solution in terms of ellipsoidal geodesics.
For the basic geodesic calculations, you can use
GeographicLib or the port of these algorithms to C which are included in version 4.9.0 of PROJ.4. This C interface is documented here.
Here's the result of compiling and running intercept.cpp:
$ echo 3.222895 101.719751 3.227511 101.724318 3.224972 101.722932 | ./intercept
Initial guess 3.225203 101.7220345
Increment 0.0003349040566247297 0.0003313413822354505
Increment -4.440892098500626e-16 0
Increment 0 0
...
Final result 3.225537904056624 101.7223658413822
Azimuth to A1 -135.1593040635131
Azimuth to A2 44.84069593652217
Azimuth to B1 134.8406959363608
Distance to line is 88.743m:
$ echo 3.224972 101.722932 3.225537904056624 101.7223658413822 | GeodSolve -i
-45.15927221 -45.15930407 88.743
See post here:
https://stackoverflow.com/a/33343505/4083623
For distance up to a few thousands meters I would simplify the issue from sphere to plane.
Then, the issue is pretty simply as a easy triangle calculation can be used:
We have points A and B and look for a distance X to line AB. Then:
Location a;
Location b;
Location x;
double ax = a.distanceTo(x);
double alfa = (Math.abs(a.bearingTo(b) - a.bearingTo(x))) / 180
* Math.PI;
double distance = Math.sin(alfa) * ax;
If you know how to calculate the distance of two points, get the distances between each two points, you get AB, AC, and BC. You want to know the closest distance between point C and line AB.
First get the value of P
P=（AB+BC+AC)/2
Using P, you need to get S
S=SQRT(（P(P-AC)（P-AB)(P-AC))
SQRT means square root. Then you get what you want by
2*S/AB
I am studying pArk Apple sample code, and how it is works.
anyone knows how convert the latitude and longitude to ECEF coordinates,
and Covert ECEF to ENU coordinates centered at given lat, lon functions are work?
I just want to understand what is going on in this function!
thanks.
void latLonToEcef(double lat, double lon, double alt, double *x, double *y, double *z)
{
double clat = cos(lat * DEGREES_TO_RADIANS);
double slat = sin(lat * DEGREES_TO_RADIANS);
double clon = cos(lon * DEGREES_TO_RADIANS);
double slon = sin(lon * DEGREES_TO_RADIANS);
double N = WGS84_A / sqrt(1.0 - WGS84_E * WGS84_E * slat * slat);
*x = (N + alt) * clat * clon;
*y = (N + alt) * clat * slon;
*z = (N * (1.0 - WGS84_E * WGS84_E) + alt) * slat;
}
// Coverts ECEF to ENU coordinates centered at given lat, lon
void ecefToEnu(double lat, double lon, double x, double y, double z, double xr, double yr, double zr, double *e, double *n, double *u)
{
double clat = cos(lat * DEGREES_TO_RADIANS);
double slat = sin(lat * DEGREES_TO_RADIANS);
double clon = cos(lon * DEGREES_TO_RADIANS);
double slon = sin(lon * DEGREES_TO_RADIANS);
double dx = x - xr;
double dy = y - yr;
double dz = z - zr;
*e = -slon*dx + clon*dy;
*n = -slat*clon*dx - slat*slon*dy + clat*dz;
*u = clat*clon*dx + clat*slon*dy + slat*dz;
}
The latLonToEcef method is an implementation of the algorithm outlined in the Geographic coordinate conversion - From geodetic to ECEF coordinates wikipedia page:
where
Φ is latitude, λ is longitude, and
Likewise the ecefToEnu method is an implementation of the ECEF to ENU algorithm:
If you need further references, they can be found at the bottom of that Wikipedia page. You might also refer to the World Geodetic System 1984 spec.
I am trying to determine the midpoint between two locations in an MKMapView. I am following the method outlined here (and here) and rewrote it in Objective-C, but the map is being centered somewhere northeast of Baffin Island, which is no where near the two points.
My method based on the java method linked above:
+(CLLocationCoordinate2D)findCenterPoint:(CLLocationCoordinate2D)_lo1 :(CLLocationCoordinate2D)_loc2 {
CLLocationCoordinate2D center;
double lon1 = _lo1.longitude * M_PI / 180;
double lon2 = _loc2.longitude * M_PI / 100;
double lat1 = _lo1.latitude * M_PI / 180;
double lat2 = _loc2.latitude * M_PI / 100;
double dLon = lon2 - lon1;
double x = cos(lat2) * cos(dLon);
double y = cos(lat2) * sin(dLon);
double lat3 = atan2( sin(lat1) + sin(lat2), sqrt((cos(lat1) + x) * (cos(lat1) + x) + y * y) );
double lon3 = lon1 + atan2(y, cos(lat1) + x);
center.latitude = lat3 * 180 / M_PI;
center.longitude = lon3 * 180 / M_PI;
return center;
}
The 2 parameters have the following data:
_loc1:
latitude = 45.4959839
longitude = -73.67826455
_loc2:
latitude = 45.482889
longitude = -73.57522299
The above are correctly place on the map (in and around Montreal). I am trying to center the map in the midpoint between the 2, yet my method return the following:
latitude = 65.29055
longitude = -82.55425
which somewhere in the arctic, when it should be around 500 miles south.
In case someone need code in Swift, I have written library function in Swift to calculate the midpoint between MULTIPLE coordinates:
// /** Degrees to Radian **/
class func degreeToRadian(angle:CLLocationDegrees) -> CGFloat {
return ( (CGFloat(angle)) / 180.0 * CGFloat(M_PI) )
}
// /** Radians to Degrees **/
class func radianToDegree(radian:CGFloat) -> CLLocationDegrees {
return CLLocationDegrees( radian * CGFloat(180.0 / M_PI) )
}
class func middlePointOfListMarkers(listCoords: [CLLocationCoordinate2D]) -> CLLocationCoordinate2D {
var x = 0.0 as CGFloat
var y = 0.0 as CGFloat
var z = 0.0 as CGFloat
for coordinate in listCoords{
var lat:CGFloat = degreeToRadian(coordinate.latitude)
var lon:CGFloat = degreeToRadian(coordinate.longitude)
x = x + cos(lat) * cos(lon)
y = y + cos(lat) * sin(lon)
z = z + sin(lat)
}
x = x/CGFloat(listCoords.count)
y = y/CGFloat(listCoords.count)
z = z/CGFloat(listCoords.count)
var resultLon: CGFloat = atan2(y, x)
var resultHyp: CGFloat = sqrt(x*x+y*y)
var resultLat:CGFloat = atan2(z, resultHyp)
var newLat = radianToDegree(resultLat)
var newLon = radianToDegree(resultLon)
var result:CLLocationCoordinate2D = CLLocationCoordinate2D(latitude: newLat, longitude: newLon)
return result
}
Detailed answer can be found here
Updated For Swift 5
func geographicMidpoint(betweenCoordinates coordinates: [CLLocationCoordinate2D]) -> CLLocationCoordinate2D {
guard coordinates.count > 1 else {
return coordinates.first ?? // return the only coordinate
CLLocationCoordinate2D(latitude: 0, longitude: 0) // return null island if no coordinates were given
}
var x = Double(0)
var y = Double(0)
var z = Double(0)
for coordinate in coordinates {
let lat = coordinate.latitude.toRadians()
let lon = coordinate.longitude.toRadians()
x += cos(lat) * cos(lon)
y += cos(lat) * sin(lon)
z += sin(lat)
}
x /= Double(coordinates.count)
y /= Double(coordinates.count)
z /= Double(coordinates.count)
let lon = atan2(y, x)
let hyp = sqrt(x * x + y * y)
let lat = atan2(z, hyp)
return CLLocationCoordinate2D(latitude: lat.toDegrees(), longitude: lon.toDegrees())
}
}
Just a hunch, but I noticed your lon2 and lat2 variables are being computed with M_PI/100 and not M_PI/180.
double lon1 = _lo1.longitude * M_PI / 180;
double lon2 = _loc2.longitude * M_PI / 100;
double lat1 = _lo1.latitude * M_PI / 180;
double lat2 = _loc2.latitude * M_PI / 100;
Changing those to 180 might help you out a bit.
For swift users, corrected variant as #dinjas suggest
import Foundation
import MapKit
extension CLLocationCoordinate2D {
// MARK: CLLocationCoordinate2D+MidPoint
func middleLocationWith(location:CLLocationCoordinate2D) -> CLLocationCoordinate2D {
let lon1 = longitude * M_PI / 180
let lon2 = location.longitude * M_PI / 180
let lat1 = latitude * M_PI / 180
let lat2 = location.latitude * M_PI / 180
let dLon = lon2 - lon1
let x = cos(lat2) * cos(dLon)
let y = cos(lat2) * sin(dLon)
let lat3 = atan2( sin(lat1) + sin(lat2), sqrt((cos(lat1) + x) * (cos(lat1) + x) + y * y) )
let lon3 = lon1 + atan2(y, cos(lat1) + x)
let center:CLLocationCoordinate2D = CLLocationCoordinate2DMake(lat3 * 180 / M_PI, lon3 * 180 / M_PI)
return center
}
}
It's important to say that the formula the OP used to calculate geographic midpoint is based on this formula which explains the cos/sin/sqrt calculation.
This formula will give you the geographic midpoint for any long distance including the four quarters and the prime meridian.
But, if your calculation is for short-range around 1 Kilometer, using a simple average will produce the same midpoint results.
i.e:
let firstPoint = CLLocation(....)
let secondPoint = CLLocation(....)
let midPointLat = (firstPoint.coordinate.latitude + secondPoint.coordinate.latitude) / 2
let midPointLong = (firstPoint.coordinate.longitude + secondPoint.coordinate.longitude) / 2
You can actually use it for 10km but expect a deviation - if you only need an estimation for a short range midpoint with a fast solution it will be sufficient.
I think you are over thinking it a bit. Just do:
float lon3 = ((lon1 + lon2) / 2)
float lat3 = ((lat1 + lat2) / 2)
lat3 and lon3 will be the center point.
I found an answer here, but can't understand how to transfer the math to Objective C
Find the third point
I have two points and I also have the angle relative to the axes. How do I find a third point which will form a straight line? The distance should be variable.
This is the code that I am using:
float distanceFromPx2toP3 = 1300.0;
float mag = sqrt(pow((px2.x - px1.x),2) + pow((px2.y - px1.y),2));
float P3x = px2.x + distanceFromPx2toP3 * (px2.x - px1.x) / mag;
float P3y = px2.y + distanceFromPx2toP3 * (px2.y - px1.y) / mag;
CGPoint P3 = CGPointMake(P3x, P3y);
Let's say I have two points pointA and pointB. The slope of the line formed by the two points m is:
static CGFloat calculateSlope(CGPoint pointA, CGPoint pointB) {
CGFloat m = (pointB.y - pointA.y) / (pointB.x - pointA.x);
return m;
}
A third point pointC a distance d from pointA on the line would be given by:
static CGPoint calculatePointOnLine(
CGPoint pointA, CGPoint pointB, CGFloat d, BOOL startAtB) {
CGFloat m = calculateSlope(pointA, pointB);
CGFloat dX = pointB.x - pointA.x;
CGFloat dY = pointB.y - pointA.y;
CGFloat signDX = dX / fabsf(dX);
CGFloat signDY = dY / fabsf(dY);
CGFloat dSquared = d * d;
CGFloat mSquared = m * m;
// We know pointC is distance d from pointA,
// and that pointA and pointC are on the
// same line
// dXSquared + dYSquared = dSquared
// m = dY / dX
// dY = m * dX
// dXSquared + mSquared * dXSquared = dSquared
// dXSquared * ( 1 + mSquared ) = dSquared
// dXSquared = dSquared / ( 1 + mSquared )
// Handle a vertical line, dX == 0, and a horizontal line, dY == 0
if (dX != 0 && dY != 0) {
// Account for the sign of dX
dX = signDX * sqrtf(dSquared / ( 1 + mSquared ));
// Account for the sign of dY
dY = signDY * m * dX;
}
// Handle a vertical line, dX == 0
if (dX == 0 && dY != 0) {
dY = signDY * d;
}
// Handle a horizontal line, dY == 0
if (dY == 0 && dX != 0) {
dX = signDX * d;
}
CGPoint startingPoint = pointA;
if (startAtB) {
startingPoint = pointB;
}
CGPoint pointC = CGMakePoint(startingPoint.x + dX,
startingPoint.y + dY);
return pointC;
}
pointC will now always lie a distance d along the line from pointA,
in the direction from pointA to pointB. Pass startAtB to have pointC
lie a distance d along the line from pointB, in the direction from
pointA to pointB.
Exchange the order of piintA and pointB in the call to calculatPointOnLine
to calculate a pointC which lies a distance d along the line from
PointB, in the direction from pointB to pointA.
You can use these two functions to calculate a third point on the line.
Thanks for accepting this answer if this helps you.
I have some problems figuring out where my error is. I got the following:
Have an image and corresponding GPS coordinates of its top-left and bottom-right vertices.
E.g:
topLeft.longitude = 8.235128;
topLeft.latitude = 49.632383;
bottomRight.longitude = 8.240547;
bottomRight.latitude = 49.629808;
Now a have an Point that lies in that map:
p.longitude = 8.238567;
p.latitude = 49.630664;
I draw my image in landscape fullscreen (1024*748).
Now I want to calculate the exact Pixel position (x,y) of my point.
For doing that I am trying to use the great circle distance approach from here: Link.
CGFloat DegreesToRadians(CGFloat degrees)
{
return degrees * M_PI / 180;
};
- (float) calculateDistanceP1:(CLLocationCoordinate2D)p1 andP2:(CLLocationCoordinate2D)p2 {
double circumference = 40000.0; // Erdumfang in km am Äquator
double distance = 0.0;
double latitude1Rad = DegreesToRadians(p1.latitude);
double longitude1Rad = DegreesToRadians(p1.longitude);
double latititude2Rad = DegreesToRadians(p2.latitude);
double longitude2Rad = DegreesToRadians(p2.longitude);
double logitudeDiff = fabs(longitude1Rad - longitude2Rad);
if (logitudeDiff > M_PI)
{
logitudeDiff = 2.0 * M_PI - logitudeDiff;
}
double angleCalculation =
acos(sin(latititude2Rad) * sin(latitude1Rad) + cos(latititude2Rad) * cos(latitude1Rad) * cos(logitudeDiff));
distance = circumference * angleCalculation / (2.0 * M_PI);
NSLog(#"%f",distance);
return distance;
}
Here is my code for getting the Pixel position:
- (CGPoint) calculatePoint:(CLLocationCoordinate2D)point {
float x_coord;
float y_coord;
CLLocationCoordinate2D x1;
CLLocationCoordinate2D x2;
x1.longitude = p.longitude;
x1.latitude = topLeft.latitude;
x2.longitude = p.longitude;
x2.latitude = bottomRight.latitude;
CLLocationCoordinate2D y1;
CLLocationCoordinate2D y2;
y1.longitude = topLeft.longitude;
y1.latitude = p.latitude;
y2.longitude = bottomRight.longitude;
y2.latitude = p.latitude;
float distanceX = [self calculateDistanceP1:x1 andP2:x2];
float distanceY = [self calculateDistanceP1:y1 andP2:y2];
float distancePX = [self calculateDistanceP1:x1 andP2:p];
float distancePY = [self calculateDistanceP1:y1 andP2:p];
x_coord = fabs(distancePX * (1024 / distanceX))-1;
y_coord = fabs(distancePY * (748 / distanceY))-1;
return CGPointMake(x_coord,y_coord);
}
x1 and x2 are the points on the longitude of p and with latitude of topLeft and bottomRight.
y1 and y2 are the points on the latitude of p and with longitude of topLeft and bottomRight.
So I got the distance between left and right on longitude of p and distance between top and bottom on latitude of p. (Needed for calculate the pixel position)
Now I calculate the distance between x1 and p (my distance between x_0 and x_p) after that I calculate the distance between y1 and p (distance between y_0 and y_p)
Last but not least the Pixel position is calculated and returned.
The Result is, that my point is on the red and NOT on the blue position:
Maybe you find any mistakes or have any suggestions for improving the accuracy.
Maybe I didn't understand your question, but shouldn't you be using the Converting Map Coordinates methods of MKMapView?
See this image
I used your co-ordinates, and simply did the following:
x_coord = 1024 * (p.longitude - topLeft.longitude)/(bottomRight.longitude - topLeft.longitude);
y_coord = 748 - (748 * (p.latitude - bottomRight.latitude)/(topLeft.latitude - bottomRight.latitude));
The red dot markes this point. For such small distances you don't really need to use great circles, and your rounding errors will be making things much more inaccurate