[DRAFT] Use 3d Geometry for Odometry classes / Pose Estimation

[jlmcmchl]

WIP. Java odometry tests pass, but pose estimation tests do not pass. Some data collected suggests that the theta error doesn't appear to be converging, which would certainly cause bad poses to never converge. When working on #4668 it seemed clear that in order for the position to converge, the rotation has to converge as well. It's clear that the position is attempting to converge, but from the data I have so far it doesn't look like any significant adjustment is being made to the pose rotation.

Posted for visibility and feedback on naming choices. Three functions needed new names for 3d geometry:

• getPoseMeters()
• getEstimatedPose()
• setVisionMeasurementStdDevs()
  • This one is also because it takes a different size Vector (6 entries vs 3), but java generics don't provide any additional type hints to the compiler. From an ABI standpoint they look the same.

Plan is to return to this later this weekend or early next week.

[calcmogul]

Why does this have four elements?

[jlmcmchl]

I use the Rotation times(double scalar) function to scale the rotation component of the twist, but I could scale each component of the rvec individually I suppose. Would that be preferred over one scalar used for the rotation component?

[calcmogul]

What you did makes sense. The docs for each element needs to be updated though, because it still says there's three elements: x, y, and heading; instead of x, y, z, and angle (used for all three rotation axes).

[jlmcmchl]

Did some additional reading and came across this - https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Angle-Angle-Angle

This would seem to suggest we should go ahead with 3 orthogonal std dev inputs for rotation, because they're measured independently on orthogonal axes. This makes some sense; gyroscopes typically measure 3 independent axes so the input signals themselves are not coupled tightly, and could behave differently.

I'll update docs for the new functions to recognize std dev inputs as x, y, z, roll, pitch, yaw. Think I should change heading to yaw in the 2d function docs as well?

[calcmogul]

Does scaling the twist elements individually work as expected? They aren't actually angles afterall, so they don't match the stddev units.

[jlmcmchl]

I've decided to revert the change from earlier back to [x, y, z, heading] for stdev parameters + kalman gain after speaking with some people who know more about 3d rotation than I do. This hasn't yet led to the new 3d test passing, unfortunately.

As per discussion on discord, it appears that pose.exp(pose.log(other)) == other doesn't appear to be held true within a reasonable margin of error - the translation component of the
computed pose is always correct, but the heading differs significantly between the computed pose and the expected result. Data demonstrating this as attached.

robot_vision_data.csv

columns are presented for the following:

• time of measurement within test
• robot_pose (sampled pose from pose buffer)
• vision_pose (input pose to addVisionMeasurement function)
• twist (robot_pose.log(vision_pose))
• exp_pose ('robot_pose.exp(twist)`)
• pose_error (Translation3d between exp_pose and vision_pose)

[calcmogul]

Maybe it's the Transform3d applying things in the wrong order?

  // Get left Jacobian of SO3. See first line in right column of
  // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
  Matrixd<3, 3> J;
  if (thetaSq < 1E-9 * 1E-9) {
    // J = I + 0.5ω
    J = Matrixd<3, 3>::Identity() + 0.5 * Omega;
  } else {
    double theta = std::sqrt(thetaSq);
    // J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω²
    J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega +
        (theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq;
  }

  // Get translation component
  Vectord<3> translation =
      J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};

  const Transform3d transform{Translation3d{units::meter_t{translation(0)},
                                            units::meter_t{translation(1)},
                                            units::meter_t{translation(2)}},
                              Rotation3d{twist.rx, twist.ry, twist.rz}};

  return *this + transform;

[calcmogul]

The command tests don't seem to include wpimath JNI, so Matrix.pow(double) isn't available (unless we add it), and that's what fails the build. It was originally added for fractional matrix powers in LinearQuadraticRegulator.latencyCompensate(). We could replace the pow(2) in Pose3d.exp() with just multiplying the matrix by itself to avoid the JNI dependency.

[Starlight220]

We should add the JNI for the tests.

[calcmogul]

Sure, but we can do that in a separate PR since this one no longer requires it.

[calcmogul]

@jlmcmchl Is this PR going to require a complete rewrite? I recall you mentioning wanting to do an alternative design.

[jlmcmchl]

Yes - the bulk of the rewrite work is begun on #5473. The rewrite I previously referred to has already been applied in #5355

[calcmogul]

OBE by kinematics rewrite in #5355. Needs a rewrite.