C++ NonlinearFactorGraph::resize方法代码示例

/* ************************************************************************* */
int main(int argc, char* argv[]) {

  // Define the camera calibration parameters
  Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));

  // Define the camera observation noise model
  noiseModel::Isotropic::shared_ptr noise = //
      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v

  // Create the set of ground-truth landmarks
  vector<Point3> points = createPoints();

  // Create the set of ground-truth poses
  vector<Pose3> poses = createPoses();

  // Create a NonlinearISAM object which will relinearize and reorder the variables
  // every "relinearizeInterval" updates
  int relinearizeInterval = 3;
  NonlinearISAM isam(relinearizeInterval);

  // Create a Factor Graph and Values to hold the new data
  NonlinearFactorGraph graph;
  Values initialEstimate;

  // Loop over the different poses, adding the observations to iSAM incrementally
  for (size_t i = 0; i < poses.size(); ++i) {

    // Add factors for each landmark observation
    for (size_t j = 0; j < points.size(); ++j) {
      // Create ground truth measurement
      SimpleCamera camera(poses[i], *K);
      Point2 measurement = camera.project(points[j]);
      // Add measurement
      graph.add(
          GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, noise,
              Symbol('x', i), Symbol('l', j), K));
    }

    // Intentionally initialize the variables off from the ground truth
    Pose3 noise(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
    Pose3 initial_xi = poses[i].compose(noise);

    // Add an initial guess for the current pose
    initialEstimate.insert(Symbol('x', i), initial_xi);

    // If this is the first iteration, add a prior on the first pose to set the coordinate frame
    // and a prior on the first landmark to set the scale
    // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
    // adding it to iSAM.
    if (i == 0) {
      // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
      noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
          (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
      graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));

      // Add a prior on landmark l0
      noiseModel::Isotropic::shared_ptr pointNoise =
          noiseModel::Isotropic::Sigma(3, 0.1);
      graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise));

      // Add initial guesses to all observed landmarks
      Point3 noise(-0.25, 0.20, 0.15);
      for (size_t j = 0; j < points.size(); ++j) {
        // Intentionally initialize the variables off from the ground truth
        Point3 initial_lj = points[j].compose(noise);
        initialEstimate.insert(Symbol('l', j), initial_lj);
      }

    } else {
      // Update iSAM with the new factors
      isam.update(graph, initialEstimate);
      Values currentEstimate = isam.estimate();
      cout << "****************************************************" << endl;
      cout << "Frame " << i << ": " << endl;
      currentEstimate.print("Current estimate: ");

      // Clear the factor graph and values for the next iteration
      graph.resize(0);
      initialEstimate.clear();
    }
  }

  return 0;
}

int main(int argc, char** argv) {

  // Define the smoother lag (in seconds)
  double lag = 2.0;

  // Create a Concurrent Filter and Smoother
  ConcurrentBatchFilter concurrentFilter;
  ConcurrentBatchSmoother concurrentSmoother;

  // And a fixed lag smoother with a short lag
  BatchFixedLagSmoother fixedlagSmoother(lag);

  // And a fixed lag smoother with very long lag (i.e. a full batch smoother)
  BatchFixedLagSmoother batchSmoother(1000.0);


  // Create containers to store the factors and linearization points that
  // will be sent to the smoothers
  NonlinearFactorGraph newFactors;
  Values newValues;
  FixedLagSmoother::KeyTimestampMap newTimestamps;

  // Create a prior on the first pose, placing it at the origin
  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
  Key priorKey = 0;
  newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
  newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
  newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;

  // Now, loop through several time steps, creating factors from different "sensors"
  // and adding them to the fixed-lag smoothers
  double deltaT = 0.25;
  for(double time = 0.0+deltaT; time <= 5.0; time += deltaT) {

    // Define the keys related to this timestamp
    Key previousKey(1000 * (time-deltaT));
    Key currentKey(1000 * (time));

    // Assign the current key to the current timestamp
    newTimestamps[currentKey] = time;

    // Add a guess for this pose to the new values
    // Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
    // {This is not a particularly good way to guess, but this is just an example}
    Pose2 currentPose(time * 2.0, 0.0, 0.0);
    newValues.insert(currentKey, currentPose);

    // Add odometry factors from two different sources with different error stats
    Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
    noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
    newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));

    Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
    noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
    newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));

    // Unlike the fixed-lag versions, the concurrent filter implementation
    // requires the user to supply the specify which keys to move to the smoother
    FastList<Key> oldKeys;
    if(time >= lag+deltaT) {
      oldKeys.push_back(1000 * (time-lag-deltaT));
    }

    // Update the various inference engines
    concurrentFilter.update(newFactors, newValues, oldKeys);
    fixedlagSmoother.update(newFactors, newValues, newTimestamps);
    batchSmoother.update(newFactors, newValues, newTimestamps);

    // Manually synchronize the Concurrent Filter and Smoother every 1.0 s
    if(fmod(time, 1.0) < 0.01) {
      // Synchronize the Filter and Smoother
      concurrentSmoother.update();
      synchronize(concurrentFilter, concurrentSmoother);
    }

    // Print the optimized current pose
    cout << setprecision(5) << "Timestamp = " << time << endl;
    concurrentFilter.calculateEstimate<Pose2>(currentKey).print("Concurrent Estimate: ");
    fixedlagSmoother.calculateEstimate<Pose2>(currentKey).print("Fixed Lag Estimate:  ");
    batchSmoother.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:      ");
    cout << endl;

    // Clear contains for the next iteration
    newTimestamps.clear();
    newValues.clear();
    newFactors.resize(0);
  }
  cout << "******************************************************************" << endl;
  cout << "All three versions should be identical." << endl;
  cout << "Adding a loop closure factor to the Batch version only." << endl;
  cout << "******************************************************************" << endl;
  cout << endl;

  // At the moment, all three versions produce the same output.
  // Now lets create a "loop closure" factor between the first pose and the current pose
  Key loopKey1(1000 * (0.0));
  Key loopKey2(1000 * (5.0));
  Pose2 loopMeasurement = Pose2(9.5, 1.00, 0.00);
  noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.25));
