void StateEstimator::optimizationLoop()
{
ISAM2Params parameters;
// parameters.relinearizeThreshold = 0.0; // Set the relin threshold to zero such that the batch estimate is recovered
// parameters.relinearizeSkip = 1; // Relinearize every time
gtsam::IncrementalFixedLagSmoother graph(optLag_, parameters);
double startTime;
sensor_msgs::ImuConstPtr lastImu;
double lastImuT;
int imuKey = 1;
int gpsKey = 1;
// first we will initialize the graph with appropriate priors
NonlinearFactorGraph newFactors;
Values newVariables;
FixedLagSmoother::KeyTimestampMap newTimestamps;
sensor_msgs::NavSatFixConstPtr fix = gpsQ_.pop();
startTime = ROS_TIME(fix);
enu_.Reset(fix->latitude, fix->longitude, fix->altitude);
sensor_msgs::ImuConstPtr imu = imuQ_.pop();
lastImu = imu;
lastImuT = ROS_TIME(imu) - 1 / imuFreq_;
Rot3 initialOrientation =
Rot3::Quaternion(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
// we set out initial position to the origin and assume we are stationary
Pose3 x0(initialOrientation, Point3(0, 0, 0));
PriorFactor<Pose3> priorPose(X(0), x0,
noiseModel::Diagonal::Sigmas((Vector(6) << priorOSigma_, priorOSigma_, priorOSigma_,
priorPSigma_, priorPSigma_, priorPSigma_)
.finished()));
newFactors.add(priorPose);
Vector3 v0 = Vector3(0, 0, 0);
PriorFactor<Vector3> priorVel(
V(0), v0, noiseModel::Diagonal::Sigmas((Vector(3) << priorVSigma_, priorVSigma_, priorVSigma_).finished()));
newFactors.add(priorVel);
imuBias::ConstantBias b0((Vector(6) << 0, 0, 0, 0, 0, 0).finished());
PriorFactor<imuBias::ConstantBias> priorBias(
B(0), b0,
noiseModel::Diagonal::Sigmas(
(Vector(6) << priorABias_, priorABias_, priorABias_, priorGBias_, priorGBias_, priorGBias_).finished()));
newFactors.add(priorBias);
noiseModel::Diagonal::shared_ptr imuToGpsFactorNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << gpsTSigma_, gpsTSigma_, gpsTSigma_, gpsTSigma_, gpsTSigma_, gpsTSigma_).finished());
newFactors.add(BetweenFactor<Pose3>(X(0), G(0), imuToGps_, imuToGpsFactorNoise));
newVariables.insert(X(0), x0);
newVariables.insert(V(0), v0);
newVariables.insert(B(0), b0);
newVariables.insert(G(0), x0.compose(imuToGps_));
newTimestamps[X(0)] = 0;
newTimestamps[G(0)] = 0;
newTimestamps[V(0)] = 0;
newTimestamps[B(0)] = 0;
graph.update(newFactors, newVariables); //, newTimestamps);
Pose3 prevPose = prevPose_ = x0;
Vector3 prevVel = prevVel_ = v0;
imuBias::ConstantBias prevBias = prevBias_ = b0;
// remove old imu messages
while (!imuQ_.empty() && ROS_TIME(imuQ_.front()) < ROS_TIME(fix))
{
lastImuT = ROS_TIME(lastImu);
lastImu = imuQ_.pop();
}
// setting up the IMU integration
boost::shared_ptr<gtsam::PreintegrationParams> preintegrationParams =
PreintegrationParams::MakeSharedU(gravityMagnitude_);
preintegrationParams->accelerometerCovariance = accelSigma_ * I_3x3;
preintegrationParams->gyroscopeCovariance = gyroSigma_ * I_3x3;
preintegrationParams->integrationCovariance = imuIntSigma_ * I_3x3;
PreintegratedImuMeasurements imuIntegrator(preintegrationParams, prevBias);
Vector noiseModelBetweenBias =
(Vector(6) << accelBSigma_, accelBSigma_, accelBSigma_, gyroBSigma_, gyroBSigma_, gyroBSigma_).finished();
SharedDiagonal gpsNoise = noiseModel::Diagonal::Sigmas(Vector3(gpsSigma_, gpsSigma_, 3 * gpsSigma_));
newFactors.resize(0);
newVariables.clear();
newTimestamps.clear();
// now we loop and let use the queues to grab messages
while (ros::ok())
{
bool optimize = false;
// integrate imu messages
while (!imuQ_.empty() && ROS_TIME(imuQ_.back()) > (startTime + 0.1 * imuKey) && !optimize)
{
double curTime = startTime + 0.1 * imuKey;
// we reset the integrator, then integrate
imuIntegrator.resetIntegrationAndSetBias(prevBias);
while (ROS_TIME(lastImu) < curTime)
{
double dt = ROS_TIME(lastImu) - lastImuT;
imuIntegrator.integrateMeasurement(
Vector3(lastImu->linear_acceleration.x, lastImu->linear_acceleration.y, lastImu->linear_acceleration.z),
Vector3(lastImu->angular_velocity.x, lastImu->angular_velocity.y, lastImu->angular_velocity.z), dt);
lastImuT = ROS_TIME(lastImu);
lastImu = imuQ_.pop();
}
// now put this into the graph
ImuFactor imuf(X(imuKey - 1), V(imuKey - 1), X(imuKey), V(imuKey), B(imuKey - 1), imuIntegrator);
newFactors.add(imuf);
newFactors.add(BetweenFactor<imuBias::ConstantBias>(
B(imuKey - 1), B(imuKey), imuBias::ConstantBias(),
noiseModel::Diagonal::Sigmas(sqrt(imuIntegrator.deltaTij()) * noiseModelBetweenBias)));
Rot3 orientation = Rot3::Quaternion(lastImu->orientation.w, lastImu->orientation.x, lastImu->orientation.y,
lastImu->orientation.z);
// std::cout << "adding orientation: " << orientation.xyz() << std::endl;
newFactors.add(UnaryRotationFactor(X(imuKey), orientation.yaw(),
noiseModel::Diagonal::Sigmas((Vector(1) << yawSigma_).finished())));
NavState cur(prevPose, prevVel);
NavState next = imuIntegrator.predict(cur, prevBias);
prevPose = next.pose();
prevVel = next.v();
newVariables.insert(X(imuKey), prevPose);
newVariables.insert(G(imuKey), prevPose.compose(imuToGps_));
newVariables.insert(V(imuKey), prevVel);
newVariables.insert(B(imuKey), prevBias);
Pose3 temp = prevPose.compose(imuToGps_);
std::cout << "imu(" << imuKey << "): " << temp.x() << " " << temp.y() << " " << temp.z() << std::endl;
// for marginalizing out past the time window
newTimestamps[X(imuKey)] = 0.1 * imuKey;
newTimestamps[G(imuKey)] = 0.1 * imuKey;
newTimestamps[V(imuKey)] = 0.1 * imuKey;
newTimestamps[B(imuKey)] = 0.1 * imuKey;
++imuKey;
optimize = true;
}
while (!gpsQ_.empty() && gpsKey < imuKey && optimize && ROS_TIME(gpsQ_.back()) > (startTime + gpsKey * 0.1))
{
fix = gpsQ_.pop();
// we don't want all gps messages, just ones that are very close to the factors (10 hz)
if (std::abs(ROS_TIME(fix) - (startTime + gpsKey * 0.1)) > 1e-2)
continue;
double E, N, U;
enu_.Forward(fix->latitude, fix->longitude, fix->altitude, E, N, U);
// we should maybe do a check on the GPS to make sure it's valid
newFactors.add(GPSFactor(G(gpsKey), Point3(E, N, U), gpsNoise));
newFactors.add(BetweenFactor<Pose3>(X(gpsKey), G(gpsKey), imuToGps_, imuToGpsFactorNoise));
std::cout << "gps(" << gpsKey << "): " << E << " " << N << " " << U << std::endl;
++gpsKey;
}
if (!optimize)
continue;
try
{
graph.update(newFactors, newVariables); //, newTimestamps);
prevPose = graph.calculateEstimate<Pose3>(X(imuKey - 1));
prevVel = graph.calculateEstimate<Vector3>(V(imuKey - 1));
prevBias = graph.calculateEstimate<imuBias::ConstantBias>(B(imuKey - 1));
// pass this to the other thread
{
std::lock_guard<std::mutex> lock(mutex_);
prevPose_ = prevPose;
prevVel_ = prevVel;
prevBias_ = prevBias;
currentTime_ = (imuKey - 1) * 0.1 + startTime;
doneFirstOpt_ = true;
}
}
catch (IndeterminantLinearSystemException ex)
{
// optimization blew up, not much to do just warn user
ROS_ERROR("Indeterminant linear system error");
}
newFactors.resize(0);
newVariables.clear();
newTimestamps.clear();
}
}
