diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 96bdce69b..b37674b92 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -365,7 +365,7 @@ boost::optional Pose2::Align(const Matrix& a, const Matrix& b) { "Pose2:Align expects 2*N matrices of equal shape."); } Point2Pairs ab_pairs; - for (size_t j=0; j < a.cols(); j++) { + for (Eigen::Index j = 0; j < a.cols(); j++) { ab_pairs.emplace_back(a.col(j), b.col(j)); } return Pose2::Align(ab_pairs); diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b88d812a4..2da51a625 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -473,7 +473,7 @@ boost::optional Pose3::Align(const Matrix& a, const Matrix& b) { "Pose3:Align expects 3*N matrices of equal shape."); } Point3Pairs abPointPairs; - for (size_t j=0; j < a.cols(); j++) { + for (Eigen::Index j = 0; j < a.cols(); j++) { abPointPairs.emplace_back(a.col(j), b.col(j)); } return Pose3::Align(abPointPairs); diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 10c33d101..c7d82975a 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -90,7 +90,11 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation /** * Add a single Gyroscope measurement to the preintegration. - * @param measureOmedga Measured angular velocity (in body frame) + * Measurements are taken to be in the sensor + * frame and conversion to the body frame is handled by `body_P_sensor` in + * `PreintegratedRotationParams` (if provided). + * + * @param measuredOmega Measured angular velocity (as given by the sensor) * @param deltaT Time step */ void integrateMeasurement(const Vector3& measuredOmega, double deltaT); diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 068a17ca4..213f5f223 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -208,8 +208,11 @@ public: /** * Add a single IMU measurement to the preintegration. - * @param measuredAcc Measured acceleration (in body frame, as given by the - * sensor) + * Both accelerometer and gyroscope measurements are taken to be in the sensor + * frame and conversion to the body frame is handled by `body_P_sensor` in + * `PreintegrationParams`. + * + * @param measuredAcc Measured acceleration (as given by the sensor) * @param measuredOmega Measured angular velocity (as given by the sensor) * @param dt Time interval between two consecutive IMU measurements */ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 35207e452..740827162 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -121,7 +121,11 @@ public: /** * Add a single IMU measurement to the preintegration. - * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) + * Both accelerometer and gyroscope measurements are taken to be in the sensor + * frame and conversion to the body frame is handled by `body_P_sensor` in + * `PreintegrationParams`. + * + * @param measuredAcc Measured acceleration (as given by the sensor) * @param measuredOmega Measured angular velocity (as given by the sensor) * @param dt Time interval between this and the last IMU measurement */