Develop Reference

how to draw a line knowing the starting coordinate, line length x angle and y, angle

I am trying to draw a line knowing the starting coordinate, line length x angle and y, angle. I am trying to visualize the eye gaze direction of a human in a video.
For an example, I want to draw the blue lines here:
I already have the center point of the eye (start of the blue line), and I already have the x and y angle of the eye. How do I draw this line?
I've found a lot of tutorials on how to draw such lines (https://learnopencv.com/head-pose-estimation-using-opencv-and-dlib/), but they involve translation and rotation vectors. Is there a clean way to do this just using the known x/y angles?

Given a length r and an angle φ between the x-axis and your line to draw, you can calculate a coordinate for a point P(x, y) like this:
x = r * cos(φ)
y = r * sin(φ)
For example, a vertical line with r = 1; φ = 90° = π / 2 would return y = 1 * sin(π / 2) = 1
Then you can import your image in opencv and draw a line using cv2.line.

It’s basically just math:
As far as I know, the height of the triangle is height = math.sinus(h) * l and its width is width = math.cos(h) * l.
I hope this help.

Move xy coordinates based on a given distance and bearing from transect

I have a boat moving along a transect, looking for animals. Someone is stood on the top of the boat, facing forward, and is logging the distance from the boat and the bearing from the front of the boat when an animal is seen. I have this information as well as the xy coordinates of the boat at the point at which the animal was seen. I need to get the xy coordinates of the animal itself based on this information.
I don't have the original compass bearing of the boat, which makes this tricky; but what I do have is the next GPS (xy) coordinate of the boat, from which I can calculate a starting angle. From this, it should be possible to add or subtract the bearing at which the animal was seen to give a normalised angle which can be used to find the xy coordinates of the animal using trigonometry. Unfortunately my maths skills aren't quite up to the job.
I have several hundred points so I need to put this into a Python script to go through all the points.
In summary, the dataset has:
Original X, Original Y, End(next) X, End(next) Y, Bearing, Distance
EDIT: Sorry, I was in a rush and didn't explain this very well.
I see there being 3 stages to this problem.
Finding the original bearing of the transect
Finding the bearing of the point relative to the transect
Finding the new coordinates of the point based on this normalised angle and distance from the boat at the start xy
The Python code I had originally is below, although it's not much use - the figures given are examples.
distFromBoat = 100
bearing = 45
lengthOpposite = origX-nextX
lengthAdjacent = origY - nextY
virtX = origX #virtual X
virtY = origY-lengthOpposite #virtual Y
angle = math.degrees(math.asin(math.radians((lengthOpposite/transectLen))))
newangle = angle + bearing
newLenAdj = math.cos(newangle)*distFromBoat
newLenOpp = math.sqrt(math.pow(distFromBoat,2) + math.pow(newLenAdj,2) - 2*(distFromBoat*newLenAdj)*(math.cos(newangle)))
newX = virtX-newLenOpp
newY = origY-newLenAdj
print str(newX) +"---"+str(newY)
Thanks for any help in advance!

