12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compile deprecated sections

release/4.3a0
Frank Dellaert 2024-06-09 21:47:15 -07:00
parent 1bb26f86af
commit ab73b956e0
2 changed files with 26 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -122,7 +122,7 @@ void PreintegratedRotation::integrateMeasurement(
// If asked, pass obsolete Jacobians as well // If asked, pass obsolete Jacobians as well
if (optional_D_incrR_integratedOmega) { if (optional_D_incrR_integratedOmega) {
Matrix3 H_bias; 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); const Rot3 incrR = f(biasHat, H_bias);
*optional_D_incrR_integratedOmega << H_bias / -deltaT; *optional_D_incrR_integratedOmega << H_bias / -deltaT;
} }

50
gtsam/navigation/PreintegratedRotation.h
View File

@ -27,6 +27,30 @@
namespace gtsam { 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: /// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor /// Usage: Create just a single Params and pass a shared pointer to the constructor
struct GTSAM_EXPORT PreintegratedRotationParams { struct GTSAM_EXPORT PreintegratedRotationParams {
@ -184,7 +208,7 @@ class GTSAM_EXPORT PreintegratedRotation {
inline Rot3 GTSAM_DEPRECATED incrementalRotation( inline Rot3 GTSAM_DEPRECATED incrementalRotation(
const Vector3& measuredOmega, const Vector3& bias, double deltaT, const Vector3& measuredOmega, const Vector3& bias, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega) const { 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); Rot3 incrR = f(bias, D_incrR_integratedOmega);
// Backwards compatible "weird" Jacobian, no longer used. // Backwards compatible "weird" Jacobian, no longer used.
if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT; if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT;
@ -224,28 +248,4 @@ class GTSAM_EXPORT PreintegratedRotation {
template <> template <>
struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {}; 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 } /// namespace gtsam