Merged gtborg/gtsam into develop
Mike Sheffler
commit
136559d24a
|
|
@ -52,7 +52,7 @@ function(pyx_to_cpp target pyx_file generated_cpp include_dirs)
|
|||
add_custom_command(
|
||||
OUTPUT ${generated_cpp}
|
||||
COMMAND
|
||||
${CYTHON_EXECUTABLE} -X boundscheck=False -a -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
|
||||
${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
|
||||
VERBATIM)
|
||||
add_custom_target(${target} ALL DEPENDS ${generated_cpp})
|
||||
endfunction()
|
||||
|
|
@ -95,7 +95,7 @@ function(cythonize target pyx_file output_lib_we output_dir include_dirs libs in
|
|||
|
||||
# Late dependency injection, to make sure this gets called whenever the interface header is updated
|
||||
# See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes
|
||||
add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} APPEND)
|
||||
add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} ${pyx_file} APPEND)
|
||||
if (NOT "${dependencies}" STREQUAL "")
|
||||
add_dependencies(${target}_pyx2cpp "${dependencies}")
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -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,52 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Simple robot motion example, with prior and two odometry measurements
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
# Create noise models
|
||||
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
|
||||
# Create an empty nonlinear factor graph
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# Add a prior on the first pose, setting it to the origin
|
||||
# A prior factor consists of a mean and a noise model (covariance matrix)
|
||||
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
|
||||
graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE))
|
||||
|
||||
# Add odometry factors
|
||||
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
|
||||
# For simplicity, we will use the same noise model for each odometry factor
|
||||
# Create odometry (Between) factors between consecutive poses
|
||||
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE))
|
||||
print("\nFactor Graph:\n{}".format(graph))
|
||||
|
||||
# Create the data structure to hold the initialEstimate estimate to the solution
|
||||
# For illustrative purposes, these have been deliberately set to incorrect values
|
||||
initial = gtsam.Values()
|
||||
initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||
initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||
initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
|
||||
print("\nInitial Estimate:\n{}".format(initial))
|
||||
|
||||
# optimize using Levenberg-Marquardt optimization
|
||||
params = gtsam.LevenbergMarquardtParams()
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
|
||||
result = optimizer.optimize()
|
||||
print("\nFinal Result:\n{}".format(result))
|
||||
|
|
@ -0,0 +1,80 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Simple robotics example using odometry measurements and bearing-range (laser) measurements
|
||||
Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
# Create noise models
|
||||
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
|
||||
|
||||
# Create an empty nonlinear factor graph
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# Create the keys corresponding to unknown variables in the factor graph
|
||||
X1 = gtsam.symbol(ord('x'), 1)
|
||||
X2 = gtsam.symbol(ord('x'), 2)
|
||||
X3 = gtsam.symbol(ord('x'), 3)
|
||||
L1 = gtsam.symbol(ord('l'), 4)
|
||||
L2 = gtsam.symbol(ord('l'), 5)
|
||||
|
||||
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
|
||||
graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
|
||||
|
||||
# Add odometry factors between X1,X2 and X2,X3, respectively
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
|
||||
|
||||
# Add Range-Bearing measurements to two different landmarks L1 and L2
|
||||
graph.add(gtsam.BearingRangeFactor2D(
|
||||
X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))
|
||||
graph.add(gtsam.BearingRangeFactor2D(
|
||||
X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
|
||||
graph.add(gtsam.BearingRangeFactor2D(
|
||||
X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
|
||||
|
||||
# Print graph
|
||||
print("Factor Graph:\n{}".format(graph))
|
||||
|
||||
# Create (deliberately inaccurate) initial estimate
|
||||
initial_estimate = gtsam.Values()
|
||||
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
|
||||
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
|
||||
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
|
||||
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
|
||||
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
|
||||
|
||||
# Print
|
||||
print("Initial Estimate:\n{}".format(initial_estimate))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization. The optimizer
|
||||
# accepts an optional set of configuration parameters, controlling
|
||||
# things like convergence criteria, the type of linear system solver
|
||||
# to use, and the amount of information displayed during optimization.
|
||||
# Here we will use the default set of parameters. See the
|
||||
# documentation for the full set of parameters.
|
||||
params = gtsam.LevenbergMarquardtParams()
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)
|
||||
result = optimizer.optimize()
|
||||
print("\nFinal Result:\n{}".format(result))
|
||||
|
||||
# Calculate and print marginal covariances for all variables
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]:
|
||||
print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key)))
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Simple robotics example using odometry measurements and bearing-range (laser) measurements
|
||||
Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
|
||||
def vector3(x, y, z):
|
||||
"""Create 3d double numpy array."""
|
||||
return np.array([x, y, z], dtype=np.float)
|
||||
|
||||
|
||||
# Create noise models
|
||||
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.3, 0.3, 0.1))
|
||||
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.2, 0.2, 0.1))
|
||||
|
||||
# 1. Create a factor graph container and add factors to it
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# 2a. Add a prior on the first pose, setting it to the origin
|
||||
# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix)
|
||||
graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
|
||||
|
||||
# 2b. Add odometry factors
|
||||
# Create odometry (Between) factors between consecutive poses
|
||||
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
||||
|
||||
# 2c. Add the loop closure constraint
|
||||
# This factor encodes the fact that we have returned to the same pose. In real
|
||||
# systems, these constraints may be identified in many ways, such as appearance-based
|
||||
# techniques with camera images. We will use another Between Factor to enforce this constraint:
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
||||
print("\nFactor Graph:\n{}".format(graph)) # print
|
||||
|
||||
# 3. Create the data structure to hold the initial_estimate estimate to the
|
||||
# solution. For illustrative purposes, these have been deliberately set to incorrect values
|
||||
initial_estimate = gtsam.Values()
|
||||
initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||
initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||
initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2))
|
||||
initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi))
|
||||
initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2))
|
||||
print("\nInitial Estimate:\n{}".format(initial_estimate)) # print
|
||||
|
||||
# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
|
||||
# The optimizer accepts an optional set of configuration parameters,
|
||||
# controlling things like convergence criteria, the type of linear
|
||||
# system solver to use, and the amount of information displayed during
|
||||
# optimization. We will set a few parameters as a demonstration.
|
||||
parameters = gtsam.GaussNewtonParams()
|
||||
|
||||
# Stop iterating once the change in error between steps is less than this value
|
||||
parameters.setRelativeErrorTol(1e-5)
|
||||
# Do not perform more than N iteration steps
|
||||
parameters.setMaxIterations(100)
|
||||
# Create the optimizer ...
|
||||
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters)
|
||||
# ... and optimize
|
||||
result = optimizer.optimize()
|
||||
print("Final Result:\n{}".format(result))
|
||||
|
||||
# 5. Calculate and print marginal covariances for all variables
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
for i in range(1, 6):
|
||||
print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
These examples are almost identical to the old handwritten python wrapper
|
||||
examples. However, there are just some slight name changes, for example
|
||||
noiseModel.Diagonal becomes noiseModel_Diagonal etc...
|
||||
Also, annoyingly, instead of gtsam.Symbol('b',0) we now need to say gtsam.symbol(ord('b'), 0))
|
||||
|
|
@ -0,0 +1,38 @@
|
|||
"""
|
||||
A structure-from-motion example with landmarks
|
||||
- The landmarks form a 10 meter cube
|
||||
- The robot rotates around the landmarks, always facing towards the cube
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
|
||||
def createPoints():
|
||||
# Create the set of ground-truth landmarks
|
||||
points = [gtsam.Point3(10.0, 10.0, 10.0),
|
||||
gtsam.Point3(-10.0, 10.0, 10.0),
|
||||
gtsam.Point3(-10.0, -10.0, 10.0),
|
||||
gtsam.Point3(10.0, -10.0, 10.0),
|
||||
gtsam.Point3(10.0, 10.0, -10.0),
|
||||
gtsam.Point3(-10.0, 10.0, -10.0),
|
||||
gtsam.Point3(-10.0, -10.0, -10.0),
|
||||
gtsam.Point3(10.0, -10.0, -10.0)]
|
||||
return points
|
||||
|
||||
|
||||
def createPoses(K):
|
||||
# Create the set of ground-truth poses
|
||||
radius = 30.0
|
||||
angles = np.linspace(0, 2*np.pi, 8, endpoint=False)
|
||||
up = gtsam.Point3(0, 0, 1)
|
||||
target = gtsam.Point3(0, 0, 0)
|
||||
poses = []
|
||||
for theta in angles:
|
||||
position = gtsam.Point3(radius*np.cos(theta),
|
||||
radius*np.sin(theta), 0.0)
|
||||
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
|
||||
poses.append(camera.pose())
|
||||
return poses
|
||||
|
|
@ -0,0 +1,163 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
An example of running visual SLAM using iSAM2.
|
||||
Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
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
|
||||
from gtsam.examples import SFMdata
|
||||
|
||||
|
||||
def X(i):
|
||||
"""Create key for pose i."""
|
||||
return int(gtsam.symbol(ord('x'), i))
|
||||
|
||||
|
||||
def L(j):
|
||||
"""Create key for landmark j."""
|
||||
return int(gtsam.symbol(ord('l'), j))
|
||||
|
||||
|
||||
def visual_ISAM2_plot(result):
|
||||
"""
|
||||
VisualISAMPlot plots current state of ISAM2 object
|
||||
Author: Ellon Paiva
|
||||
Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
|
||||
"""
|
||||
|
||||
# Declare an id for the figure
|
||||
fignum = 0
|
||||
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca(projection='3d')
|
||||
plt.cla()
|
||||
|
||||
# Plot points
|
||||
# Can't use data because current frame might not see all points
|
||||
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate())
|
||||
# gtsam.plot_3d_points(result, [], marginals)
|
||||
gtsam_plot.plot_3d_points(fignum, result, 'rx')
|
||||
|
||||
# Plot cameras
|
||||
i = 0
|
||||
while result.exists(X(i)):
|
||||
pose_i = result.atPose3(X(i))
|
||||
gtsam_plot.plot_pose3(fignum, pose_i, 10)
|
||||
i += 1
|
||||
|
||||
# draw
|
||||
axes.set_xlim3d(-40, 40)
|
||||
axes.set_ylim3d(-40, 40)
|
||||
axes.set_zlim3d(-40, 40)
|
||||
plt.pause(1)
|
||||
|
||||
|
||||
def visual_ISAM2_example():
|
||||
plt.ion()
|
||||
|
||||
# Define the camera calibration parameters
|
||||
K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
|
||||
|
||||
# Define the camera observation noise model
|
||||
measurement_noise = gtsam.noiseModel_Isotropic.Sigma(
|
||||
2, 1.0) # one pixel in u and v
|
||||
|
||||
# Create the set of ground-truth landmarks
|
||||
points = SFMdata.createPoints()
|
||||
|
||||
# Create the set of ground-truth poses
|
||||
poses = SFMdata.createPoses(K)
|
||||
|
||||
# Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
|
||||
# to maintain proper linearization and efficient variable ordering, iSAM2
|
||||
# performs partial relinearization/reordering at each step. A parameter
|
||||
# structure is available that allows the user to set various properties, such
|
||||
# as the relinearization threshold and type of linear solver. For this
|
||||
# example, we we set the relinearization threshold small so the iSAM2 result
|
||||
# will approach the batch result.
|
||||
parameters = gtsam.ISAM2Params()
|
||||
parameters.setRelinearizeThreshold(0.01)
|
||||
parameters.setRelinearizeSkip(1)
|
||||
isam = gtsam.ISAM2(parameters)
|
||||
|
||||
# Create a Factor Graph and Values to hold the new data
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
initial_estimate = gtsam.Values()
|
||||
|
||||
# Loop over the different poses, adding the observations to iSAM incrementally
|
||||
for i, pose in enumerate(poses):
|
||||
|
||||
# Add factors for each landmark observation
|
||||
for j, point in enumerate(points):
|
||||
camera = gtsam.PinholeCameraCal3_S2(pose, K)
|
||||
measurement = camera.project(point)
|
||||
graph.push_back(gtsam.GenericProjectionFactorCal3_S2(
|
||||
measurement, measurement_noise, X(i), L(j), K))
|
||||
|
||||
# Add an initial guess for the current pose
|
||||
# Intentionally initialize the variables off from the ground truth
|
||||
initial_estimate.insert(X(i), pose.compose(gtsam.Pose3(
|
||||
gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
|
||||
|
||||
# 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
|
||||
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array(
|
||||
[0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))
|
||||
|
||||
# Add a prior on landmark l0
|
||||
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
|
||||
graph.push_back(gtsam.PriorFactorPoint3(
|
||||
L(0), points[0], point_noise)) # add directly to graph
|
||||
|
||||
# Add initial guesses to all observed landmarks
|
||||
# Intentionally initialize the variables off from the ground truth
|
||||
for j, point in enumerate(points):
|
||||
initial_estimate.insert(L(j), gtsam.Point3(
|
||||
point.x()-0.25, point.y()+0.20, point.z()+0.15))
|
||||
else:
|
||||
# Update iSAM with the new factors
|
||||
isam.update(graph, initial_estimate)
|
||||
# Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver.
|
||||
# If accuracy is desired at the expense of time, update(*) can be called additional
|
||||
# times to perform multiple optimizer iterations every step.
|
||||
isam.update()
|
||||
current_estimate = isam.calculateEstimate()
|
||||
print("****************************************************")
|
||||
print("Frame", i, ":")
|
||||
for j in range(i + 1):
|
||||
print(X(j), ":", current_estimate.atPose3(X(j)))
|
||||
|
||||
for j in range(len(points)):
|
||||
print(L(j), ":", current_estimate.atPoint3(L(j)))
|
||||
|
||||
visual_ISAM2_plot(current_estimate)
|
||||
|
||||
# Clear the factor graph and values for the next iteration
|
||||
graph.resize(0)
|
||||
initial_estimate.clear()
|
||||
|
||||
plt.ioff()
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
visual_ISAM2_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,102 @@
|
|||
"""Various plotting utlities."""
|
||||
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
|
||||
def plot_pose2_on_axes(axes, pose, axis_length=0.1):
|
||||
"""Plot a 2D pose on given axis 'axes' with given 'axis_length'."""
|
||||
# get rotation and translation (center)
|
||||
gRp = pose.rotation().matrix() # rotation from pose to global
|
||||
t = pose.translation()
|
||||
origin = np.array([t.x(), t.y()])
|
||||
|
||||
# draw the camera axes
|
||||
x_axis = origin + gRp[:, 0] * axis_length
|
||||
line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], 'r-')
|
||||
|
||||
y_axis = origin + gRp[:, 1] * axis_length
|
||||
line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], 'g-')
|
||||
|
||||
|
||||
def plot_pose2(fignum, pose, axis_length=0.1):
|
||||
"""Plot a 2D pose on given figure with given 'axis_length'."""
|
||||
# get figure object
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca()
|
||||
plot_pose2_on_axes(axes, pose, axis_length)
|
||||
|
||||
|
||||
def plot_point3_on_axes(axes, point, linespec):
|
||||
"""Plot a 3D point on given axis 'axes' with given 'linespec'."""
|
||||
axes.plot([point.x()], [point.y()], [point.z()], linespec)
|
||||
|
||||
|
||||
def plot_point3(fignum, point, linespec):
|
||||
"""Plot a 3D point on given figure with given 'linespec'."""
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca(projection='3d')
|
||||
plot_point3_on_axes(axes, point, linespec)
|
||||
|
||||
|
||||
def plot_3d_points(fignum, values, linespec, marginals=None):
|
||||
"""
|
||||
Plots the Point3s in 'values', with optional covariances.
|
||||
Finds all the Point3 objects in the given Values object and plots them.
|
||||
If a Marginals object is given, this function will also plot marginal
|
||||
covariance ellipses for each point.
|
||||
"""
|
||||
|
||||
keys = values.keys()
|
||||
|
||||
# Plot points and covariance matrices
|
||||
for i in range(keys.size()):
|
||||
try:
|
||||
p = values.atPoint3(keys.at(i))
|
||||
# if haveMarginals
|
||||
# P = marginals.marginalCovariance(key);
|
||||
# gtsam.plot_point3(p, linespec, P);
|
||||
# else
|
||||
plot_point3(fignum, p, linespec)
|
||||
except RuntimeError:
|
||||
continue
|
||||
# I guess it's not a Point3
|
||||
|
||||
|
||||
def plot_pose3_on_axes(axes, pose, axis_length=0.1):
|
||||
"""Plot a 3D pose on given axis 'axes' with given 'axis_length'."""
|
||||
# get rotation and translation (center)
|
||||
gRp = pose.rotation().matrix() # rotation from pose to global
|
||||
t = pose.translation()
|
||||
origin = np.array([t.x(), t.y(), t.z()])
|
||||
|
||||
# draw the camera axes
|
||||
x_axis = origin + gRp[:, 0] * axis_length
|
||||
line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'r-')
|
||||
|
||||
y_axis = origin + gRp[:, 1] * axis_length
|
||||
line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'g-')
|
||||
|
||||
z_axis = origin + gRp[:, 2] * axis_length
|
||||
line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-')
|
||||
|
||||
# plot the covariance
|
||||
# TODO (dellaert): make this work
|
||||
# if (nargin>2) && (~isempty(P))
|
||||
# pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame
|
||||
# gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
|
||||
# gtsam.covarianceEllipse3D(origin,gPp);
|
||||
# end
|
||||
|
||||
|
||||
def plot_pose3(fignum, pose, axis_length=0.1):
|
||||
"""Plot a 3D pose on given figure with given 'axis_length'."""
|
||||
# get figure object
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca(projection='3d')
|
||||
plot_pose3_on_axes(axes, pose, axis_length)
|
||||
|
|
@ -306,10 +306,17 @@ public:
|
|||
: cols),
|
||||
stride
|
||||
) {}
|
||||
|
||||
MapBase &operator=(const MatrixType &other) {
|
||||
Base::operator=(other);
|
||||
return *this;
|
||||
}
|
||||
|
||||
virtual ~MapBase() { }
|
||||
};
|
||||
|
||||
|
||||
template <template<class,int,int,int,int,int> class DenseBase,
|
||||
template <template<class,int,int,int,int,int> class EigencyDenseBase,
|
||||
typename Scalar,
|
||||
int _Rows, int _Cols,
|
||||
int _Options = Eigen::AutoAlign |
|
||||
|
|
@ -341,16 +348,18 @@ template <template<class,int,int,int,int,int> class DenseBase,
|
|||
int _StrideOuter=0, int _StrideInner=0,
|
||||
int _MaxRows = _Rows,
|
||||
int _MaxCols = _Cols>
|
||||
class FlattenedMap: public MapBase<DenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > {
|
||||
class FlattenedMap: public MapBase<EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > {
|
||||
public:
|
||||
typedef MapBase<DenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > Base;
|
||||
typedef MapBase<EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > Base;
|
||||
|
||||
FlattenedMap()
|
||||
: Base(NULL, 0, 0) {}
|
||||
: Base(NULL, 0, 0),
|
||||
object_(NULL) {}
|
||||
|
||||
FlattenedMap(Scalar *data, long rows, long cols, long outer_stride=0, long inner_stride=0)
|
||||
: Base(data, rows, cols,
|
||||
Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)) {
|
||||
Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)),
|
||||
object_(NULL) {
|
||||
}
|
||||
|
||||
FlattenedMap(PyArrayObject *object)
|
||||
|
|
@ -359,18 +368,26 @@ public:
|
|||
(((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
|
||||
(((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0],
|
||||
Eigen::Stride<_StrideOuter, _StrideInner>(_StrideOuter != Eigen::Dynamic ? _StrideOuter : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
|
||||
_StrideInner != Eigen::Dynamic ? _StrideInner : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0])) {
|
||||
_StrideInner != Eigen::Dynamic ? _StrideInner : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0])),
|
||||
object_(object) {
|
||||
|
||||
if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object))
|
||||
throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
|
||||
|
||||
Py_XINCREF(object_);
|
||||
}
|
||||
FlattenedMap &operator=(const FlattenedMap &other) {
|
||||
// Replace the memory that we point to (not a memory allocation)
|
||||
new (this) FlattenedMap(const_cast<Scalar*>(other.data()),
|
||||
other.rows(),
|
||||
other.cols(),
|
||||
other.outerStride(),
|
||||
other.innerStride());
|
||||
if (other.object_) {
|
||||
new (this) FlattenedMap(other.object_);
|
||||
} else {
|
||||
// Replace the memory that we point to (not a memory allocation)
|
||||
new (this) FlattenedMap(const_cast<Scalar*>(other.data()),
|
||||
other.rows(),
|
||||
other.cols(),
|
||||
other.outerStride(),
|
||||
other.innerStride());
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
|
@ -382,9 +399,16 @@ public:
|
|||
return static_cast<Base&>(*this);
|
||||
}
|
||||
|
||||
operator DenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>() const {
|
||||
return DenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>(static_cast<Base>(*this));
|
||||
operator EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>() const {
|
||||
return EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>(static_cast<Base>(*this));
|
||||
}
|
||||
|
||||
virtual ~FlattenedMap() {
|
||||
Py_XDECREF(object_);
|
||||
}
|
||||
|
||||
private:
|
||||
PyArrayObject * const object_;
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -394,39 +418,60 @@ public:
|
|||
typedef MapBase<MatrixType> Base;
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
|
||||
enum {
|
||||
RowsAtCompileTime = Base::Base::RowsAtCompileTime,
|
||||
ColsAtCompileTime = Base::Base::ColsAtCompileTime
|
||||
};
|
||||
|
||||
Map()
|
||||
: Base(NULL, 0, 0) {
|
||||
: Base(NULL,
|
||||
(RowsAtCompileTime == Eigen::Dynamic) ? 0 : RowsAtCompileTime,
|
||||
(ColsAtCompileTime == Eigen::Dynamic) ? 0 : ColsAtCompileTime),
|
||||
object_(NULL) {
|
||||
}
|
||||
|
||||
Map(Scalar *data, long rows, long cols)
|
||||
: Base(data, rows, cols) {}
|
||||
: Base(data, rows, cols),
|
||||
object_(NULL) {}
|
||||
|
||||
Map(PyArrayObject *object)
|
||||
: Base((PyObject*)object == Py_None? NULL: (Scalar *)object->data,
|
||||
// ROW: If array is in row-major order, transpose (see README)
|
||||
(PyObject*)object == Py_None? 0 :
|
||||
(PyArray_IS_C_CONTIGUOUS(object)
|
||||
(!PyArray_IS_F_CONTIGUOUS(object)
|
||||
? ((object->nd == 1)
|
||||
? 1 // ROW: If 1D row-major numpy array, set to 1 (row vector)
|
||||
: object->dimensions[1])
|
||||
: object->dimensions[0]),
|
||||
// COLUMN: If array is in row-major order: transpose (see README)
|
||||
(PyObject*)object == Py_None? 0 :
|
||||
(PyArray_IS_C_CONTIGUOUS(object)
|
||||
(!PyArray_IS_F_CONTIGUOUS(object)
|
||||
? object->dimensions[0]
|
||||
: ((object->nd == 1)
|
||||
? 1 // COLUMN: If 1D col-major numpy array, set to length (column vector)
|
||||
: object->dimensions[1]))) {
|
||||
: object->dimensions[1]))),
|
||||
object_(object) {
|
||||
|
||||
if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object))
|
||||
throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
|
||||
Py_XINCREF(object_);
|
||||
}
|
||||
|
||||
Map &operator=(const Map &other) {
|
||||
// Replace the memory that we point to (not a memory allocation)
|
||||
new (this) Map(const_cast<Scalar*>(other.data()),
|
||||
other.rows(),
|
||||
other.cols());
|
||||
if (other.object_) {
|
||||
new (this) Map(other.object_);
|
||||
} else {
|
||||
// Replace the memory that we point to (not a memory allocation)
|
||||
new (this) Map(const_cast<Scalar*>(other.data()),
|
||||
other.rows(),
|
||||
other.cols());
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
Map &operator=(const MatrixType &other) {
|
||||
MapBase<MatrixType>::operator=(other);
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
|
@ -440,7 +485,14 @@ public:
|
|||
|
||||
operator MatrixType() const {
|
||||
return MatrixType(static_cast<Base>(*this));
|
||||
}
|
||||
}
|
||||
|
||||
virtual ~Map() {
|
||||
Py_XDECREF(object_);
|
||||
}
|
||||
|
||||
private:
|
||||
PyArrayObject * const object_;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,134 @@
|
|||
|
||||
#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.
|
||||
// 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z.
|
||||
auto noise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).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);
|
||||
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;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
82
gtsam.h
82
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();
|
||||
|
|
@ -938,6 +938,8 @@ virtual class SimpleCamera {
|
|||
static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
|
||||
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
|
||||
const gtsam::Point3& upVector, const gtsam::Cal3_S2& K);
|
||||
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
|
||||
const gtsam::Point3& upVector);
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
|
|
@ -948,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();
|
||||
|
|
@ -990,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();
|
||||
|
|
@ -1225,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);
|
||||
|
|
@ -1413,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;
|
||||
|
||||
|
|
@ -2502,6 +2504,11 @@ class NavState {
|
|||
#include <gtsam/navigation/PreintegratedRotation.h>
|
||||
virtual class PreintegratedRotationParams {
|
||||
PreintegratedRotationParams();
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::PreintegratedRotationParams& expected, double tol);
|
||||
|
||||
void setGyroscopeCovariance(Matrix cov);
|
||||
void setOmegaCoriolis(Vector omega);
|
||||
void setBodyPSensor(const gtsam::Pose3& pose);
|
||||
|
|
@ -2509,13 +2516,23 @@ virtual class PreintegratedRotationParams {
|
|||
Matrix getGyroscopeCovariance() const;
|
||||
|
||||
// TODO(frank): allow optional
|
||||
// boost::optional<Vector3> getOmegaCoriolis() const;
|
||||
// boost::optional<Vector> getOmegaCoriolis() const;
|
||||
// boost::optional<Pose3> getBodyPSensor() const;
|
||||
};
|
||||
|
||||
#include <gtsam/navigation/PreintegrationParams.h>
|
||||
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
|
||||
PreintegrationParams(Vector n_gravity);
|
||||
|
||||
static gtsam::PreintegrationParams* MakeSharedD(double g);
|
||||
static gtsam::PreintegrationParams* MakeSharedU(double g);
|
||||
static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81
|
||||
static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::PreintegrationParams& expected, double tol);
|
||||
|
||||
void setAccelerometerCovariance(Matrix cov);
|
||||
void setIntegrationCovariance(Matrix cov);
|
||||
void setUse2ndOrderCoriolis(bool flag);
|
||||
|
|
@ -2523,7 +2540,6 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
|
|||
Matrix getAccelerometerCovariance() const;
|
||||
Matrix getIntegrationCovariance() const;
|
||||
bool getUse2ndOrderCoriolis() const;
|
||||
void print(string s) const;
|
||||
};
|
||||
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
|
|
@ -2649,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;
|
||||
|
|
@ -2660,6 +2678,30 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
|
|||
gtsam::Unit3 bRef() const;
|
||||
};
|
||||
|
||||
#include <gtsam/navigation/Scenario.h>
|
||||
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,
|
||||
Vector v0, Vector a_n,
|
||||
Vector omega_b);
|
||||
};
|
||||
|
||||
//*************************************************************************
|
||||
// Utilities
|
||||
//*************************************************************************
|
||||
|
|
|
|||
|
|
@ -65,69 +65,76 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
|
|||
return Unit3(d[0], d[1], d[2]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Get the axis of rotation with the minimum projected length of the point
|
||||
static Point3 CalculateBestAxis(const Point3& n) {
|
||||
double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
|
||||
if ((mx <= my) && (mx <= mz)) {
|
||||
return Point3(1.0, 0.0, 0.0);
|
||||
} else if ((my <= mx) && (my <= mz)) {
|
||||
return Point3(0.0, 1.0, 0.0);
|
||||
} else {
|
||||
return Point3(0, 0, 1);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
|
||||
#ifdef GTSAM_USE_TBB
|
||||
// NOTE(hayk): At some point it seemed like this reproducably resulted in deadlock. However, I
|
||||
// can't see the reason why and I can no longer reproduce it. It may have been a red herring, or
|
||||
// there is still a latent bug to watch out for.
|
||||
// NOTE(hayk): At some point it seemed like this reproducably resulted in
|
||||
// deadlock. However, I don't know why and I can no longer reproduce it.
|
||||
// It either was a red herring or there is still a latent bug left to debug.
|
||||
tbb::mutex::scoped_lock lock(B_mutex_);
|
||||
#endif
|
||||
|
||||
// Return cached basis if available and the Jacobian isn't needed.
|
||||
if (B_ && !H) {
|
||||
return *B_;
|
||||
}
|
||||
|
||||
// Return cached basis and derivatives if available.
|
||||
if (B_ && H && H_B_) {
|
||||
*H = *H_B_;
|
||||
return *B_;
|
||||
}
|
||||
|
||||
// Get the unit vector and derivative wrt this.
|
||||
// NOTE(hayk): We can't call point3(), because it would recursively call basis().
|
||||
const Point3 n(p_);
|
||||
|
||||
// Get the axis of rotation with the minimum projected length of the point
|
||||
Point3 axis(0, 0, 1);
|
||||
double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
|
||||
if ((mx <= my) && (mx <= mz)) {
|
||||
axis = Point3(1.0, 0.0, 0.0);
|
||||
} else if ((my <= mx) && (my <= mz)) {
|
||||
axis = Point3(0.0, 1.0, 0.0);
|
||||
}
|
||||
|
||||
// Choose the direction of the first basis vector b1 in the tangent plane by crossing n with
|
||||
// the chosen axis.
|
||||
Matrix33 H_B1_n;
|
||||
Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr);
|
||||
|
||||
// Normalize result to get a unit vector: b1 = B1 / |B1|.
|
||||
Matrix33 H_b1_B1;
|
||||
Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr);
|
||||
|
||||
// Get the second basis vector b2, which is orthogonal to n and b1, by crossing them.
|
||||
// No need to normalize this, p and b1 are orthogonal unit vectors.
|
||||
Matrix33 H_b2_n, H_b2_b1;
|
||||
Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr);
|
||||
|
||||
// Create the basis by stacking b1 and b2.
|
||||
B_.reset(Matrix32());
|
||||
(*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
|
||||
const bool cachedBasis = static_cast<bool>(B_);
|
||||
const bool cachedJacobian = static_cast<bool>(H_B_);
|
||||
|
||||
if (H) {
|
||||
// Chain rule tomfoolery to compute the derivative.
|
||||
const Matrix32& H_n_p = *B_;
|
||||
const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p;
|
||||
const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
|
||||
if (!cachedJacobian) {
|
||||
// Compute Jacobian. Recomputes B_
|
||||
Matrix32 B;
|
||||
Matrix62 jacobian;
|
||||
Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1;
|
||||
|
||||
// Cache the derivative and fill the result.
|
||||
H_B_.reset(Matrix62());
|
||||
(*H_B_) << H_b1_p, H_b2_p;
|
||||
// Choose the direction of the first basis vector b1 in the tangent plane
|
||||
// by crossing n with the chosen axis.
|
||||
const Point3 n(p_), axis = CalculateBestAxis(n);
|
||||
const Point3 B1 = gtsam::cross(n, axis, &H_B1_n);
|
||||
|
||||
// Normalize result to get a unit vector: b1 = B1 / |B1|.
|
||||
B.col(0) = normalize(B1, &H_b1_B1);
|
||||
|
||||
// Get the second basis vector b2, which is orthogonal to n and b1.
|
||||
B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1);
|
||||
|
||||
// Chain rule tomfoolery to compute the jacobian.
|
||||
const Matrix32& H_n_p = B;
|
||||
jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p;
|
||||
auto H_b1_p = jacobian.block<3, 2>(0, 0);
|
||||
jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
|
||||
|
||||
// Cache the result and jacobian
|
||||
H_B_.reset(jacobian);
|
||||
B_.reset(B);
|
||||
}
|
||||
|
||||
// Return cached jacobian, possibly computed just above
|
||||
*H = *H_B_;
|
||||
}
|
||||
|
||||
if (!cachedBasis) {
|
||||
// Same calculation as above, without derivatives.
|
||||
// Done after H block, as that possibly computes B_ for the first time
|
||||
Matrix32 B;
|
||||
|
||||
const Point3 n(p_), axis = CalculateBestAxis(n);
|
||||
const Point3 B1 = gtsam::cross(n, axis);
|
||||
B.col(0) = normalize(B1);
|
||||
B.col(1) = gtsam::cross(n, B.col(0));
|
||||
B_.reset(B);
|
||||
}
|
||||
|
||||
return *B_;
|
||||
}
|
||||
|
||||
|
|
@ -257,7 +264,7 @@ Unit3 Unit3::retract(const Vector2& v) const {
|
|||
std::cos(theta) * p_ + xi_hat * (sin(theta) / theta);
|
||||
return Unit3(exp_p_xi_hat);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector2 Unit3::localCoordinates(const Unit3& other) const {
|
||||
const double x = p_.dot(other.p_);
|
||||
|
|
|
|||
|
|
@ -314,15 +314,24 @@ TEST(Unit3, basis) {
|
|||
Unit3 p(0.1, -0.2, 0.9);
|
||||
|
||||
Matrix expected(3, 2);
|
||||
expected << 0.0, -0.994169047, 0.97618706,
|
||||
-0.0233922129, 0.216930458, 0.105264958;
|
||||
expected << 0.0, -0.994169047, 0.97618706, -0.0233922129, 0.216930458, 0.105264958;
|
||||
|
||||
Matrix62 actualH;
|
||||
Matrix actual = p.basis(actualH);
|
||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||
|
||||
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
|
||||
boost::bind(BasisTest, _1, boost::none), p);
|
||||
boost::bind(BasisTest, _1, boost::none), p);
|
||||
|
||||
// without H, first time
|
||||
EXPECT(assert_equal(expected, p.basis(), 1e-6));
|
||||
|
||||
// without H, cached
|
||||
EXPECT(assert_equal(expected, p.basis(), 1e-6));
|
||||
|
||||
// with H, first time
|
||||
EXPECT(assert_equal(expected, p.basis(actualH), 1e-6));
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
|
||||
// with H, cached
|
||||
EXPECT(assert_equal(expected, p.basis(actualH), 1e-6));
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
|
|
@ -432,7 +441,8 @@ TEST(Unit3, ErrorBetweenFactor) {
|
|||
// Add process factors using the dot product error function.
|
||||
SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01);
|
||||
for (size_t i = 0; i < data.size() - 1; i++) {
|
||||
Expression<Vector2> exp(Expression<Unit3>(U(i)), &Unit3::errorVector, Expression<Unit3>(U(i + 1)));
|
||||
Expression<Vector2> exp(Expression<Unit3>(U(i)), &Unit3::errorVector,
|
||||
Expression<Unit3>(U(i + 1)));
|
||||
graph.addExpressionFactor<Vector2>(R_process, Vector2::Zero(), exp);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -26,10 +26,9 @@
|
|||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Character and index key used in VectorValues, GaussianFactorGraph,
|
||||
* GaussianFactor, etc. These keys are generated at runtime from TypedSymbol
|
||||
* keys when linearizing a nonlinear factor graph. This key is not type
|
||||
* safe, so cannot be used with any Nonlinear* classes.
|
||||
* Character and index key used to refer to variables. Will simply cast to a Key,
|
||||
* i.e., a large integer. Keys are used to retrieve values from Values,
|
||||
* specify what variables factors depend on, etc.
|
||||
*/
|
||||
class GTSAM_EXPORT Symbol {
|
||||
protected:
|
||||
|
|
|
|||
|
|
@ -70,9 +70,6 @@ Scatter::Scatter(const GaussianFactorGraph& gfg,
|
|||
iterator first = begin();
|
||||
if (ordering) first += ordering->size();
|
||||
if (first != end()) std::sort(first, end());
|
||||
|
||||
// Filter out keys with zero dimensions (if ordering had more keys)
|
||||
erase(std::remove_if(begin(), end(), SlotEntry::Zero), end());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -113,9 +113,9 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState {
|
|||
// Small cache of A|b|model indexed by dimension. Can save many mallocs.
|
||||
mutable std::vector<CachedModel> noiseModelCache;
|
||||
CachedModel* getCachedModel(size_t dim) const {
|
||||
if (dim > noiseModelCache.size())
|
||||
noiseModelCache.resize(dim);
|
||||
CachedModel* item = &noiseModelCache[dim - 1];
|
||||
if (dim >= noiseModelCache.size())
|
||||
noiseModelCache.resize(dim+1);
|
||||
CachedModel* item = &noiseModelCache[dim];
|
||||
if (!item->model)
|
||||
*item = CachedModel(dim, 1.0 / std::sqrt(lambda));
|
||||
return item;
|
||||
|
|
|
|||
|
|
@ -121,17 +121,18 @@ class SmartRangeFactor: public NoiseModelFactor {
|
|||
|
||||
// use best fh to find actual intersection points
|
||||
if (bestCircle2 && best_fh) {
|
||||
std::list<Point2> intersections = circleCircleIntersection(
|
||||
circle1.center, bestCircle2->center, best_fh);
|
||||
auto bestCircleCenter = bestCircle2->center;
|
||||
std::list<Point2> intersections =
|
||||
circleCircleIntersection(circle1.center, bestCircleCenter, best_fh);
|
||||
|
||||
// pick winner based on other measurements
|
||||
double error1 = 0, error2 = 0;
|
||||
Point2 p1 = intersections.front(), p2 = intersections.back();
|
||||
for (const Circle2& it : circles) {
|
||||
error1 += distance2(it.center, p1);
|
||||
error2 += distance2(it.center, p2);
|
||||
}
|
||||
return (error1 < error2) ? p1 : p2;
|
||||
// pick winner based on other measurements
|
||||
double error1 = 0, error2 = 0;
|
||||
Point2 p1 = intersections.front(), p2 = intersections.back();
|
||||
for (const Circle2& it : circles) {
|
||||
error1 += distance2(it.center, p1);
|
||||
error2 += distance2(it.center, p2);
|
||||
}
|
||||
return (error1 < error2) ? p1 : p2;
|
||||
} else {
|
||||
throw std::runtime_error("triangulate failed");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -106,7 +106,7 @@ void Method::emit_cython_pyx_no_overload(FileWriter& file,
|
|||
|
||||
// leverage python's special treatment for print
|
||||
if (funcName == "print_") {
|
||||
file.oss << " def __str__(self):\n";
|
||||
file.oss << " def __repr__(self):\n";
|
||||
file.oss << " strBuf = RedirectCout()\n";
|
||||
file.oss << " self.print_('')\n";
|
||||
file.oss << " return strBuf.str()\n";
|
||||
|
|
|
|||
Loading…
Reference in New Issue