Compile deprecated sections
parent
1bb26f86af
commit
ab73b956e0
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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<Pose3>& 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<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
|
||||
|
||||
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<Pose3>& 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
|
||||
|
|
Loading…
Reference in New Issue