diff --git a/.gitignore b/.gitignore index 2b940a9..de3618b 100644 --- a/.gitignore +++ b/.gitignore @@ -12,8 +12,11 @@ wheels/ htmlcov/ .coverage +# Output files +pointOutput.txt + # Virtual environments .venv # Exclude all .Ds_Store files (these are generated by macOS) -/*.DS_Store \ No newline at end of file +/*.DS_Store diff --git a/src/test-generators/README.md b/src/test-generators/README.md new file mode 100644 index 0000000..8586810 --- /dev/null +++ b/src/test-generators/README.md @@ -0,0 +1,61 @@ +

Test Generators

+These scripts generate test data, whether that be images or raw edge points. + +

Point Generation

+point-generator.py analytically computes edge points that can be submitted to Zernike-Moments or CRA. +Position can be either randomly generated or specified on execution. +Rotation can be randomly generated or specified on execution if position was specified + +

Usage

+There are three ways to call point-generator.py: +

Random position and rotation:

+python point-generator.py [num points] [noise; not implemented yet] [angle redundancy] [x resolution] [y resolution] [sensor width] [focal length]

+ +Example: python point-generator.py 23 0 0.3 700 600 0.036 0.05 +

Fixed position, random rotation

+Same as random position and rotation, but the position is passed in first
+python point-generator.py [position.x] [position.y] [position.z] [... rest of the paramaters] +

Fixed position, fixed rotation

+Same as random position and rotation, but position and rotation are passed in first
+python point-generator.py [position.x] [position.y] [position.z] [rotation.x] [rotation.y] [rotation.z] [... rest of the paramaters]
+Rotation corresponds to a global XYZ rotation in radians (to be changed (like everything else)) + +

Output

