Use normalized pixels in cPNP problem formulation (#1816)

## Description

instead of using camera calibration, the CasADi problem formulation now
expect the caller to provide "normalized pixel coordinates". This
reduces the number of floating point operations we need to perform (and
reduces total LOC by 6%). `casadi_wrapper.cpp` now converts from (u, v)
coordinates to normalized (x'', y'') coordinates with:

x'' = (u - c_x) / f_x
y'' = (u - c_y) / f_y

Which is the inverse of this (from [opencv
docs](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html), and
assuming no distortion):

![image](https://github.com/user-attachments/assets/41ca657d-af42-4cdf-9c25-6a4f04d20940)

In my testing, this is ~16% faster on my x86 laptop. Would love some rio
benchmarks.

## Meta

Merge checklist:
- [x] Pull Request title is [short, imperative
summary](https://cbea.ms/git-commit/) of proposed changes
- [x] The description documents the _what_ and _why_
- [~] If this PR changes behavior or adds a feature, user documentation
is updated
- [~] If this PR touches photon-serde, all messages have been
regenerated and hashes have not changed unexpectedly
- [~] If this PR touches configuration, this is backwards compatible
with settings back to v2024.3.1
- [~] If this PR addresses a bug, a regression test for it is added

scripts/casadi_generate_tag_costs.py

@@ -9,12 +9,6 @@ from numpy import *
 
 def generate_costs(num_tags, robot_heading_free):
 
-    # Camera calibration parameters
-    fx = ca.SX.sym("fx")
-    fy = ca.SX.sym("fy")
-    cx = ca.SX.sym("cx")
-    cy = ca.SX.sym("cy")
-
     # Decision variables
     robot_x = ca.SX.sym("robot_x")
     robot_y = ca.SX.sym("robot_y")
@@ -60,22 +54,24 @@ def generate_costs(num_tags, robot_heading_free):
     z = camera2point[2, :]
 
     # Observed coordinates
-    u_observed = point_observations[0, :]
-    v_observed = point_observations[1, :]
+    # Note that instead of using camera calibration, we expect the caller to provide
+    # "normalized pixel coordinates". Convert from (u, v) coordinates to normalized
+    # (x'', y'') coordinates with:
+    # x'' = (u - c_x) / f_x
+    # y'' = (u - c_y) / f_y
+    xʼʼ_observed = point_observations[0, :]
+    yʼʼ_observed = point_observations[1, :]
 
-    # Project to image plane
-    X = x / z
-    Y = y / z
-
-    u = fx * X + cx
-    v = fy * Y + cy
+    # Where we expected to see the landmarks at, in normalized pixel coordinates
+    xʼʼ = x / z
+    yʼʼ = y / z
 
     # Reprojection error
-    u_err = u - u_observed
-    v_err = v - v_observed
+    xʼʼ_err = xʼʼ - xʼʼ_observed
+    yʼʼ_err = yʼʼ - yʼʼ_observed
 
     # Frobenius norm - sqrt(sum squared of each component). Square to remove sqrt
-    J = ca.norm_fro(u_err) ** 2 + ca.norm_fro(v_err) ** 2
+    J = ca.norm_fro(xʼʼ_err) ** 2 + ca.norm_fro(yʼʼ_err) ** 2
 
     # And penalize gyro error excursion
     if not robot_heading_free:
@@ -93,10 +89,10 @@ def generate_costs(num_tags, robot_heading_free):
         robot_x,
         robot_y,
         robot_θ,
-        fx,
-        fy,
-        cx,
-        cy,
+        # fx,
+        # fy,
+        # cx,
+        # cy,
         robot2camera,
         field2points,
         point_observations,
@@ -107,10 +103,10 @@ def generate_costs(num_tags, robot_heading_free):
         "robot_x",
         "robot_y",
         "robot_θ",
-        "fx",
-        "fy",
-        "cx",
-        "cy",
+        # "fx",
+        # "fy",
+        # "cx",
+        # "cy",
         "robot2camera",
         "field2points",
         "point_observations",