Merged in fix/ImuFactor (pull request #325)
close issue #280 TBB/cython bug on Mac seems unrelatedrelease/4.3a0
Frank Dellaert
commit
c6d9baf3ce
|
|
@ -0,0 +1,176 @@
|
|||
"""
|
||||
iSAM2 example with ImuFactor.
|
||||
Author: Robert Truax (C++), Frank Dellaert (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
|
||||
|
||||
import gtsam
|
||||
import gtsam.utils.plot as gtsam_plot
|
||||
|
||||
|
||||
def X(key):
|
||||
"""Create symbol for pose key."""
|
||||
return gtsam.symbol(ord('x'), key)
|
||||
|
||||
|
||||
def V(key):
|
||||
"""Create symbol for velocity key."""
|
||||
return gtsam.symbol(ord('v'), key)
|
||||
|
||||
|
||||
def vector3(x, y, z):
|
||||
"""Create 3d double numpy array."""
|
||||
return np.array([x, y, z], dtype=np.float)
|
||||
|
||||
|
||||
def ISAM2_plot(values, fignum=0):
|
||||
"""Plot poses."""
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca(projection='3d')
|
||||
plt.cla()
|
||||
|
||||
i = 0
|
||||
min_bounds = 0, 0, 0
|
||||
max_bounds = 0, 0, 0
|
||||
while values.exists(X(i)):
|
||||
pose_i = values.atPose3(X(i))
|
||||
gtsam_plot.plot_pose3(fignum, pose_i, 10)
|
||||
position = pose_i.translation().vector()
|
||||
min_bounds = [min(v1, v2) for (v1, v2) in zip(position, min_bounds)]
|
||||
max_bounds = [max(v1, v2) for (v1, v2) in zip(position, max_bounds)]
|
||||
# max_bounds = min(pose_i.x(), max_bounds[0]), 0, 0
|
||||
i += 1
|
||||
|
||||
# draw
|
||||
axes.set_xlim3d(min_bounds[0]-1, max_bounds[0]+1)
|
||||
axes.set_ylim3d(min_bounds[1]-1, max_bounds[1]+1)
|
||||
axes.set_zlim3d(min_bounds[2]-1, max_bounds[2]+1)
|
||||
plt.pause(1)
|
||||
|
||||
|
||||
# IMU preintegration parameters
|
||||
# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
|
||||
g = 9.81
|
||||
n_gravity = vector3(0, 0, -g)
|
||||
PARAMS = gtsam.PreintegrationParams.MakeSharedU(g)
|
||||
I = np.eye(3)
|
||||
PARAMS.setAccelerometerCovariance(I * 0.1)
|
||||
PARAMS.setGyroscopeCovariance(I * 0.1)
|
||||
PARAMS.setIntegrationCovariance(I * 0.1)
|
||||
PARAMS.setUse2ndOrderCoriolis(False)
|
||||
PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
|
||||
|
||||
BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
|
||||
DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0),
|
||||
gtsam.Point3(0.05, -0.10, 0.20))
|
||||
|
||||
|
||||
def IMU_example():
|
||||
"""Run iSAM 2 example with IMU factor."""
|
||||
|
||||
# Start with a camera on x-axis looking at origin
|
||||
radius = 30
|
||||
up = gtsam.Point3(0, 0, 1)
|
||||
target = gtsam.Point3(0, 0, 0)
|
||||
position = gtsam.Point3(radius, 0, 0)
|
||||
camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
|
||||
pose_0 = camera.pose()
|
||||
|
||||
# Create the set of ground-truth landmarks and poses
|
||||
angular_velocity = math.radians(180) # rad/sec
|
||||
delta_t = 1.0/18 # makes for 10 degrees per step
|
||||
|
||||
angular_velocity_vector = vector3(0, -angular_velocity, 0)
|
||||
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
|
||||
scenario = gtsam.ConstantTwistScenario(
|
||||
angular_velocity_vector, linear_velocity_vector, pose_0)
|
||||
|
||||
# Create a factor graph
|
||||
newgraph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# Create (incremental) ISAM2 solver
|
||||
isam = gtsam.ISAM2()
|
||||
|
||||
# Create the initial estimate to the solution
|
||||
# Intentionally initialize the variables off from the ground truth
|
||||
initialEstimate = gtsam.Values()
|
||||
|
||||
# Add a prior on pose x0. This indirectly specifies where the origin is.
|
||||
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
noise = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
|
||||
newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
|
||||
|
||||
# Add imu priors
|
||||
biasKey = gtsam.symbol(ord('b'), 0)
|
||||
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
|
||||
biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
|
||||
biasnoise)
|
||||
newgraph.push_back(biasprior)
|
||||
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
|
||||
velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
|
||||
|
||||
# Calculate with correct initial velocity
|
||||
n_velocity = vector3(0, angular_velocity * radius, 0)
|
||||
velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
|
||||
newgraph.push_back(velprior)
|
||||
initialEstimate.insert(V(0), n_velocity)
|
||||
|
||||
accum = gtsam.PreintegratedImuMeasurements(PARAMS)
|
||||
|
||||
# Simulate poses and imu measurements, adding them to the factor graph
|
||||
for i in range(80):
|
||||
t = i * delta_t # simulation time
|
||||
if i == 0: # First time add two poses
|
||||
pose_1 = scenario.pose(delta_t)
|
||||
initialEstimate.insert(X(0), pose_0.compose(DELTA))
|
||||
initialEstimate.insert(X(1), pose_1.compose(DELTA))
|
||||
elif i >= 2: # Add more poses as necessary
|
||||
pose_i = scenario.pose(t)
|
||||
initialEstimate.insert(X(i), pose_i.compose(DELTA))
|
||||
|
||||
if i > 0:
|
||||
# Add Bias variables periodically
|
||||
if i % 5 == 0:
|
||||
biasKey += 1
|
||||
factor = gtsam.BetweenFactorConstantBias(
|
||||
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
|
||||
newgraph.add(factor)
|
||||
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
|
||||
|
||||
# Predict acceleration and gyro measurements in (actual) body frame
|
||||
nRb = scenario.rotation(t).matrix()
|
||||
bRn = np.transpose(nRb)
|
||||
measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
|
||||
measuredOmega = scenario.omega_b(t)
|
||||
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
|
||||
|
||||
# Add Imu Factor
|
||||
imufac = gtsam.ImuFactor(
|
||||
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
|
||||
newgraph.add(imufac)
|
||||
|
||||
# insert new velocity, which is wrong
|
||||
initialEstimate.insert(V(i), n_velocity)
|
||||
accum.resetIntegration()
|
||||
|
||||
# Incremental solution
|
||||
isam.update(newgraph, initialEstimate)
|
||||
result = isam.calculateEstimate()
|
||||
ISAM2_plot(result)
|
||||
|
||||
# reset
|
||||
newgraph = gtsam.NonlinearFactorGraph()
|
||||
initialEstimate.clear()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
IMU_example()
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
import math
|
||||
import unittest
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
|
||||
class TestScenario(unittest.TestCase):
|
||||
def setUp(self):
|
||||
pass
|
||||
|
||||
def test_loop(self):
|
||||
# Forward velocity 2m/s
|
||||
# Pitch up with angular velocity 6 degree/sec (negative in FLU)
|
||||
v = 2
|
||||
w = math.radians(6)
|
||||
W = np.array([0, -w, 0])
|
||||
V = np.array([v, 0, 0])
|
||||
scenario = gtsam.ConstantTwistScenario(W, V)
|
||||
|
||||
T = 30
|
||||
np.testing.assert_almost_equal(W, scenario.omega_b(T))
|
||||
np.testing.assert_almost_equal(V, scenario.velocity_b(T))
|
||||
np.testing.assert_almost_equal(
|
||||
np.cross(W, V), scenario.acceleration_b(T))
|
||||
|
||||
# R = v/w, so test if loop crests at 2*R
|
||||
R = v / w
|
||||
T30 = scenario.pose(T)
|
||||
np.testing.assert_almost_equal(
|
||||
np.array([math.pi, 0, math.pi]), T30.rotation().xyz())
|
||||
self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,136 @@
|
|||
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/navigation/Scenario.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Shorthand for velocity and pose variables
|
||||
using symbol_shorthand::V;
|
||||
using symbol_shorthand::X;
|
||||
|
||||
const double kGravity = 9.81;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(int argc, char* argv[]) {
|
||||
auto params = PreintegrationParams::MakeSharedU(kGravity);
|
||||
params->setAccelerometerCovariance(I_3x3 * 0.1);
|
||||
params->setGyroscopeCovariance(I_3x3 * 0.1);
|
||||
params->setIntegrationCovariance(I_3x3 * 0.1);
|
||||
params->setUse2ndOrderCoriolis(false);
|
||||
params->setOmegaCoriolis(Vector3(0, 0, 0));
|
||||
|
||||
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
|
||||
|
||||
// Start with a camera on x-axis looking at origin
|
||||
double radius = 30;
|
||||
const Point3 up(0, 0, 1), target(0, 0, 0);
|
||||
const Point3 position(radius, 0, 0);
|
||||
const SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
|
||||
const auto pose_0 = camera.pose();
|
||||
|
||||
// Now, create a constant-twist scenario that makes the camera orbit the
|
||||
// origin
|
||||
double angular_velocity = M_PI, // rad/sec
|
||||
delta_t = 1.0 / 18; // makes for 10 degrees per step
|
||||
Vector3 angular_velocity_vector(0, -angular_velocity, 0);
|
||||
Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0);
|
||||
auto scenario = ConstantTwistScenario(angular_velocity_vector,
|
||||
linear_velocity_vector, pose_0);
|
||||
|
||||
// Create a factor graph
|
||||
NonlinearFactorGraph newgraph;
|
||||
|
||||
// Create (incremental) ISAM2 solver
|
||||
ISAM2 isam;
|
||||
|
||||
// Create the initial estimate to the solution
|
||||
// Intentionally initialize the variables off from the ground truth
|
||||
Values initialEstimate, totalEstimate, result;
|
||||
|
||||
// Add a prior on pose x0. This indirectly specifies where the origin is.
|
||||
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
auto noise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
|
||||
newgraph.push_back(PriorFactor<Pose3>(X(0), pose_0, noise));
|
||||
|
||||
// Add imu priors
|
||||
Key biasKey = Symbol('b', 0);
|
||||
auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
|
||||
PriorFactor<imuBias::ConstantBias> biasprior(biasKey, imuBias::ConstantBias(),
|
||||
biasnoise);
|
||||
newgraph.push_back(biasprior);
|
||||
initialEstimate.insert(biasKey, imuBias::ConstantBias());
|
||||
auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
|
||||
|
||||
Vector n_velocity(3);
|
||||
n_velocity << 0, angular_velocity * radius, 0;
|
||||
PriorFactor<Vector> velprior(V(0), n_velocity, velnoise);
|
||||
newgraph.push_back(velprior);
|
||||
|
||||
initialEstimate.insert(V(0), n_velocity);
|
||||
|
||||
// IMU preintegrator
|
||||
PreintegratedImuMeasurements accum(params);
|
||||
|
||||
// Simulate poses and imu measurements, adding them to the factor graph
|
||||
for (size_t i = 0; i < 36; ++i) {
|
||||
double t = i * delta_t;
|
||||
if (i == 0) { // First time add two poses
|
||||
auto pose_1 = scenario.pose(delta_t);
|
||||
initialEstimate.insert(X(0), pose_0.compose(delta));
|
||||
initialEstimate.insert(X(1), pose_1.compose(delta));
|
||||
} else if (i >= 2) { // Add more poses as necessary
|
||||
auto pose_i = scenario.pose(t);
|
||||
initialEstimate.insert(X(i), pose_i.compose(delta));
|
||||
}
|
||||
|
||||
if (i > 0) {
|
||||
// Add Bias variables periodically
|
||||
if (i % 5 == 0) {
|
||||
biasKey++;
|
||||
Symbol b1 = biasKey - 1;
|
||||
Symbol b2 = biasKey;
|
||||
Vector6 covvec;
|
||||
covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
|
||||
auto cov = noiseModel::Diagonal::Variances(covvec);
|
||||
Vector6 zerovec;
|
||||
zerovec << 0, 0, 0, 0, 0, 0;
|
||||
auto f = boost::make_shared<BetweenFactor<imuBias::ConstantBias> >(
|
||||
b1, b2, imuBias::ConstantBias(), cov);
|
||||
newgraph.add(f);
|
||||
initialEstimate.insert(biasKey, imuBias::ConstantBias());
|
||||
}
|
||||
// Predict acceleration and gyro measurements in (actual) body frame
|
||||
auto measuredAcc = scenario.acceleration_b(t) -
|
||||
scenario.rotation(t).transpose() * params->n_gravity;
|
||||
auto measuredOmega = scenario.omega_b(t);
|
||||
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t);
|
||||
|
||||
// Add Imu Factor
|
||||
ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum);
|
||||
newgraph.add(imufac);
|
||||
|
||||
// insert new velocity, which is wrong
|
||||
initialEstimate.insert(V(i), n_velocity);
|
||||
accum.resetIntegration();
|
||||
}
|
||||
|
||||
// Incremental solution
|
||||
isam.update(newgraph, initialEstimate);
|
||||
result = isam.calculateEstimate();
|
||||
newgraph = NonlinearFactorGraph();
|
||||
initialEstimate.clear();
|
||||
}
|
||||
GTSAM_PRINT(result);
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
61
gtsam.h
61
gtsam.h
|
|
@ -568,7 +568,7 @@ class Pose2 {
|
|||
static gtsam::Pose2 Expmap(Vector v);
|
||||
static Vector Logmap(const gtsam::Pose2& p);
|
||||
Matrix AdjointMap() const;
|
||||
Vector Adjoint(const Vector& xi) const;
|
||||
Vector Adjoint(Vector xi) const;
|
||||
static Matrix wedge(double vx, double vy, double w);
|
||||
|
||||
// Group Actions on Point2
|
||||
|
|
@ -865,7 +865,7 @@ class CalibratedCamera {
|
|||
// Standard Constructors and Named Constructors
|
||||
CalibratedCamera();
|
||||
CalibratedCamera(const gtsam::Pose3& pose);
|
||||
CalibratedCamera(const Vector& v);
|
||||
CalibratedCamera(Vector v);
|
||||
static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height);
|
||||
|
||||
// Testable
|
||||
|
|
@ -875,7 +875,7 @@ class CalibratedCamera {
|
|||
// Manifold
|
||||
static size_t Dim();
|
||||
size_t dim() const;
|
||||
gtsam::CalibratedCamera retract(const Vector& d) const;
|
||||
gtsam::CalibratedCamera retract(Vector d) const;
|
||||
Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
|
||||
|
||||
// Action on Point3
|
||||
|
|
@ -911,7 +911,7 @@ class PinholeCamera {
|
|||
CALIBRATION calibration() const;
|
||||
|
||||
// Manifold
|
||||
This retract(const Vector& d) const;
|
||||
This retract(Vector d) const;
|
||||
Vector localCoordinates(const This& T2) const;
|
||||
size_t dim() const;
|
||||
static size_t Dim();
|
||||
|
|
@ -950,7 +950,7 @@ virtual class SimpleCamera {
|
|||
gtsam::Cal3_S2 calibration() const;
|
||||
|
||||
// Manifold
|
||||
gtsam::SimpleCamera retract(const Vector& d) const;
|
||||
gtsam::SimpleCamera retract(Vector d) const;
|
||||
Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
|
||||
size_t dim() const;
|
||||
static size_t Dim();
|
||||
|
|
@ -992,7 +992,7 @@ class StereoCamera {
|
|||
gtsam::Cal3_S2Stereo calibration() const;
|
||||
|
||||
// Manifold
|
||||
gtsam::StereoCamera retract(const Vector& d) const;
|
||||
gtsam::StereoCamera retract(Vector d) const;
|
||||
Vector localCoordinates(const gtsam::StereoCamera& T2) const;
|
||||
size_t dim() const;
|
||||
static size_t Dim();
|
||||
|
|
@ -1227,12 +1227,12 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
|
|||
};
|
||||
|
||||
virtual class Constrained : gtsam::noiseModel::Diagonal {
|
||||
static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas);
|
||||
static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas);
|
||||
static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances);
|
||||
static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances);
|
||||
static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions);
|
||||
static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions);
|
||||
static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas);
|
||||
static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas);
|
||||
static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances);
|
||||
static gtsam::noiseModel::Constrained* MixedVariances(Vector variances);
|
||||
static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions);
|
||||
static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions);
|
||||
|
||||
static gtsam::noiseModel::Constrained* All(size_t dim);
|
||||
static gtsam::noiseModel::Constrained* All(size_t dim, double mu);
|
||||
|
|
@ -1415,12 +1415,12 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
|
|||
pair<Matrix, Vector> jacobianUnweighted() const;
|
||||
Matrix augmentedJacobianUnweighted() const;
|
||||
|
||||
void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const;
|
||||
void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const;
|
||||
gtsam::JacobianFactor whiten() const;
|
||||
|
||||
pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
|
||||
|
||||
void setModel(bool anyConstrained, const Vector& sigmas);
|
||||
void setModel(bool anyConstrained, Vector sigmas);
|
||||
|
||||
gtsam::noiseModel::Diagonal* get_model() const;
|
||||
|
||||
|
|
@ -2665,10 +2665,12 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
|
|||
gtsam::Unit3 bRef() const;
|
||||
};
|
||||
|
||||
virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
|
||||
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
|
||||
const gtsam::Unit3& bRef);
|
||||
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
|
||||
virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor {
|
||||
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
|
||||
const gtsam::noiseModel::Diagonal* model,
|
||||
const gtsam::Unit3& bRef);
|
||||
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
|
||||
const gtsam::noiseModel::Diagonal* model);
|
||||
Pose3AttitudeFactor();
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
|
||||
|
|
@ -2677,24 +2679,27 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
|
|||
};
|
||||
|
||||
#include <gtsam/navigation/Scenario.h>
|
||||
virtual class Scenario {};
|
||||
|
||||
virtual class ConstantTwistScenario : gtsam::Scenario {
|
||||
ConstantTwistScenario(const Vector& w, const Vector& v);
|
||||
virtual class Scenario {
|
||||
gtsam::Pose3 pose(double t) const;
|
||||
Vector omega_b(double t) const;
|
||||
Vector velocity_n(double t) const;
|
||||
Vector acceleration_n(double t) const;
|
||||
gtsam::Rot3 rotation(double t) const;
|
||||
gtsam::NavState navState(double t) const;
|
||||
Vector velocity_b(double t) const;
|
||||
Vector acceleration_b(double t) const;
|
||||
};
|
||||
|
||||
virtual class ConstantTwistScenario : gtsam::Scenario {
|
||||
ConstantTwistScenario(Vector w, Vector v);
|
||||
ConstantTwistScenario(Vector w, Vector v,
|
||||
const Pose3& nTb0);
|
||||
};
|
||||
|
||||
virtual class AcceleratingScenario : gtsam::Scenario {
|
||||
AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0,
|
||||
const Vector& v0, const Vector& a_n,
|
||||
const Vector& omega_b);
|
||||
gtsam::Pose3 pose(double t) const;
|
||||
Vector omega_b(double t) const;
|
||||
Vector velocity_n(double t) const;
|
||||
Vector acceleration_n(double t) const;
|
||||
Vector v0, Vector a_n,
|
||||
Vector omega_b);
|
||||
};
|
||||
|
||||
//*************************************************************************
|
||||
|
|
|
|||
|
|
@ -57,10 +57,13 @@ class Scenario {
|
|||
class ConstantTwistScenario : public Scenario {
|
||||
public:
|
||||
/// Construct scenario with constant twist [w,v]
|
||||
ConstantTwistScenario(const Vector3& w, const Vector3& v)
|
||||
: twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)) {}
|
||||
ConstantTwistScenario(const Vector3& w, const Vector3& v,
|
||||
const Pose3& nTb0 = Pose3())
|
||||
: twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {}
|
||||
|
||||
Pose3 pose(double t) const override { return Pose3::Expmap(twist_ * t); }
|
||||
Pose3 pose(double t) const override {
|
||||
return nTb0_ * Pose3::Expmap(twist_ * t);
|
||||
}
|
||||
Vector3 omega_b(double t) const override { return twist_.head<3>(); }
|
||||
Vector3 velocity_n(double t) const override {
|
||||
return rotation(t).matrix() * twist_.tail<3>();
|
||||
|
|
@ -70,6 +73,7 @@ class ConstantTwistScenario : public Scenario {
|
|||
private:
|
||||
const Vector6 twist_;
|
||||
const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b
|
||||
const Pose3 nTb0_;
|
||||
};
|
||||
|
||||
/// Accelerating from an arbitrary initial state, with optional rotation
|
||||
|
|
|
|||
|
|
@ -15,8 +15,8 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/navigation/Scenario.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/navigation/Scenario.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/bind.hpp>
|
||||
|
|
@ -96,9 +96,33 @@ TEST(Scenario, Loop) {
|
|||
const double R = v / w;
|
||||
const Pose3 T30 = scenario.pose(30);
|
||||
EXPECT(assert_equal(Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9));
|
||||
EXPECT(assert_equal(Vector3(M_PI, 0, M_PI), T30.rotation().xyz()));
|
||||
EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Scenario, LoopWithInitialPose) {
|
||||
// Forward velocity 2m/s
|
||||
// Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
|
||||
const double v = 2, w = 6 * kDegree;
|
||||
const Vector3 W(0, -w, 0), V(v, 0, 0);
|
||||
const Rot3 nRb0 = Rot3::yaw(M_PI);
|
||||
const Pose3 nTb0(nRb0, Point3(1, 2, 3));
|
||||
const ConstantTwistScenario scenario(W, V, nTb0);
|
||||
|
||||
const double T = 30;
|
||||
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
|
||||
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
|
||||
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
|
||||
|
||||
// R = v/w, so test if loop crests at 2*R
|
||||
const double R = v / w;
|
||||
const Pose3 T30 = scenario.pose(30);
|
||||
EXPECT(
|
||||
assert_equal(nRb0 * Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9));
|
||||
EXPECT(assert_equal(Point3(1, 2, 3 + 2 * R), T30.translation(), 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Scenario, Accelerating) {
|
||||
// Set up body pointing towards y axis, and start at 10,20,0 with velocity
|
||||
|
|
|
|||
Loading…
Reference in New Issue