+The results are appended to pointOutput.txt in the following format: + +``` +POINTS FOR +local (rc) [-6.65458800358326, -0.1779251688783572, -0.7354764868268421] +global (rp) [0.7167086525860719, 6.438189385508616, 1.7006383123587012] +with rotation [-56.743072836274024, 164.2314185171583, 72.50350056743514]: +{static_cast(563.8804701214169), static_cast(66.7912733800368)},{static_cast(559.4548008809911), static_cast(112.58215587790039)},{static_cast(555.6378930744293), static_cast(159.09540388081345)},{static_cast(553.6248941177013), static_cast(187.88767868685713)},{static_cast(553.788625492359), static_cast(587.4287948302749)},{static_cast(559.8946581416474), static_cast(107.70809728855897)},{static_cast(556.1332398670804), static_cast(152.54736814256754)},{static_cast(556.2650699061115), static_cast(150.83487825459935)},{static_cast(563.9773089136995), static_cast(65.85923507236154)},{static_cast(564.4447406943715), static_cast(61.39593825283362)},{static_cast(570.2646980759681), static_cast(9.976812850743437)},{static_cast(557.2612534993655), static_cast(138.26932993985065)},{static_cast(559.0600786953607), static_cast(117.02867341902697)},{static_cast(569.9389614875669), static_cast(12.680874331890895)},{static_cast(557.0837868873962), static_cast(140.46176532893597)},{static_cast(553.602720545366), static_cast(188.22793973755353)},{static_cast(553.0358544492206), static_cast(197.13355570161164)},{static_cast(553.867672460827), static_cast(184.19943599383095)},{static_cast(556.271433287473), static_cast(150.75252661623324)},{static_cast(555.4003382700106), static_cast(162.30252927723555)},{static_cast(559.487284497024), static_cast(112.21934025705433)},{static_cast(565.0126311322574), static_cast(56.05004985913748)},{static_cast(562.4685656287348), static_cast(80.685927347834)}, +TPC: +{-0.07260182653404851, -0.9861641641565356, -0.14902756831325298, +0.8492207415743102, 0.01723376352152481, -0.5277566953387355, +-0.5230230462072379, 0.1648734021251624, -0.8362198600893045, +} + +Calibrated conic equation: 1.0x^2 + 0.028843656609156755*2xy + -0.10596510642526717y^2 + -236.5793680872786*2x + 24.972778168972972*2y+-56192.05968037166 = 0 + +Other settings: +num points: 23 +point noise: 0.0 +angle redundancy: 0.3 +resolution: 700.0x600.0 +sensor width: 0.036 +focal length: 0.05 +FOV: 0.6911111611634242 +AOR: [-0.14902757 -0.5277567 -0.83621986] +``` +The line starting with {static_cast(563.8804701214169), can be copy-pasted straight into C++, i.e Points pts = {{static_cast(117), ... +The calibrated conic equation can be copy-pasted into desmos, but any scientific notation will have to be fixed manually. +TPC is the global to local coordinate transformation matrix. +AOR is the Earth's axis of rotation. diff --git a/src/test-generators/point-generator.py b/src/test-generators/point-generator.py new file mode 100644 index 0000000..892b83a --- /dev/null +++ b/src/test-generators/point-generator.py @@ -0,0 +1,252 @@ +import numpy as np +import random +import math +import sys + +#CONVENTION: UNITS ARE IN KM AND RADIANS +#CONVENTION: GLOBAL COORDS ARE RIGHT-HANDED Z-UP; LOCAL COORDS ARE LEFT-HANDED Z-FORWARD + +OUTPUT_FILE_PATH = "./pointOutput.txt" + +#PARAMETERS +# camera +xRes = 700 +yRes = 600 +sensorWidth = 0.036 +focalLength = 0.05 +# point generation +angleRedundancy = 0.1 # ~5.7 degrees +pointNoise = 0 +numPoints = 23 # number of points to generate + +#CONSTANTS +pi = math.pi +Identity3 = np.array([[1,0,0],[0,1,0],[0,0,1]]) +# 160 - 2000km above earth +LEOmin = 0.160+6.400 +LEOmax = 2.000+6.400 + +#Ellipsoid defining matrix in world coords +#6,378,136.6 - Equatorial axes +#6,356,751.9 - Polar axis +Ap = np.array([[1/6.3781366**2, 0 , 0], [0, 1/6.3781366**2, 0], [0,0,1/6.3567519**2]]) + +# distribution makes x way more likely to have high values +# refactor to randomize which coord gets chosen first? +def generatePos(dist): + x = random.random()*2 - 1 + y = (random.random()*2 - 1) * math.sqrt(1-x**2) # i think..? + z = math.sqrt(1-x**2-y**2) # uhhh + vec = np.array([x,y,z]) * dist + return vec + +# find analytical solution instead of brute forcing whether the horizon is visible maybe +def generateRot(): + x = random.random() * pi - pi/2 + y = random.random() * 2 * pi + z = random.random() * 2 * pi + rot = np.array([x,y,z]) + return rot + +def seesHorizon(radius, distVecC, fov, redundancy): + forward = np.array([0,0,1]) + #print(distVecC/np.linalg.norm(distVecC)) + #print(fov) + angle = math.acos(forward.dot(distVecC/np.linalg.norm(distVecC))) + #print(angle) + sphereAngle = math.atan(radius/np.linalg.norm(distVecC)) + #print(sphereAngle) + # print("\n") + # print(f"angle: {angle}") + # print(f"sphereAngle: {sphereAngle}") + # print(f"fov: {fov}") + # print(f"upper bound: {fov/2+sphereAngle}") + # print(f"lower bound: {sphereAngle-fov/2}") + # print((angle+redundancy) < (fov/2+sphereAngle)) and ((angle - redundancy) > (sphereAngle-fov/2)) + return ((angle+redundancy) < (fov/2+sphereAngle)) and ((angle - redundancy) > (sphereAngle-fov/2)) + +# this will not be the same matrix as the one CR generates, there's one (two?) degree(s?) of freedom +# ZXZ rotation +def generateTPC(rotation): + #rotation = rotation * np.array([-1,-1,1]) + Xrot = np.array( + [[1, 0, 0], + [0, math.cos(rotation[0]), -math.sin(rotation[0])], + [0, math.sin(rotation[0]), math.cos(rotation[0])]]) + adjust = np.array( + [[1, 0, 0], + [0, math.cos(pi/2), -math.sin(pi/2)], + [0, math.sin(pi/2), math.cos(pi/2)]]) + # we use a z rotation cause we're doing ZXZ + Yrot = np.array( + [[math.cos(rotation[1]), 0, math.sin(rotation[1])], + [0, 1, 0], + [-math.sin(rotation[1]), 0, math.cos(rotation[1])]]) + # [[math.cos(rotation[1]), 0, math.sin(rotation[1])], + # [0, 1, 0], + # [-math.sin(rotation[1]), 0, math.cos(rotation[1])]]) + Zrot = np.array( + [[math.cos(rotation[2]), -math.sin(rotation[2]), 0], + [math.sin(rotation[2]), math.cos(rotation[2]), 0], + [0, 0, 1]]) + + invertZ = np.array([[1,0,0],[0,1,0],[0,0,-1]]) # DON'T FORGET TO CHANGE BACK + + # RIGHT HAND GLOBAL COORDS (Z UP) -> LEFT HAND LOCAL CAM COORDS (Z FORWARD) + TPC = Zrot.dot(Xrot.dot(Yrot.dot(adjust.dot(invertZ)))) + return np.transpose(TPC) + +#generates the conic in image coords +def generateConic(rc, Ap, TPC): + Ac = np.transpose(TPC).dot(Ap.dot(TPC)) + C = Ac.dot(np.outer(rc, rc).dot(Ac)) - (rc.dot(Ac.dot(rc))*Identity3 - Identity3).dot(Ac) + return C + +def generateInvCameraMat(sensorWidth, xRes): + pixelSize = sensorWidth/xRes + + dx = focalLength/pixelSize + dy = dx # square pixels + + KInv = np.array([[1/dx, 0, -(xRes/2)/(dx) ], + [0, 1/dy, -(yRes/2)/(dy) ], + [0, 0, 1 ]]) + return KInv + +#generates the conic in pixel coords +def generateCalibratedConic(C, KInv): + calibratedC = np.transpose(KInv).dot(C.dot(KInv)) + calibratedC = calibratedC/calibratedC[0][0] + # print("\n\n") + # print("Calibrated conic:") + # # plug this into desmos to see the curve in pixel coords + # print(f"{calibratedC[0][0]}x^2 + {calibratedC[0][1]}*2xy + {calibratedC[1][1]}y^2 + {calibratedC[0][2]}*2x + {calibratedC[1][2]}*2y+{calibratedC[2][2]} = 0") + # print("\n\n") + return calibratedC + +def noise(pointNoise, points): + return points # trust me it works + +def generatePoints(calibratedConic, pointNoise, numPoints): + points = np.zeros((numPoints, 2)) + for i in range(numPoints): + x = random.random() * xRes + a = calibratedConic[1][1] + b = calibratedConic[0][1]*2*x+2*calibratedConic[1][2] + c = (calibratedConic[0][0]*x*x+calibratedConic[0][2]*2*x+calibratedConic[2][2]) + plusorminus = round(random.random())*2-1 + det = b**2 - 4*a*c + y = 0 + if(det>0): + y = (-b + plusorminus*math.sqrt(det))/(2*a) + counter = 0 + while (True): # make sure there are real roots + x = random.random() * xRes + counter += 1 + if (counter > numPoints*1000): + points[i] = None + return points + a = calibratedConic[1][1] + b = calibratedConic[0][1]*2*x+2*calibratedConic[1][2] + c = (calibratedConic[0][0]*x*x+calibratedConic[0][2]*2*x+calibratedConic[2][2]) + det = (b)**2 - 4*a*c + if (det < 0): + continue + y = (-b + plusorminus*math.sqrt(det))/(2*a) + if (y<0 or y>yRes): + plusorminus = -plusorminus + y = (-b + plusorminus*math.sqrt(det))/(2*a) + if (y<0 or y>yRes): + continue + break + points[i] = np.array([x,y]) + + + #print(f"{{static_cast({x}), static_cast({y})}},") + points = noise(pointNoise, points) + return points + +# position in world coords, local rotation +def posrotmain(positionx, positiony, positionz, rotationx, rotationy, rotationz, numPoints, pointNoise, angleRedundancy, xRes, yRes, sensorWidth, focalLength): + rp = np.array([positionx, positiony, positionz]) + rotation = np.array([rotationx, rotationy, rotationz]) + print("\n\n") + print(f"rp: {rp}") + print(f"rotation: {rotation*180/(math.pi)}") + TPC = generateTPC(rotation) + rc = TPC.dot(rp) + print(f"rc: {rc}") + print("\n\n") + TCP = np.transpose(TPC) + positions = np.array([[0.097372,-0.315722,0.943843],[-0.15943,0.931154,0.327925],[-0.982396,-0.182407,0.040333]]) + print(f"positions:\n{positions}") + print(f"calc TCP:\n{TCP}") + print(f"det: {np.linalg.det(TCP)}") + print("\n\n") + fov = 2*math.atan(sensorWidth/(2*focalLength)) + # if not seesHorizon(math.sqrt(1/Ap[0][0]), -1*rc, fov, angleRedundancy): + # raise ValueError("Camera can't see the horizon idiot!") + C = generateConic(rc, Ap, TPC) + KInv = generateInvCameraMat(sensorWidth, xRes) + calibratedC = generateCalibratedConic(C, KInv) + points = generatePoints(calibratedC, pointNoise, numPoints) + if (not points.all()): + print("AAAAARGHHHH") + return False + # appends + with open(OUTPUT_FILE_PATH, "a") as f: + rotation = rotation*180/(math.pi) + rc = rc*100000 + rp = rp*100000 + f.write(f"\n\nPOINTS FOR\nlocal (rc) [{rc[0]}, {rc[1]}, {rc[2]}]\nglobal (rp) [{rp[0]}, {rp[1]}, {rp[2]}] \nwith rotation [{rotation[0]}, {rotation[1]}, {rotation[2]}]:\n") + for point in points: + f.write(f"{{static_cast({point[0]}), static_cast({point[1]})}},") + f.write("\nTPC:\n{") + for row in TPC: + f.write(f"{row[0]}, {row[1]}, {row[2]},\n") + f.write("}") + f.write("\n\nCalibrated conic equation: ") + f.write(f"{calibratedC[0][0]}x^2 + {calibratedC[0][1]}*2xy + {calibratedC[1][1]}y^2 + {calibratedC[0][2]}*2x + {calibratedC[1][2]}*2y+{calibratedC[2][2]} = 0") + f.write(f"\n\nOther settings:\nnum points: {numPoints}\npoint noise: {pointNoise}\nangle redundancy: {angleRedundancy}\nresolution: {xRes}x{yRes}\nsensor width: {sensorWidth}\nfocal length: {focalLength}\nFOV: {fov}\nAOR: {TPC.dot([0,0,1])}\n\n------------------------------------------------") + return True +# position in world coords, local rotation +def posmain(positionx, positiony, positionz, numPoints, pointNoise, angleRedundancy, xRes, yRes, sensorWidth, focalLength): + rp = np.array([positionx, positiony, positionz]) + rotation = generateRot() + TPC = generateTPC(rotation) + rc = TPC.dot(rp) + fov = 2*math.atan(sensorWidth/(2*focalLength)) + while not seesHorizon(math.sqrt(1/Ap[0][0]), -1*rc, fov, angleRedundancy): + #print(rotation) + rotation = generateRot() + TPC = np.linalg.inv(generateTPC(rotation)) + rc = TPC.dot(rp) + while (not posrotmain(positionx, positiony, positionz, rotation[0], rotation[1], rotation[2], numPoints, pointNoise, angleRedundancy, xRes, yRes, sensorWidth, focalLength)): # this is clean code. + rotation = generateRot() + TPC = generateTPC(rotation) + rc = TPC.dot(rp) + fov = 2*math.atan(sensorWidth/(2*focalLength)) + while not seesHorizon(math.sqrt(1/Ap[0][0]), -1*rc, fov, angleRedundancy): + #print(rotation) + rotation = generateRot() + TPC = np.linalg.inv(generateTPC(rotation)) + rc = TPC.dot(rp) + +# position in world coords, local rotation +def rawmain(numPoints, pointNoise, angleRedundancy, xRes, yRes, sensorWidth, focalLength): + dist = LEOmin + random.random()*(LEOmax-LEOmin) + position = generatePos(dist) + posmain(position[0], position[1], position[2], numPoints, pointNoise, angleRedundancy, xRes, yRes, sensorWidth, focalLength) + + +if len(sys.argv) == 14: + posrotmain(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]), float(sys.argv[4])/180*pi, float(sys.argv[5])/180*pi, float(sys.argv[6])/180*pi, int(sys.argv[7]), float(sys.argv[8]), float(sys.argv[9]), float(sys.argv[10]), float(sys.argv[11]), float(sys.argv[12]), float(sys.argv[13])) +elif len(sys.argv) == 11: + posmain(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]), int(sys.argv[4]), float(sys.argv[5]), float(sys.argv[6]), float(sys.argv[7]), float(sys.argv[8]), float(sys.argv[9]), float(sys.argv[10])) +elif len(sys.argv) == 8: + rawmain(int(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]), float(sys.argv[4]), float(sys.argv[5]), float(sys.argv[6]), float(sys.argv[7])) +else: + print("Usage: point-generator.py positionx, positiony, positionz, rotationx, rotationy, rotationz, numPoints, pointNoise, angleRedundancy, xRes, yRes, sensorWidth, focalLength") + print("Position and rotation optional") + +