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

Attempt to resolve Windows linking

release/4.3a0
Frank Dellaert 2024-06-09 21:34:37 -07:00
parent fccf3cedd9
commit 1bb26f86af
3 changed files with 30 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -68,7 +68,8 @@ bool PreintegratedRotation::equals(const PreintegratedRotation& other,
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
} }
Rot3 PreintegratedRotation::IncrementalRotation::operator()( namespace internal {
Rot3 IncrementalRotation::operator()(
const Vector3& bias, OptionalJacobian<3, 3> H_bias) const { const Vector3& bias, OptionalJacobian<3, 3> H_bias) const {
// First we compensate the measurements for the bias // First we compensate the measurements for the bias
Vector3 correctedOmega = measuredOmega - bias; Vector3 correctedOmega = measuredOmega - bias;
@ -93,12 +94,13 @@ Rot3 PreintegratedRotation::IncrementalRotation::operator()(
} }
return incrR; return incrR;
} }
} // namespace internal
void PreintegratedRotation::integrateGyroMeasurement( void PreintegratedRotation::integrateGyroMeasurement(
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> F) { OptionalJacobian<3, 3> F) {
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);
// Update deltaTij and rotation // Update deltaTij and rotation

50
gtsam/navigation/PreintegratedRotation.h
View File

@ -176,32 +176,6 @@ class GTSAM_EXPORT PreintegratedRotation {
/// @} /// @}
/// @name Internal, exposed for testing only
/// @{
/**
* @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;
};
/// @}
/// @name Deprecated API /// @name Deprecated API
/// @{ /// @{
@ -250,4 +224,28 @@ 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

6
gtsam/navigation/tests/testPreintegratedRotation.cpp
View File

@ -52,8 +52,7 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) {
// Check the value. // Check the value.
Matrix3 H_bias; Matrix3 H_bias;
PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()};
p->getBodyPSensor()};
const Rot3 incrR = f(bias, H_bias); const Rot3 incrR = f(bias, H_bias);
Rot3 expected = Rot3::Roll(omega * deltaT); Rot3 expected = Rot3::Roll(omega * deltaT);
EXPECT(assert_equal(expected, incrR, 1e-9)); EXPECT(assert_equal(expected, incrR, 1e-9));
@ -98,8 +97,7 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) {
// Check the value. // Check the value.
Matrix3 H_bias; Matrix3 H_bias;
PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()};
p->getBodyPSensor()};
Rot3 expected = Rot3::Pitch(omega * deltaT); Rot3 expected = Rot3::Pitch(omega * deltaT);
EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9));