Point generator

.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
+/*.DS_Store

src/test-generators/README.md

@@ -0,0 +1,61 @@
+<H1>Test Generators</H1>
+These scripts generate test data, whether that be images or raw edge points.
+
+<h2>Point Generation</h2>
+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
+
+<h3>Usage</h3>
+There are three ways to call point-generator.py:
+<h4>Random position and rotation:</h4>
+<code>python point-generator.py [num points] [noise; not implemented yet] [angle redundancy] [x resolution] [y resolution] [sensor width] [focal length]</code> <br/><br/>
+<ul>
+  <li>num points: the number of points to generate</li>
+  <li>noise: how much noise error to add to the points (unimplemented)</li>
+  <li>angle redundancy: the horizon appears more than this many radians away from the edge (may be removed soon)</li>
+  <li>x resolution: the width of the image</li>
+  <li>y resolution: the height of the image</li>
+  <li>sensor width: the sensor width of the camera</li>
+  <li>focal length: the focal length of the camera (may be changed to pixel size soon)</li>
+</ul>
+Example: <code>python point-generator.py 23 0 0.3 700 600 0.036 0.05</code>
+<h4>Fixed position, random rotation</h4>
+Same as random position and rotation, but the position is passed in first <br/>
+<code>python point-generator.py [position.x] [position.y] [position.z] [... rest of the paramaters]</code>
+<h4>Fixed position, fixed rotation</h4>
+Same as random position and rotation, but position and rotation are passed in first <br/>
+<code>python point-generator.py [position.x] [position.y] [position.z] [rotation.x] [rotation.y] [rotation.z] [... rest of the paramaters]</code><br/>
+Rotation corresponds to a global XYZ rotation in radians (to be changed (like everything else))
+
+<h3>Output</h3>
+The results are appended to pointOutput.txt in the following format:<br\>
+
+```
+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<decimal>(563.8804701214169), static_cast<decimal>(66.7912733800368)},{static_cast<decimal>(559.4548008809911), static_cast<decimal>(112.58215587790039)},{static_cast<decimal>(555.6378930744293), static_cast<decimal>(159.09540388081345)},{static_cast<decimal>(553.6248941177013), static_cast<decimal>(187.88767868685713)},{static_cast<decimal>(553.788625492359), static_cast<decimal>(587.4287948302749)},{static_cast<decimal>(559.8946581416474), static_cast<decimal>(107.70809728855897)},{static_cast<decimal>(556.1332398670804), static_cast<decimal>(152.54736814256754)},{static_cast<decimal>(556.2650699061115), static_cast<decimal>(150.83487825459935)},{static_cast<decimal>(563.9773089136995), static_cast<decimal>(65.85923507236154)},{static_cast<decimal>(564.4447406943715), static_cast<decimal>(61.39593825283362)},{static_cast<decimal>(570.2646980759681), static_cast<decimal>(9.976812850743437)},{static_cast<decimal>(557.2612534993655), static_cast<decimal>(138.26932993985065)},{static_cast<decimal>(559.0600786953607), static_cast<decimal>(117.02867341902697)},{static_cast<decimal>(569.9389614875669), static_cast<decimal>(12.680874331890895)},{static_cast<decimal>(557.0837868873962), static_cast<decimal>(140.46176532893597)},{static_cast<decimal>(553.602720545366), static_cast<decimal>(188.22793973755353)},{static_cast<decimal>(553.0358544492206), static_cast<decimal>(197.13355570161164)},{static_cast<decimal>(553.867672460827), static_cast<decimal>(184.19943599383095)},{static_cast<decimal>(556.271433287473), static_cast<decimal>(150.75252661623324)},{static_cast<decimal>(555.4003382700106), static_cast<decimal>(162.30252927723555)},{static_cast<decimal>(559.487284497024), static_cast<decimal>(112.21934025705433)},{static_cast<decimal>(565.0126311322574), static_cast<decimal>(56.05004985913748)},{static_cast<decimal>(562.4685656287348), static_cast<decimal>(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 <code>{static_cast<decimal>(563.8804701214169),</code> can be copy-pasted straight into C++, i.e <code>Points pts = {{static_cast<decimal>(117), ...</code>
+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.

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<decimal>({x}), static_cast<decimal>({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<decimal>({point[0]}), static_cast<decimal>({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")
+
+