diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index ebf98bc15..04e201a34 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -122,7 +122,7 @@ void PreintegratedRotation::integrateMeasurement( // If asked, pass obsolete Jacobians as well if (optional_D_incrR_integratedOmega) { Matrix3 H_bias; - IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; const Rot3 incrR = f(biasHat, H_bias); *optional_D_incrR_integratedOmega << H_bias / -deltaT; } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 3ff3eb3f1..146a47a19 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -27,6 +27,30 @@ namespace gtsam { +namespace internal { +/** + * @brief Function object for incremental rotation. + * @param measuredOmega The measured angular velocity (as given by the sensor) + * @param deltaT The time interval over which the rotation is integrated. + * @param body_P_sensor Optional transform between body and IMU. + */ +struct IncrementalRotation { + const Vector3& measuredOmega; + const double deltaT; + const std::optional& body_P_sensor; + + /** + * @brief Integrate angular velocity, but corrected by bias. + * @param bias The bias estimate + * @param H_bias Jacobian of the rotation w.r.t. bias. + * @return The incremental rotation + */ + Rot3 operator()(const Vector3& bias, + OptionalJacobian<3, 3> H_bias = {}) const; +}; + +} // namespace internal + /// Parameters for pre-integration: /// Usage: Create just a single Params and pass a shared pointer to the constructor struct GTSAM_EXPORT PreintegratedRotationParams { @@ -184,7 +208,7 @@ class GTSAM_EXPORT PreintegratedRotation { inline Rot3 GTSAM_DEPRECATED incrementalRotation( const Vector3& measuredOmega, const Vector3& bias, double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const { - IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; Rot3 incrR = f(bias, D_incrR_integratedOmega); // Backwards compatible "weird" Jacobian, no longer used. if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT; @@ -224,28 +248,4 @@ class GTSAM_EXPORT PreintegratedRotation { template <> struct traits : public Testable {}; -namespace internal { -/** - * @brief Function object for incremental rotation. - * @param measuredOmega The measured angular velocity (as given by the sensor) - * @param deltaT The time interval over which the rotation is integrated. - * @param body_P_sensor Optional transform between body and IMU. - */ -struct IncrementalRotation { - const Vector3& measuredOmega; - const double deltaT; - const std::optional& body_P_sensor; - - /** - * @brief Integrate angular velocity, but corrected by bias. - * @param bias The bias estimate - * @param H_bias Jacobian of the rotation w.r.t. bias. - * @return The incremental rotation - */ - Rot3 operator()(const Vector3& bias, - OptionalJacobian<3, 3> H_bias = {}) const; -}; - -} // namespace internal - } /// namespace gtsam