C++ Pose3::compose方法代码示例

/* ************************************************************************* */
TEST( dataSet, writeBALfromValues_Dubrovnik){

  ///< Read a file using the unit tested readBAL
  const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
  SfM_data readData;
  readBAL(filenameToRead, readData);

  Pose3 poseChange = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3));

  Values value;
  for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera
    Key poseKey = symbol('x',i);
    Pose3 pose = poseChange.compose(readData.cameras[i].pose());
    value.insert(poseKey, pose);
  }
  for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
    Key pointKey = P(j);
    Point3 point = poseChange.transform_from( readData.tracks[j].p );
    value.insert(pointKey, point);
  }

  // Write values and readData to a file
  const string filenameToWrite = createRewrittenFileName(filenameToRead);
  writeBALfromValues(filenameToWrite, readData, value);

  // Read the file we wrote
  SfM_data writtenData;
  readBAL(filenameToWrite, writtenData);

  // Check that the reprojection errors are the same and the poses are correct
  // Check number of things
  EXPECT_LONGS_EQUAL(3,writtenData.number_cameras());
  EXPECT_LONGS_EQUAL(7,writtenData.number_tracks());
  const SfM_Track& track0 = writtenData.tracks[0];
  EXPECT_LONGS_EQUAL(3,track0.number_measurements());

  // Check projection of a given point
  EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
  const SfM_Camera& camera0 = writtenData.cameras[0];
  Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
  EXPECT(assert_equal(expected,actual,12));

  Pose3 expectedPose = camera0.pose();
  Key poseKey = symbol('x',0);
  Pose3 actualPose = value.at<Pose3>(poseKey);
  EXPECT(assert_equal(expectedPose,actualPose, 1e-7));

  Point3 expectedPoint = track0.p;
  Key pointKey = P(0);
  Point3 actualPoint = value.at<Point3>(pointKey);
  EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
}

PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
    const PinholeCamera<CALIBRATION>& camera) {
  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
      Point3(0.5, 0.1, 0.3));
  Pose3 cameraPose = camera.pose();
  Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
  typename gtsam::traits<CALIBRATION>::TangentVector d;
  d.setRandom();
  d *= 0.1;
  CALIBRATION perturbedCalibration = camera.calibration().retract(d);
  return PinholeCamera<CALIBRATION>(perturbedCameraPose, perturbedCalibration);
}

/* ************************************************************************* */
PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
    OptionalJacobian<9, 6> Dtrans) const {

  // Pose3 transform is just compose
  Matrix6 D_newpose_trans, D_newpose_pose;
  Pose3 newpose = trans.compose(pose(), D_newpose_trans, D_newpose_pose);

  // Note that we rotate the velocity
  Matrix3 D_newvel_R, D_newvel_v;
  Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v);

  if (Dglobal) {
    Dglobal->setZero();
    Dglobal->topLeftCorner<6,6>() = D_newpose_pose;
    Dglobal->bottomRightCorner<3,3>() = D_newvel_v;
  }

  if (Dtrans) {
    Dtrans->setZero();
    Dtrans->topLeftCorner<6,6>() = D_newpose_trans;
    Dtrans->bottomLeftCorner<3,3>() = D_newvel_R;
  }
  return PoseRTV(newpose, newvel);
}

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