There is a little problem with Matt's function, so I used atan2 to get you the angle that the boat is going.
Edit: That was more complicated than I expected. In the end you need to subtract 90 and take the inverse to go from the geo-referenced angles to the trig angles.
(There is also an angles library (and probably other geography ones) that have this built in.
Now this takes the origX and origY, finds the trig angle and converts it to a heading, adds the bearing to the angle determined for the transect. Then it does trig on the distance, but using the angle converted back to trig degrees -(X-90). It is kind of warped, because we are used to thinking of 0 degrees as north/up, but in trig it is "to the right", and trig goes counter clockwise vs clockwise for navigation.
import math
origX = 0.0
origY = 0.0
nextX = 0.0
nextY = -1.0
distance = 100.0
bearing = 45
def angle(origX,origY,nextX,nextY):
opp = float(nextY - origY)
adj = float(nextX - origX)
return(math.degrees(math.atan2(adj,opp)))
# atan2 seems to even work correctly (return zero) when origin equals next
transectAngle = angle(origX,origY,nextX,nextY) # assuming the function has been defined
print "bearing plus trans", transectAngle + bearing
trigAngle = -(transectAngle + bearing -90)
print "trig equiv angle", trigAngle
newX = origX + distance * math.cos(math.radians(trigAngle))
newY = origY + distance * math.sin(math.radians(trigAngle))
print "position",newX,newY
Output:
-70.7106781187 -70.7106781187
Here is a function to print out a bunch of test cases (uses global vars so should be folded into the code above)
def testcase():
bearinglist = [-45,45,135,-135]
dist = 10
for bearing in bearinglist:
print "----transect assuming relative bearing of {}------".format(bearing)
print "{:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6}".format("x","y","tran","head","trigT","trigH","newX","newY")
for x in [0,.5,-.5]:
for y in [0,.5,1,-.5]:
# print "A:", x,y,angle(origX,origY,x,y)
tA = newangle(origX,origY,x,y)
trigA = -(tA-90)
heading = tA + bearing
trigHead = -(heading-90)
Xnew = distance * math.cos(math.radians(trigHead))
Ynew = distance * math.sin(math.radians(trigHead))
print "{:>6.1f} {:>6.1f} {:>6.1f} {:>6.1f} {:>6.1f} {:>6.1f} {:>6.1f} {:>6.1f}".format(x,y,tA,heading,trigA,trigHead,Xnew,Ynew)

From what I understand, this is your problem:
You have 2 points, start and next that you are walking between
You want to find the coordinates of a third point, New that should be some distance and bearing from start, given that you are already facing from start to next.
My solution is this:
Create a normalized vector from start to next
Rotate your normalized vector by given bearing
Multiply the normalized, rotated vector by your distance, and then add it to start
treating start as a vector, the result of the addition is your new point
Because a counter-clockwise rotation ("left" from the current point) is considered positive, you need to invert bearing so that port is associated with negative, and starboard with positive.
Code
import math
origX = 95485
origY = 729380
nextX = 95241
nextY = 729215
distance = 2000.0
bearing = 45
origVec = origX, origY
nextVec = nextX, nextY
#Euclidean distance between vectors (L2 norm)
dist = math.sqrt((nextVec[0] - origVec[0])**2 + (nextVec[1] - origVec[1])**2)
#Get a normalized difference vector
diffVec = (nextVec[0] - origVec[0])/dist, (nextVec[1] - origVec[1])/dist
#rotate our vector by bearing to get a vector from orig towards new point
#also, multiply by distance to get new value
#invert bearing, because +45 in math is counter-clockwise (left), not starboard
angle = math.radians(-bearing)
newVec = origVec[0]+(diffVec[0]*math.cos(angle) - diffVec[1]*math.sin(angle))*distance, \
origVec[1]+(diffVec[0]*math.sin(angle) + diffVec[1]*math.cos(angle))*distance
print newVec
Output:
(93521.29597031244, 729759.2973553676)

It's (probably) possible there's a more elegant solution than this...assuming I understood your issue correctly. But, this will give you the bearing from your original location:
(inputs hard-coded as an example)
import math
origX = 0.0
origY = 0.0
nextX = 1.0
nextY = 0.0
Dist = ((nextX - origX)**2 + (nextY - origY)**2)**0.5
if origX == nextX and origY == nextY:
angle = 0
if origX == nextX and nextY < origY:
angle = 180
if nextY < origY and origX > nextX:
angle = math.degrees(math.asin((nextX -origX)/Dist)) - 90
if nextX > origX and nextY < origY:
angle = math.degrees(math.asin((nextX -origX)/Dist)) + 90
else:
angle = math.degrees(math.asin((nextX -origX)/Dist))
print angle

How to generate regular points on cylindrical surface

I am a beginner in Python and I have to work on a project using Numpy.
I need to generate some points (e.g. one million) on one part of the surface of a cylinder. These points should be regularly distributed on a subregion of the surface defined by a given angle. How could I go about doing this?
My input parameters are:
position of the center of cylinder (e.g. [0,0,0] )
the orientation of cylinder
length of cylinder
radius of cylinder
angle (this defines the part of cylinder which the points should be distributed on it.) for alpha = 360, the whole surface
delta_l is the distance between each two points in the length direction
delta_alpha is the distance between each two points in the alpha (rotation) direction
My output parameters :
an array containing the coordinates of all points
Could anyone help me, or give me a hint about how to do this?
Many thanks

This is taken from a previous project of mine:
def make_cylinder(radius, length, nlength, alpha, nalpha, center, orientation):
#Create the length array
I = np.linspace(0, length, nlength)
#Create alpha array avoid duplication of endpoints
#Conditional should be changed to meet your requirements
if int(alpha) == 360:
A = np.linspace(0, alpha, num=nalpha, endpoint=False)/180*np.pi
else:
A = np.linspace(0, alpha, num=nalpha)/180*np.pi
#Calculate X and Y
X = radius * np.cos(A)
Y = radius * np.sin(A)
#Tile/repeat indices so all unique pairs are present
pz = np.tile(I, nalpha)
px = np.repeat(X, nlength)
py = np.repeat(Y, nlength)
points = np.vstack(( pz, px, py )).T
#Shift to center
shift = np.array(center) - np.mean(points, axis=0)
points += shift
#Orient tube to new vector
#Grabbed from an old unutbu answer
def rotation_matrix(axis,theta):
a = np.cos(theta/2)
b,c,d = -axis*np.sin(theta/2)
return np.array([[a*a+b*b-c*c-d*d, 2*(b*c-a*d), 2*(b*d+a*c)],
[2*(b*c+a*d), a*a+c*c-b*b-d*d, 2*(c*d-a*b)],
[2*(b*d-a*c), 2*(c*d+a*b), a*a+d*d-b*b-c*c]])
ovec = orientation / np.linalg.norm(orientation)
cylvec = np.array([1,0,0])
if np.allclose(cylvec, ovec):
return points
#Get orthogonal axis and rotation
oaxis = np.cross(ovec, cylvec)
rot = np.arccos(np.dot(ovec, cylvec))
R = rotation_matrix(oaxis, rot)
return points.dot(R)
Plotted points for:
points = make_cylinder(3, 5, 5, 360, 10, [0,2,0], [1,0,0])
The rotation part is quick and dirty- you should likely double check it. Euler-Rodrigues formula thanks to unutbu.

python optimize.leastsq: fitting a circle to 3d set of points

I am trying to use circle fitting code for 3D data set. I have modified it for 3D points just adding z-coordinate where necessary. My modification works fine for one set of points and works bad for another. Please look at the code, if it has some errors.
import trig_items
import numpy as np
from trig_items import *
from numpy import *
from matplotlib import pyplot as p
from scipy import optimize
# Coordinates of the 3D points
##x = r_[36, 36, 19, 18, 33, 26]
##y = r_[14, 10, 28, 31, 18, 26]
##z = r_[0, 1, 2, 3, 4, 5]
x = r_[ 2144.18908574, 2144.26880854, 2144.05552972, 2143.90303742, 2143.62520676,
2143.43628579, 2143.14005775, 2142.79919654, 2142.51436023, 2142.11240866,
2141.68564346, 2141.29333828, 2140.92596405, 2140.3475612, 2139.90848046,
2139.24661021, 2138.67384709, 2138.03313547, 2137.40301734, 2137.40908256,
2137.06611224, 2136.50943781, 2136.0553113, 2135.50313189, 2135.07049922,
2134.62098139, 2134.10459535, 2133.50838433, 2130.6600465, 2130.03537342,
2130.04047644, 2128.83522468, 2127.79827542, 2126.43513385, 2125.36700593,
2124.00350543, 2122.68564431, 2121.20709478, 2119.79047011, 2118.38417647,
2116.90063343, 2115.52685778, 2113.82246629, 2112.21159431, 2110.63180117,
2109.00713198, 2108.94434529, 2106.82777156, 2100.62343757, 2098.5090226,
2096.28787738, 2093.91550703, 2091.66075061, 2089.15316429, 2086.69753869,
2084.3002414, 2081.87590579, 2079.19141866, 2076.5394574, 2073.89128676,
2071.18786213]
y = r_[ 725.74913818, 724.43874065, 723.15226506, 720.45950581, 717.77827954,
715.07048092, 712.39633862, 709.73267688, 707.06039438, 704.43405908,
701.80074596, 699.15371526, 696.5309022, 693.96109921, 691.35585501,
688.83496327, 686.32148661, 683.80286662, 681.30705568, 681.30530975,
679.66483676, 678.01922321, 676.32721779, 674.6667554, 672.9658024,
671.23686095, 669.52021535, 667.84999077, 659.19757984, 657.46179949,
657.45700508, 654.46901086, 651.38177517, 648.41739432, 645.32356976,
642.39034578, 639.42628453, 636.51107198, 633.57732055, 630.63825133,
627.75308356, 624.80162215, 622.01980232, 619.18814892, 616.37688894,
613.57400131, 613.61535723, 610.4724493, 600.98277781, 597.84782844,
594.75983001, 591.77946964, 588.74874068, 585.84525834, 582.92311166,
579.99564481, 577.06666417, 574.30782762, 571.54115037, 568.79760614,
566.08551098]
z = r_[ 339.77146775, 339.60021095, 339.47645894, 339.47130963, 339.37216218,
339.4126132, 339.67942046, 339.40917728, 339.39500353, 339.15041461,
339.38959195, 339.3358209, 339.47764895, 339.17854867, 339.14624071,
339.16403926, 339.02308811, 339.27011082, 338.97684183, 338.95087698,
338.97321177, 339.02175448, 339.02543922, 338.88725411, 339.06942374,
339.0557553, 339.04414618, 338.89234303, 338.95572249, 339.00880416,
339.00413073, 338.91080374, 338.98214758, 339.01135789, 338.96393537,
338.73446188, 338.62784913, 338.72443217, 338.74880562, 338.69090173,
338.50765186, 338.49056867, 338.57353355, 338.6196255, 338.43754399,
338.27218569, 338.10587265, 338.43880881, 338.28962141, 338.14338705,
338.25784154, 338.49792568, 338.15572139, 338.52967693, 338.4594245,
338.1511823, 338.03711207, 338.19144663, 338.22022045, 338.29032321,
337.8623197 ]
# coordinates of the barycenter
xm = mean(x)
ym = mean(y)
zm = mean(z)
### Basic usage of optimize.leastsq
def calc_R(xc, yc, zc):
""" calculate the distance of each 3D points from the center (xc, yc, zc) """
return sqrt((x - xc) ** 2 + (y - yc) ** 2 + (z - zc) ** 2)
def func(c):
""" calculate the algebraic distance between the 3D points and the mean circle centered at c=(xc, yc, zc) """
Ri = calc_R(*c)
return Ri - Ri.mean()
center_estimate = xm, ym, zm
center, ier = optimize.leastsq(func, center_estimate)
##print center
xc, yc, zc = center
Ri = calc_R(xc, yc, zc)
R = Ri.mean()
residu = sum((Ri - R)**2)
print 'R =', R
So, for the first set of x, y, z (commented in the code) it works well: the output is R = 39.0097846735. If I run the code with the second set of points (uncommented) the resulting radius is R = 108576.859834, which is almost straight line. I plotted the last one.
The blue points is a given data set, the red ones is the arc of the resulting radius R = 108576.859834. It is obvious that the given data set has much smaller radius than the result.
Here is another set of points.
It is clear that the least squares does not work correctly.
Please help me solving this issue.
UPDATE
Here is my solution:
### fit 3D arc into a set of 3D points ###
### output is the centre and the radius of the arc ###
def fitArc3d(arr, eps = 0.0001):
# Coordinates of the 3D points
x = numpy.array([arr[k][0] for k in range(len(arr))])
y = numpy.array([arr[k][4] for k in range(len(arr))])
z = numpy.array([arr[k][5] for k in range(len(arr))])
# coordinates of the barycenter
xm = mean(x)
ym = mean(y)
zm = mean(z)
### gradient descent minimisation method ###
pnts = [[x[k], y[k], z[k]] for k in range(len(x))]
meanP = Point(xm, ym, zm) # mean point
Ri = [Point(*meanP).distance(Point(*pnts[k])) for k in range(len(pnts))] # radii to the points
Rm = math.fsum(Ri) / len(Ri) # mean radius
dR = Rm + 10 # difference between mean radii
alpha = 0.1
c = meanP
cArr = []
while dR > eps:
cArr.append(c)
Jx = math.fsum([2 * (x[k] - c[0]) * (Ri[k] - Rm) / Ri[k] for k in range(len(Ri))])
Jy = math.fsum([2 * (y[k] - c[1]) * (Ri[k] - Rm) / Ri[k] for k in range(len(Ri))])
Jz = math.fsum([2 * (z[k] - c[2]) * (Ri[k] - Rm) / Ri[k] for k in range(len(Ri))])
gradJ = [Jx, Jy, Jz] # find gradient
c = [c[k] + alpha * gradJ[k] for k in range(len(c)) if len(c) == len(gradJ)] # find new centre point
Ri = [Point(*c).distance(Point(*pnts[k])) for k in range(len(pnts))] # calculate new radii
RmOld = Rm
Rm = math.fsum(Ri) / len(Ri) # calculate new mean radius
dR = abs(Rm - RmOld) # new difference between mean radii
return Point(*c), Rm
It is not very optimal code (I do not have time to fine tune it) but it works.

I guess the problem is the data and the corresponding algorithm. The least square method works fine if it produces a local parabolic minimum, such that a simple gradient method goes approximately direction minimum. Unfortunately, this is not necessarily the case for your data. You can check this by keeping some rough estimates for xc and yc fixed and plotting the sum of the squared residuals as a function of zc and R. I get a boomerang shaped minimum. Depending on your starting parameters you might end in one of the branches going away from the real minimum. Once in the valley this can be very flat such that you exceed the number of max iterations or get something that is accepted within the tolerance of the algorithm. As always, thinks are better the better your starting parameters. Unfortunately you have only a small arc of the circle, so that it is difficult to get better. I am not a specialist in Python, but I think that leastsq allows you to play with the Jacobian and Gradient Methods. Try to play with the tolerance as well.
In short: the code looks basically fine to me, but your data is pathological and you have to adapt the code to that kind of data.
There is a non-iterative solution in 2D from Karimäki, maybe you can adapt
this method to 3D. You can also look at this. Sure you will find more literature.
I just checked the data using a Simplex-Algorithm. The minimum is, as I said, not well behaved. See here some cuts of the residual function. Only in the xy-plane you get some reasonable behavior. The properties of the zr- and xr- plane make the finding process very difficult.
So in the beginning the simplex algorithm finds several almost stable solutions. You can see them as flat steps in the graph below (blue x, purple y, yellow z, green R). At the end the algorithm has to walk down the almost flat but very stretched out valley, resulting in the final conversion of z and R. Nevertheless, I expect many regions that look like a solution if the tolerance is insufficient. With the standard tolerance of 10^-5 the algoritm stopped after approx 350 iterations. I had to set it to 10^-10 to get this solution, i.e. [1899.32, 741.874, 298.696, 248.956], which seems quite ok.
Update
As mentioned earlier, the solution depends on the working precision and requested accuracy. So your hand made gradient method works probably better as these values are different compared to the build-in least square fit. Nevertheless, this is my version making a two step fit. First I fit a plane to the data. In a next step I fit a circle within this plane. Both steps use the least square method. This time it works, as each step avoids critically shaped minima. (Naturally, the plane fit runs into problems if the arc segment becomes small and the data lies virtually on a straight line. But this will happen for all algorithms)
from math import *
from matplotlib import pyplot as plt
from scipy import optimize
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import pprint as pp
dataTupel=zip(xs,ys,zs) #your data from above
# Fitting a plane first
# let the affine plane be defined by two vectors,
# the zero point P0 and the plane normal n0
# a point p is member of the plane if (p-p0).n0 = 0
def distanceToPlane(p0,n0,p):
return np.dot(np.array(n0),np.array(p)-np.array(p0))
def residualsPlane(parameters,dataPoint):
px,py,pz,theta,phi = parameters
nx,ny,nz =sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta)
distances = [distanceToPlane([px,py,pz],[nx,ny,nz],[x,y,z]) for x,y,z in dataPoint]
return distances
estimate = [1900, 700, 335,0,0] # px,py,pz and zeta, phi
#you may automize this by using the center of mass data
# note that the normal vector is given in polar coordinates
bestFitValues, ier = optimize.leastsq(residualsPlane, estimate, args=(dataTupel))
xF,yF,zF,tF,pF = bestFitValues
point = [xF,yF,zF]
normal = [sin(tF)*cos(pF),sin(tF)*sin(pF),cos(tF)]
# Fitting a circle inside the plane
#creating two inplane vectors
sArr=np.cross(np.array([1,0,0]),np.array(normal))#assuming that normal not parallel x!
sArr=sArr/np.linalg.norm(sArr)
rArr=np.cross(sArr,np.array(normal))
rArr=rArr/np.linalg.norm(rArr)#should be normalized already, but anyhow
def residualsCircle(parameters,dataPoint):
r,s,Ri = parameters
planePointArr = s*sArr + r*rArr + np.array(point)
distance = [ np.linalg.norm( planePointArr-np.array([x,y,z])) for x,y,z in dataPoint]
res = [(Ri-dist) for dist in distance]
return res
estimateCircle = [0, 0, 335] # px,py,pz and zeta, phi
bestCircleFitValues, ier = optimize.leastsq(residualsCircle, estimateCircle,args=(dataTupel))
rF,sF,RiF = bestCircleFitValues
print bestCircleFitValues
# Synthetic Data
centerPointArr=sF*sArr + rF*rArr + np.array(point)
synthetic=[list(centerPointArr+ RiF*cos(phi)*rArr+RiF*sin(phi)*sArr) for phi in np.linspace(0, 2*pi,50)]
[cxTupel,cyTupel,czTupel]=[ x for x in zip(*synthetic)]
### Plotting
d = -np.dot(np.array(point),np.array(normal))# dot product
# create x,y mesh
xx, yy = np.meshgrid(np.linspace(2000,2200,10), np.linspace(540,740,10))
# calculate corresponding z
# Note: does not work if normal vector is without z-component
z = (-normal[0]*xx - normal[1]*yy - d)/normal[2]
# plot the surface, data, and synthetic circle
fig = plt.figure()
ax = fig.add_subplot(211, projection='3d')
ax.scatter(xs, ys, zs, c='b', marker='o')
ax.plot_wireframe(xx,yy,z)
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
bx = fig.add_subplot(212, projection='3d')
bx.scatter(xs, ys, zs, c='b', marker='o')
bx.scatter(cxTupel,cyTupel,czTupel, c='r', marker='o')
bx.set_xlabel('X Label')
bx.set_ylabel('Y Label')
bx.set_zlabel('Z Label')
plt.show()
which give a radius of 245. This is close to what the other approach gave (249). So within error margins I get the same.
The plotted result looks reasonable.
Hope this helps.

