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(); } }