[Symforce] Codegen ToYawPitchRoll for C++ Rot3

I changed `Rot3::ToYawPitchRoll()` to use the same implementation as we
do in the symbolic python version. Previously we were converting the
quaternion to a rotation matrix and then using built-in Eigen functions
to get the Euler angles, but this could give ambiguous results.

For example, if you construct a rotation with YPR (-10, 10, -10)
degrees, it can sometimes be reconstructed as something like YPR (170,
170, -180) degrees. Computing Euler angles directly from the quaternion
is much more stable (and it's probably much faster).

Topic: symforce_rot3_cpp_to_yaw_pitch_roll
GitOrigin-RevId: 85de520542ba265dced379a6655f42d536cd32d7

Author: nathan-skydio

gen/cpp/sym/rot3.cc

@@ -40,13 +40,6 @@
     return Rot3(Eigen::Quaternion<Scalar>(mat));
   }
 
-  // Euler angles
-  // TODO(hayk): Could codegen this.
-
-  Vector3 ToYawPitchRoll() const {
-      return ToRotationMatrix().eulerAngles(2, 1, 0);
-  }
-
   // Generate a random element in SO3
   template <typename Generator>
   static Rot3 Random(Generator& gen) {

gen/cpp/sym/rot3.h

@@ -111,6 +111,12 @@ def codegen_mul(group: T.Type, multiplicand_type: T.Type) -> Codegen:
                 name="from_yaw_pitch_roll",
                 config=config,
             ),
+            Codegen.function(
+                func=lambda self: sf.V3(self.to_yaw_pitch_roll()),
+                input_types=[sf.Rot3],
+                name="to_yaw_pitch_roll",
+                config=config,
+            ),
         ],
         sf.Pose2: [
             codegen_mul(sf.Pose2, sf.Vector2),