commit
f4bcb11ec6
|
@ -365,7 +365,7 @@ boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
|
||||||
"Pose2:Align expects 2*N matrices of equal shape.");
|
"Pose2:Align expects 2*N matrices of equal shape.");
|
||||||
}
|
}
|
||||||
Point2Pairs ab_pairs;
|
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));
|
ab_pairs.emplace_back(a.col(j), b.col(j));
|
||||||
}
|
}
|
||||||
return Pose2::Align(ab_pairs);
|
return Pose2::Align(ab_pairs);
|
||||||
|
|
|
@ -473,7 +473,7 @@ boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
|
||||||
"Pose3:Align expects 3*N matrices of equal shape.");
|
"Pose3:Align expects 3*N matrices of equal shape.");
|
||||||
}
|
}
|
||||||
Point3Pairs abPointPairs;
|
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));
|
abPointPairs.emplace_back(a.col(j), b.col(j));
|
||||||
}
|
}
|
||||||
return Pose3::Align(abPointPairs);
|
return Pose3::Align(abPointPairs);
|
||||||
|
|
|
@ -90,7 +90,11 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Add a single Gyroscope measurement to the preintegration.
|
* 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
|
* @param deltaT Time step
|
||||||
*/
|
*/
|
||||||
void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
|
void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
|
||||||
|
|
|
@ -208,8 +208,11 @@ public:
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Add a single IMU measurement to the preintegration.
|
* Add a single IMU measurement to the preintegration.
|
||||||
* @param measuredAcc Measured acceleration (in body frame, as given by the
|
* Both accelerometer and gyroscope measurements are taken to be in the sensor
|
||||||
* 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 measuredOmega Measured angular velocity (as given by the sensor)
|
||||||
* @param dt Time interval between two consecutive IMU measurements
|
* @param dt Time interval between two consecutive IMU measurements
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -121,7 +121,11 @@ public:
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Add a single IMU measurement to the preintegration.
|
* 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 measuredOmega Measured angular velocity (as given by the sensor)
|
||||||
* @param dt Time interval between this and the last IMU measurement
|
* @param dt Time interval between this and the last IMU measurement
|
||||||
*/
|
*/
|
||||||
|
|
Loading…
Reference in New Issue