//.........这里部分代码省略.........

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)
    {
//.........这里部分代码省略.........

int main(int argc, char** argv) {

  // Define the smoother lag (in seconds)
  double lag = 2.0;

  // Create a fixed lag smoother
  // The Batch version uses Levenberg-Marquardt to perform the nonlinear optimization
  BatchFixedLagSmoother smootherBatch(lag);
  // The Incremental version uses iSAM2 to perform the nonlinear optimization
  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
  IncrementalFixedLagSmoother smootherISAM2(lag, parameters);

  // Create containers to store the factors and linearization points that
  // will be sent to the smoothers
  NonlinearFactorGraph newFactors;
  Values newValues;
  FixedLagSmoother::KeyTimestampMap newTimestamps;

  // Create a prior on the first pose, placing it at the origin
  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
  Key priorKey = 0;
  newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
  newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
  newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;

  // Now, loop through several time steps, creating factors from different "sensors"
  // and adding them to the fixed-lag smoothers
  double deltaT = 0.25;
  for(double time = deltaT; time <= 3.0; time += deltaT) {

    // Define the keys related to this timestamp
    Key previousKey(1000 * (time-deltaT));
    Key currentKey(1000 * (time));

    // Assign the current key to the current timestamp
    newTimestamps[currentKey] = time;

    // Add a guess for this pose to the new values
    // Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
    // {This is not a particularly good way to guess, but this is just an example}
    Pose2 currentPose(time * 2.0, 0.0, 0.0);
    newValues.insert(currentKey, currentPose);

    // Add odometry factors from two different sources with different error stats
    Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
    noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
    newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));

    Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
    noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
    newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));

    // Update the smoothers with the new factors
    smootherBatch.update(newFactors, newValues, newTimestamps);
    smootherISAM2.update(newFactors, newValues, newTimestamps);
    for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations
      smootherISAM2.update();
    }

    // Print the optimized current pose
    cout << setprecision(5) << "Timestamp = " << time << endl;
    smootherBatch.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:");
    smootherISAM2.calculateEstimate<Pose2>(currentKey).print("iSAM2 Estimate:");
    cout << endl;

    // Clear contains for the next iteration
    newTimestamps.clear();
    newValues.clear();
    newFactors.resize(0);
  }

  // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
  cout << "After 3.0 seconds, " << endl;
  cout << "  Batch Smoother Keys: " << endl;
  for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherBatch.timestamps()) {
    cout << setprecision(5) << "    Key: " << key_timestamp.first << "  Time: " << key_timestamp.second << endl;
  }
  cout << "  iSAM2 Smoother Keys: " << endl;
  for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherISAM2.timestamps()) {
    cout << setprecision(5) << "    Key: " << key_timestamp.first << "  Time: " << key_timestamp.second << endl;
  }

  return 0;
}