| 345 | } |
| 346 | |
| 347 | Eigen::Matrix3f IMUPreintegrator::RightJacobian(const Eigen::Vector3f& omega) const { |
| 348 | float theta_sq = omega.squaredNorm(); |
| 349 | float theta = std::sqrt(theta_sq); |
| 350 | |
| 351 | if (theta < 1e-6f) { |
| 352 | // Small angle approximation: Jr ≈ I - 0.5 * [omega]_× |
| 353 | return Eigen::Matrix3f::Identity() - 0.5f * SkewSymmetric(omega); |
| 354 | } |
| 355 | |
| 356 | Eigen::Matrix3f W = SkewSymmetric(omega); |
| 357 | |
| 358 | // Right Jacobian: Jr = I - (1-cos(θ))/θ² * W + (θ-sin(θ))/θ³ * W² |
| 359 | float theta_cu = theta_sq * theta; |
| 360 | return Eigen::Matrix3f::Identity() |
| 361 | - ((1.0f - std::cos(theta)) / theta_sq) * W |
| 362 | + ((theta - std::sin(theta)) / theta_cu) * W * W; |
| 363 | } |
| 364 | |
| 365 | } // namespace vio_360 |
nothing calls this directly
no outgoing calls
no test coverage detected