Added AHRSPose3Factor by extending AHRSFactor to enable integration with Pose3 variables.

[junlinp]

I found that the original AHRSFactor does not accept a Pose variable for integrating gyro measurements.
I added AHRSPose3Factor to accept two Pose variables and compute the rotation error using the AHRS measurements for gyro integration.

[Copilot]

Pull Request Overview

This PR adds a new AHRSPose3Factor class that operates on Pose3 variables instead of just Rot3 variables, along with corresponding test coverage. The new factor extracts rotations from poses and applies the AHRS measurement model, lifting the rotational Jacobians to 6-DOF pose Jacobians.

• Adds AHRSPose3Factor class to handle AHRS measurements for Pose3 state variables
• Exposes predict method in Python/wrapper interface for PreintegratedAhrsMeasurements
• Adds comprehensive unit tests for the new factor including error evaluation and Jacobian validation

Reviewed Changes

Copilot reviewed 4 out of 4 changed files in this pull request and generated 7 comments.

File	Description
gtsam/navigation/AHRSFactor.h	Defines the new AHRSPose3Factor class with similar interface to AHRSFactor
gtsam/navigation/AHRSFactor.cpp	Implements AHRSPose3Factor methods including error evaluation with Jacobian lifting
gtsam/navigation/navigation.i	Exposes predict method and AHRSPose3Factor class to the wrapper interface
tests/testAHRSWithPose.cpp	Adds unit tests for zero-error case and numerical Jacobian validation

[junlinp]

@github-copilot review

[junlinp]

@dellaert PTAL. I can't ask Copilot to review the pull request again.

[Copilot]

Pull Request Overview

Copilot reviewed 11 out of 11 changed files in this pull request and generated 9 comments.

[Copilot]

Corrected spelling of '@params' to '@param'.

Suggested change

	/// @params other factor to compare
	/// @params tol tolerance
	/// @param other factor to compare
	/// @param tol tolerance

[Copilot]

Corrected spelling of 'repected' to 'respected'.

Suggested change

	/// @param H1 jacobian of residuals repected to Ri
	/// @param H2 jacobian of residuals repected to Rj
	/// @param H3 jacobian of residuals repected to bias
	/// @param H1 jacobian of residuals with respect to Ri
	/// @param H2 jacobian of residuals with respect to Rj
	/// @param H3 jacobian of residuals with respect to bias

[Copilot]

The deprecated constructor (lines 367-370) and static predict function (lines 373-376) are declared but never implemented in AHRSFactor.cpp. This will cause linker errors if these functions are called. Either implement these functions or remove the declarations.

Suggested change

	AHRSPose3Factor(gtsam::Key pose_i, gtsam::Key pose_j, gtsam::Key bias,
	const PreintegratedAhrsMeasurements& pim,
	const Vector3& omegaCoriolis,
	const std::optional<Pose3>& body_P_sensor = {});
	/// @deprecated static function, but used in tests.
	static Pose3 predict(const Pose3& Ri, const Vector3& bias,
	const PreintegratedAhrsMeasurements& pim,
	const Vector3& omegaCoriolis,
	const std::optional<Pose3>& body_P_sensor = {});
	inline AHRSPose3Factor(gtsam::Key pose_i, gtsam::Key pose_j, gtsam::Key bias,
	const PreintegratedAhrsMeasurements& pim,
	const Vector3& /*omegaCoriolis*/,
	const std::optional<Pose3>& /*body_P_sensor*/ = {})
	: AHRSPose3Factor(pose_i, pose_j, bias, pim) {}
	/// @deprecated static function, but used in tests.
	inline static Pose3 predict(const Pose3& Ri, const Vector3& bias,
	const PreintegratedAhrsMeasurements& pim,
	const Vector3& /*omegaCoriolis*/,
	const std::optional<Pose3>& /*body_P_sensor*/ = {}) {
	// Call the main prediction logic if available, otherwise return Ri
	// This is a stub for deprecated/test usage
	return Ri;
	}

[Copilot]

File header comment has incorrect filename. Should be 'testDepthCamera3.cpp' not 'testDepthFactor.cpp'.

Suggested change

	* testDepthFactor.cpp
	* testDepthCamera3.cpp

[Copilot]

The documentation comment is misleading. Unlike PinholeCamera, DepthCamera3 does not store a Pose3 as a member - it stores a measurement and optional body_P_sensor transform. The description should be updated to accurately reflect what this class represents.

Suggested change

	* A pinhole camera class that has a Pose3 and a Calibration.
	* A depth camera class that stores a depth measurement, a camera calibration,
	* and an optional transform from the body frame to the sensor frame.
	* Unlike PinholeCamera, DepthCamera3 does not store a Pose3 as a member.

[Copilot]

The wrapper interface uses 'const gtsam::Pose3& body_P_sensor' but the header file (DepthCamera3.h line 53) uses 'const std::optional& body_P_sensor'. Parameter types must match exactly between .i files and header files for proper wrapper generation.

Suggested change

	DepthCamera3(const gtsam::Point3& measurement, const CALIBRATION::shared_ptr& k, const gtsam::Pose3& body_P_sensor);
	DepthCamera3(const gtsam::Point3& measurement, const CALIBRATION::shared_ptr& k, const std::optional<gtsam::Pose3>& body_P_sensor);