本文整理汇总了C++中symbol_shorthand::G方法的典型用法代码示例。如果您正苦于以下问题：C++ symbol_shorthand::G方法的具体用法？C++ symbol_shorthand::G怎么用？C++ symbol_shorthand::G使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类symbol_shorthand的用法示例。
在下文中一共展示了symbol_shorthand::G方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: optimizationLoop
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)
{
//.........这里部分代码省略.........