Feel like you missed some constraints in your 1st version code. The implementation could be explained as fitting a sphere to 3d points. So that's why the 2nd radius for 2nd data list is almost straight line. It's thinking like you are giving it a small circle on a large sphere.

Image Rotation using Python

My problem is the following:
I have two points in an image, I get the angle between these two points and rotate the image by this angle. I need to get the new position of this points in the image, but when I try to rotate those points using a rotation matrix with the same angle the points do not concur, what's wrong in the following code?
def rotMat(angle):
return asarray([[cos(angle), -sin(angle)],[sin(angle),cos(angle)]])
for i in batch2:
figure(1)
filename = "../imagens/" + i[-1]
outputFile = "./output/" + i[-1]
x1 = float(i[22]) # x coordinate of first point
y1 = float(i[23]) # y coordinate of first point
x2 = float(i[34]) # x coordinate of second point
y2 = float(i[35]) # y coordinate of second point
# angle of rotation
angle = arctan((y1-y2)/(x1-x2))
im = imread(filename)
im = ndimage.rotate(im, angle*180/pi, reshape=False)
imshow(im)
p1 = asarray([x1,y1])
p2 = asarray([x2,y2])
# Rotating the points
# [512,680] is the center of the image
p1n = (p1-[512,680]).dot(rotMat(angle)) + [512,680]
p2n = (p2-[512,680]).dot(rotMat(angle)) + [512,680]
print p1n, p2n
plot(p1n[0],p1n[1],'d')
plot(p2n[0],p2n[1],'d')
savefig(outputFile)
clf()

I don't understand 100 % what you are doing. But, did you consider that the y-axis in an image runs from 0 at the top to positive values for lower points. Therefore, the direction is opposite compared to the usual mathmetical definition. You defined rotMat in the usual way, but you have to adopt it to the changed y-axis in the image definition that runs in the oposite direction.