12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'develop' into feature/python-plotting

release/4.3a0
Varun Agrawal 2020-03-25 19:06:17 -04:00
parent 81b4765299 0c2890b815
commit a59682e608
53 changed files with 682 additions and 326 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
cython/gtsam/examples/ImuFactorExample2.py
View File

@ -14,6 +14,11 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam
import gtsam.utils.plot as gtsam_plot
from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
PinholeCameraCal3_S2, Point3, Pose3,
PriorFactorConstantBias, PriorFactorPose3,
PriorFactorVector, Rot3, Values)
def X(key):
@ -69,8 +74,8 @@ 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))
DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
Point3(0.05, -0.10, 0.20))
def IMU_example():
@ -78,10 +83,10 @@ def IMU_example():
# 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())
up = Point3(0, 0, 1)
target = Point3(0, 0, 0)
position = Point3(radius, 0, 0)
camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
pose_0 = camera.pose()
# Create the set of ground-truth landmarks and poses
@ -90,37 +95,37 @@ def IMU_example():
angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = gtsam.ConstantTwistScenario(
scenario = ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0)
# Create a factor graph
newgraph = gtsam.NonlinearFactorGraph()
newgraph = NonlinearFactorGraph()
# Create (incremental) ISAM2 solver
isam = gtsam.ISAM2()
isam = ISAM2()
# Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initialEstimate = gtsam.Values()
initialEstimate = 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.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
newgraph.push_back(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)
biasprior = 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)
velprior = PriorFactorVector(V(0), n_velocity, velnoise)
newgraph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity)
@ -141,7 +146,7 @@ def IMU_example():
# Add Bias variables periodically
if i % 5 == 0:
biasKey += 1
factor = gtsam.BetweenFactorConstantBias(
factor = BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
newgraph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
@ -154,8 +159,7 @@ def IMU_example():
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
# Add Imu Factor
imufac = gtsam.ImuFactor(
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac)
# insert new velocity, which is wrong
@ -168,7 +172,7 @@ def IMU_example():
ISAM2_plot(result)
# reset
newgraph = gtsam.NonlinearFactorGraph()
newgraph = NonlinearFactorGraph()
initialEstimate.clear()

2
cython/gtsam/examples/SFMExample.py
View File

@ -79,7 +79,7 @@ def main():
# Simulated measurements from each camera pose, adding them to the factor graph
for i, pose in enumerate(poses):
camera = SimpleCamera(pose, K)
camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(

4
cython/gtsam/examples/VisualISAMExample.py
View File

@ -18,7 +18,7 @@ from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3,
SimpleCamera, Values)
PinholeCameraCal3_S2, Values)
def symbol(name: str, index: int) -> int:
@ -54,7 +54,7 @@ def main():
# Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses):
camera = SimpleCamera(pose, K)
camera = PinholeCameraCal3_S2(pose, K)
# Add factors for each landmark observation
for j, point in enumerate(points):
measurement = camera.project(point)

8
cython/gtsam/tests/test_SimpleCamera.py
View File

@ -5,7 +5,7 @@ All Rights Reserved
See LICENSE for the license information
SimpleCamera unit tests.
PinholeCameraCal3_S2 unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import math
@ -14,7 +14,7 @@ import unittest
import numpy as np
import gtsam
from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera
from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2
from gtsam.utils.test_case import GtsamTestCase
K = Cal3_S2(625, 625, 0, 0, 0)
@ -23,14 +23,14 @@ class TestSimpleCamera(GtsamTestCase):
def test_constructor(self):
pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
camera = SimpleCamera(pose1, K)
camera = PinholeCameraCal3_S2(pose1, K)
self.gtsamAssertEquals(camera.calibration(), K, 1e-9)
self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)
def test_level2(self):
# Create a level camera, looking in Y-direction
pose2 = Pose2(0.4,0.3,math.pi/2.0)
camera = SimpleCamera.Level(K, pose2, 0.1)
camera = PinholeCameraCal3_S2.Level(K, pose2, 0.1)
# expected
x = Point3(1,0,0)

43
cython/gtsam/utils/visual_data_generator.py
View File

@ -1,8 +1,9 @@
from __future__ import print_function
import numpy as np
from math import pi, cos, sin
import gtsam
from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3
class Options:
@ -10,7 +11,7 @@ class Options:
Options to generate test scenario
"""
def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()):
def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()):
"""
Options to generate test scenario
@param triangle: generate a triangle scene with 3 points if True, otherwise
@ -27,10 +28,10 @@ class GroundTruth:
Object holding generated ground-truth data
"""
def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
self.K = K
self.cameras = [gtsam.Pose3()] * nrCameras
self.points = [gtsam.Point3()] * nrPoints
self.cameras = [Pose3()] * nrCameras
self.points = [Point3()] * nrPoints
def print_(self, s=""):
print(s)
@ -52,11 +53,11 @@ class Data:
class NoiseModels:
pass
def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
self.K = K
self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras]
self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras]
self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
self.odometry = [gtsam.Pose3()] * nrCameras
self.odometry = [Pose3()] * nrCameras
# Set Noise parameters
self.noiseModels = Data.NoiseModels()
@ -73,7 +74,7 @@ class Data:
def generate_data(options):
""" Generate ground-truth and measurement data. """
K = gtsam.Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
nrPoints = 3 if options.triangle else 8
truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints)
@ -83,26 +84,26 @@ def generate_data(options):
if options.triangle: # Create a triangle target, just 3 points on a plane
r = 10
for j in range(len(truth.points)):
theta = j * 2 * pi / nrPoints
truth.points[j] = gtsam.Point3(r * cos(theta), r * sin(theta), 0)
theta = j * 2 * np.pi / nrPoints
truth.points[j] = Point3(r * np.cos(theta), r * np.sin(theta), 0)
else: # 3D landmarks as vertices of a cube
truth.points = [
gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10),
gtsam.Point3(-10, -10, 10), gtsam.Point3(10, -10, 10),
gtsam.Point3(10, 10, -10), gtsam.Point3(-10, 10, -10),
gtsam.Point3(-10, -10, -10), gtsam.Point3(10, -10, -10)
Point3(10, 10, 10), Point3(-10, 10, 10),
Point3(-10, -10, 10), Point3(10, -10, 10),
Point3(10, 10, -10), Point3(-10, 10, -10),
Point3(-10, -10, -10), Point3(10, -10, -10)
]
# Create camera cameras on a circle around the triangle
height = 10
r = 40
for i in range(options.nrCameras):
theta = i * 2 * pi / options.nrCameras
t = gtsam.Point3(r * cos(theta), r * sin(theta), height)
truth.cameras[i] = gtsam.SimpleCamera.Lookat(t,
gtsam.Point3(),
gtsam.Point3(0, 0, 1),
truth.K)
theta = i * 2 * np.pi / options.nrCameras
t = Point3(r * np.cos(theta), r * np.sin(theta), height)
truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
Point3(),
Point3(0, 0, 1),
truth.K)
# Create measurements
for j in range(nrPoints):
# All landmarks seen in every frame

128
cython/gtsam_unstable/examples/TimeOfArrivalExample.py Normal file
View File

@ -0,0 +1,128 @@
"""
GTSAM Copyright 2010-2020, 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
Track a moving object "Time of Arrival" measurements at 4 microphones.
Author: Frank Dellaert
"""
# pylint: disable=invalid-name, no-name-in-module
from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
NonlinearFactorGraph, Point3, Values, noiseModel_Isotropic)
from gtsam_unstable import Event, TimeOfArrival, TOAFactor
# units
MS = 1e-3
CM = 1e-2
# Instantiate functor with speed of sound value
TIME_OF_ARRIVAL = TimeOfArrival(330)
def define_microphones():
"""Create microphones."""
height = 0.5
microphones = []
microphones.append(Point3(0, 0, height))
microphones.append(Point3(403 * CM, 0, height))
microphones.append(Point3(403 * CM, 403 * CM, height))
microphones.append(Point3(0, 403 * CM, 2 * height))
return microphones
def create_trajectory(n):
"""Create ground truth trajectory."""
trajectory = []
timeOfEvent = 10
# simulate emitting a sound every second while moving on straight line
for key in range(n):
trajectory.append(
Event(timeOfEvent, 245 * CM + key * 1.0, 201.5 * CM, (212 - 45) * CM))
timeOfEvent += 1
return trajectory
def simulate_one_toa(microphones, event):
"""Simulate time-of-arrival measurements for a single event."""
return [TIME_OF_ARRIVAL.measure(event, microphones[i])
for i in range(len(microphones))]
def simulate_toa(microphones, trajectory):
"""Simulate time-of-arrival measurements for an entire trajectory."""
return [simulate_one_toa(microphones, event)
for event in trajectory]
def create_graph(microphones, simulatedTOA):
"""Create factor graph."""
graph = NonlinearFactorGraph()
# Create a noise model for the TOA error
model = noiseModel_Isotropic.Sigma(1, 0.5 * MS)
K = len(microphones)
key = 0
for toa in simulatedTOA:
for i in range(K):
factor = TOAFactor(key, microphones[i], toa[i], model)
graph.push_back(factor)
key += 1
return graph
def create_initial_estimate(n):
"""Create initial estimate for n events."""
initial = Values()
zero = Event()
for key in range(n):
TOAFactor.InsertEvent(key, zero, initial)
return initial
def toa_example():
"""Run example with 4 microphones and 5 events in a straight line."""
# Create microphones
microphones = define_microphones()
K = len(microphones)
for i in range(K):
print("mic {} = {}".format(i, microphones[i]))
# Create a ground truth trajectory
n = 5
groundTruth = create_trajectory(n)
for event in groundTruth:
print(event)
# Simulate time-of-arrival measurements
simulatedTOA = simulate_toa(microphones, groundTruth)
for key in range(n):
for i in range(K):
print("z_{}{} = {} ms".format(key, i, simulatedTOA[key][i] / MS))
# create factor graph
graph = create_graph(microphones, simulatedTOA)
print(graph.at(0))
# Create initial estimate
initial_estimate = create_initial_estimate(n)
print(initial_estimate)
# Optimize using Levenberg-Marquardt optimization.
params = LevenbergMarquardtParams()
params.setAbsoluteErrorTol(1e-10)
params.setVerbosityLM("SUMMARY")
optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params)
result = optimizer.optimize()
print("Final Result:\n", result)
if __name__ == '__main__':
toa_example()
print("Example complete")

5
examples/CameraResectioning.cpp
View File

@ -18,7 +18,8 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <boost/make_shared.hpp>
using namespace gtsam;
@ -47,7 +48,7 @@ public:
/// evaluate the error
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const {
SimpleCamera camera(pose, *K_);
PinholeCamera<Cal3_S2> camera(pose, *K_);
return camera.project(P_, H, boost::none, boost::none) - p_;
}
};

3
examples/ISAM2Example_SmartFactor.cpp
View File

@ -4,7 +4,8 @@
* @author Alexander (pumaking on BitBucket)
*/
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>

5
examples/ImuFactorExample2.cpp
View File

@ -1,5 +1,6 @@
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/ImuFactor.h>
@ -34,7 +35,7 @@ int main(int argc, char* argv[]) {
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 camera = PinholeCamera<Cal3_S2>::Lookat(position, target, up);
const auto pose_0 = camera.pose();
// Now, create a constant-twist scenario that makes the camera orbit the

2
examples/SFMExample.cpp
View File

@ -79,7 +79,7 @@ int main(int argc, char* argv[]) {
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
SimpleCamera camera(poses[i], *K);
PinholeCamera<Cal3_S2> camera(poses[i], *K);
for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);

4
examples/SFMExampleExpressions.cpp
View File

@ -27,7 +27,7 @@
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// Header order is close to far
#include <examples/SFMdata.h>
#include "SFMdata.h"
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/Values.h>
@ -67,7 +67,7 @@ int main(int argc, char* argv[]) {
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
Pose3_ x('x', i);
SimpleCamera camera(poses[i], K);
PinholeCamera<Cal3_S2> camera(poses[i], K);
for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]);
// Below an expression for the prediction of the measurement:

5
examples/SFMdata.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file SFMMdata.h
* @file SFMdata.h
* @brief Simple example for the structure-from-motion problems
* @author Duy-Nguyen Ta
*/
@ -30,7 +30,8 @@
#include <gtsam/geometry/Point3.h>
// We will also need a camera object to hold calibration information and perform projections.
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
/* ************************************************************************* */
std::vector<gtsam::Point3> createPoints() {

2
examples/SelfCalibrationExample.cpp
View File

@ -61,7 +61,7 @@ int main(int argc, char* argv[]) {
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
for (size_t i = 0; i < poses.size(); ++i) {
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], K);
PinholeCamera<Cal3_S2> camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// The only real difference with the Visual SLAM example is that here we use a
// different factor type, that also calculates the Jacobian with respect to calibration

2
examples/VisualISAM2Example.cpp
View File

@ -89,7 +89,7 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < poses.size(); ++i) {
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
PinholeCamera<Cal3_S2> camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(
measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);

2
examples/VisualISAMExample.cpp
View File

@ -86,7 +86,7 @@ int main(int argc, char* argv[]) {
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) {
// Create ground truth measurement
SimpleCamera camera(poses[i], *K);
PinholeCamera<Cal3_S2> camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
// Add measurement
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, noise,

18
gtsam.h
View File

@ -281,7 +281,7 @@ virtual class Value {
};
#include <gtsam/base/GenericValue.h>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value {
void serializable() const;
};
@ -2138,7 +2138,7 @@ class Values {
void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
void insert(size_t j, const gtsam::SimpleCamera& simpel_camera);
void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void insert(size_t j, Vector vector);
void insert(size_t j, Matrix matrix);
@ -2459,7 +2459,7 @@ class ISAM2 {
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
gtsam::SimpleCamera, Vector, Matrix}>
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, Vector, Matrix}>
VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const;
Matrix marginalCovariance(size_t key) const;
@ -2497,7 +2497,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
@ -2520,7 +2520,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
#include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation
NonlinearEquality(size_t j, const T& feasible);
@ -2546,9 +2546,9 @@ typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
typedef gtsam::RangeFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> RangeFactorSimpleCameraPoint;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
typedef gtsam::RangeFactor<gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3_S2> RangeFactorSimpleCamera;
#include <gtsam/sam/RangeFactor.h>
@ -2639,7 +2639,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey);
gtsam::Point2 measured() const;
};
typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
// due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
@ -3153,7 +3153,7 @@ namespace utilities {
void perturbPoint2(gtsam::Values& values, double sigma, int seed);
void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed);
void perturbPoint3(gtsam::Values& values, double sigma, int seed);
void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCameraCal3_S2& c, Vector J, Matrix Z, double depth);
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);

4
gtsam/geometry/BearingRange.h
View File

@ -187,8 +187,8 @@ struct HasBearing {
};
// Similar helper class for to implement Range traits for classes with a range method
// For classes with overloaded range methods, such as SimpleCamera, this can even be templated:
// template <typename T> struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
// For classes with overloaded range methods, such as PinholeCamera, this can even be templated:
// template <typename T> struct Range<PinholeCamera, T> : HasRange<PinholeCamera, T, double> {};
template <class A1, typename A2, class RT>
struct HasRange {
typedef RT result_type;

26
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -17,7 +17,7 @@
*/
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Cal3Bundler.h>
@ -151,7 +151,7 @@ TEST( triangulation, fourPoses) {
// 3. Add a slightly rotated third camera above, again with measurement noise
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
SimpleCamera camera3(pose3, *sharedCal);
PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
Point2 z3 = camera3.project(landmark);
poses += pose3;
@ -168,7 +168,7 @@ TEST( triangulation, fourPoses) {
// 4. Test failure: Add a 4th camera facing the wrong way
Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
SimpleCamera camera4(pose4, *sharedCal);
PinholeCamera<Cal3_S2> camera4(pose4, *sharedCal);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
@ -185,17 +185,17 @@ TEST( triangulation, fourPoses) {
TEST( triangulation, fourPoses_distinct_Ks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
SimpleCamera camera1(pose1, K1);
PinholeCamera<Cal3_S2> camera1(pose1, K1);
// create second camera 1 meter to the right of first camera
Cal3_S2 K2(1600, 1300, 0, 650, 440);
SimpleCamera camera2(pose2, K2);
PinholeCamera<Cal3_S2> camera2(pose2, K2);
// 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark);
CameraSet<SimpleCamera> cameras;
CameraSet<PinholeCamera<Cal3_S2> > cameras;
Point2Vector measurements;
cameras += camera1, camera2;
@ -216,7 +216,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
// 3. Add a slightly rotated third camera above, again with measurement noise
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
Cal3_S2 K3(700, 500, 0, 640, 480);
SimpleCamera camera3(pose3, K3);
PinholeCamera<Cal3_S2> camera3(pose3, K3);
Point2 z3 = camera3.project(landmark);
cameras += camera3;
@ -234,7 +234,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
// 4. Test failure: Add a 4th camera facing the wrong way
Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Cal3_S2 K4(700, 500, 0, 640, 480);
SimpleCamera camera4(pose4, K4);
PinholeCamera<Cal3_S2> camera4(pose4, K4);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
@ -250,17 +250,17 @@ TEST( triangulation, fourPoses_distinct_Ks) {
TEST( triangulation, outliersAndFarLandmarks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
SimpleCamera camera1(pose1, K1);
PinholeCamera<Cal3_S2> camera1(pose1, K1);
// create second camera 1 meter to the right of first camera
Cal3_S2 K2(1600, 1300, 0, 650, 440);
SimpleCamera camera2(pose2, K2);
PinholeCamera<Cal3_S2> camera2(pose2, K2);
// 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark);
CameraSet<SimpleCamera> cameras;
CameraSet<PinholeCamera<Cal3_S2> > cameras;
Point2Vector measurements;
cameras += camera1, camera2;
@ -280,7 +280,7 @@ TEST( triangulation, outliersAndFarLandmarks) {
// 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER)
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
Cal3_S2 K3(700, 500, 0, 640, 480);
SimpleCamera camera3(pose3, K3);
PinholeCamera<Cal3_S2> camera3(pose3, K3);
Point2 z3 = camera3.project(landmark);
cameras += camera3;
@ -302,7 +302,7 @@ TEST( triangulation, outliersAndFarLandmarks) {
//******************************************************************************
TEST( triangulation, twoIdenticalPoses) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
SimpleCamera camera1(pose1, *sharedCal);
PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
// 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark);

2
gtsam/nonlinear/Expression.h
View File

@ -66,7 +66,7 @@ public:
// Expressions wrap trees of functions that can evaluate their own derivatives.
// The meta-functions below are useful to specify the type of those functions.
// Example, a function taking a camera and a 3D point and yielding a 2D point:
// Expression<Point2>::BinaryFunction<SimpleCamera,Point3>::type
// Expression<Point2>::BinaryFunction<PinholeCamera<Cal3_S2>,Point3>::type
template<class A1>
struct UnaryFunction {
typedef boost::function<

4
gtsam/nonlinear/utilities.h
View File

@ -28,7 +28,7 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <exception>
@ -169,7 +169,7 @@ void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
}
/// Insert a number of initial point values by backprojecting
void insertBackprojections(Values& values, const SimpleCamera& camera,
void insertBackprojections(Values& values, const PinholeCamera<Cal3_S2>& camera,
const Vector& J, const Matrix& Z, double depth) {
if (Z.rows() != 2)
throw std::invalid_argument("insertBackProjections: Z must be 2*K");

13
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -19,7 +19,8 @@
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/base/TestableAssertions.h>
@ -353,11 +354,13 @@ TEST(RangeFactor, Point3) {
}
/* ************************************************************************* */
// Do tests with SimpleCamera
// Do tests with PinholeCamera<Cal3_S2>
TEST( RangeFactor, Camera) {
RangeFactor<SimpleCamera,Point3> factor1(poseKey, pointKey, measurement, model);
RangeFactor<SimpleCamera,Pose3> factor2(poseKey, pointKey, measurement, model);
RangeFactor<SimpleCamera,SimpleCamera> factor3(poseKey, pointKey, measurement, model);
using Camera = PinholeCamera<Cal3_S2>;
RangeFactor<Camera, Point3> factor1(poseKey, pointKey, measurement, model);
RangeFactor<Camera, Pose3> factor2(poseKey, pointKey, measurement, model);
RangeFactor<Camera, Camera> factor3(poseKey, pointKey, measurement, model);
}
/* ************************************************************************* */

2
gtsam/slam/EssentialMatrixFactor.h
View File

@ -9,7 +9,7 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <iostream>
namespace gtsam {

3
gtsam/slam/ProjectionFactor.h
View File

@ -21,7 +21,8 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <boost/optional.hpp>
namespace gtsam {

1
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -13,6 +13,7 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>

6
gtsam/slam/tests/testTriangulationFactor.cpp
View File

@ -17,7 +17,7 @@
*/
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/nonlinear/Expression.h>
@ -39,7 +39,7 @@ static const boost::shared_ptr<Cal3_S2> sharedCal = //
// Looking along X-axis, 1 meter above ground plane (x-y)
static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
SimpleCamera camera1(pose1, *sharedCal);
PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
// landmark ~5 meters infront of camera
static const Point3 landmark(5, 0.5, 1.2);
@ -52,7 +52,7 @@ TEST( triangulation, TriangulationFactor ) {
Key pointKey(1);
SharedNoiseModel model;
typedef TriangulationFactor<SimpleCamera> Factor;
typedef TriangulationFactor<PinholeCamera<Cal3_S2> > Factor;
Factor factor(camera1, z1, model, pointKey);
// Use the factor to calculate the Jacobians

160
gtsam_unstable/examples/TimeOfArrivalExample.cpp Normal file
View File

@ -0,0 +1,160 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2020, 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
* -------------------------------------------------------------------------- */
/**
* @file TimeOfArrivalExample.cpp
* @brief Track a moving object "Time of Arrival" measurements at 4
* microphones.
* @author Frank Dellaert
* @author Jay Chakravarty
* @date March 2020
*/
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam_unstable/geometry/Event.h>
#include <gtsam_unstable/slam/TOAFactor.h>
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <vector>
using namespace std;
using namespace gtsam;
// units
static const double ms = 1e-3;
static const double cm = 1e-2;
// Instantiate functor with speed of sound value
static const TimeOfArrival kTimeOfArrival(330);
/* ************************************************************************* */
// Create microphones
vector<Point3> defineMicrophones() {
const double height = 0.5;
vector<Point3> microphones;
microphones.push_back(Point3(0, 0, height));
microphones.push_back(Point3(403 * cm, 0, height));
microphones.push_back(Point3(403 * cm, 403 * cm, height));
microphones.push_back(Point3(0, 403 * cm, 2 * height));
return microphones;
}
/* ************************************************************************* */
// Create ground truth trajectory
vector<Event> createTrajectory(int n) {
vector<Event> trajectory;
double timeOfEvent = 10;
// simulate emitting a sound every second while moving on straight line
for (size_t key = 0; key < n; key++) {
trajectory.push_back(
Event(timeOfEvent, 245 * cm + key * 1.0, 201.5 * cm, (212 - 45) * cm));
timeOfEvent += 1;
}
return trajectory;
}
/* ************************************************************************* */
// Simulate time-of-arrival measurements for a single event
vector<double> simulateTOA(const vector<Point3>& microphones,
const Event& event) {
size_t K = microphones.size();
vector<double> simulatedTOA(K);
for (size_t i = 0; i < K; i++) {
simulatedTOA[i] = kTimeOfArrival(event, microphones[i]);
}
return simulatedTOA;
}
/* ************************************************************************* */
// Simulate time-of-arrival measurements for an entire trajectory
vector<vector<double>> simulateTOA(const vector<Point3>& microphones,
const vector<Event>& trajectory) {
vector<vector<double>> simulatedTOA;
for (auto event : trajectory) {
simulatedTOA.push_back(simulateTOA(microphones, event));
}
return simulatedTOA;
}
/* ************************************************************************* */
// create factor graph
NonlinearFactorGraph createGraph(const vector<Point3>& microphones,
const vector<vector<double>>& simulatedTOA) {
NonlinearFactorGraph graph;
// Create a noise model for the TOA error
auto model = noiseModel::Isotropic::Sigma(1, 0.5 * ms);
size_t K = microphones.size();
size_t key = 0;
for (auto toa : simulatedTOA) {
for (size_t i = 0; i < K; i++) {
graph.emplace_shared<TOAFactor>(key, microphones[i], toa[i], model);
}
key += 1;
}
return graph;
}
/* ************************************************************************* */
// create initial estimate for n events
Values createInitialEstimate(int n) {
Values initial;
Event zero;
for (size_t key = 0; key < n; key++) {
initial.insert(key, zero);
}
return initial;
}
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Create microphones
auto microphones = defineMicrophones();
size_t K = microphones.size();
for (size_t i = 0; i < K; i++) {
cout << "mic" << i << " = " << microphones[i] << endl;
}
// Create a ground truth trajectory
const size_t n = 5;
auto groundTruth = createTrajectory(n);
// Simulate time-of-arrival measurements
auto simulatedTOA = simulateTOA(microphones, groundTruth);
for (size_t key = 0; key < n; key++) {
for (size_t i = 0; i < K; i++) {
cout << "z_" << key << i << " = " << simulatedTOA[key][i] / ms << " ms"
<< endl;
}
}
// Create factor graph
auto graph = createGraph(microphones, simulatedTOA);
// Create initial estimate
auto initialEstimate = createInitialEstimate(n);
initialEstimate.print("Initial Estimate:\n");
// Optimize using Levenberg-Marquardt optimization.
LevenbergMarquardtParams params;
params.setAbsoluteErrorTol(1e-10);
params.setVerbosityLM("SUMMARY");
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
Values result = optimizer.optimize();
result.print("Final Result:\n");
}
/* ************************************************************************* */

9
gtsam_unstable/geometry/Event.cpp
View File

@ -24,15 +24,16 @@ namespace gtsam {
/* ************************************************************************* */
void Event::print(const std::string& s) const {
std::cout << s << "time = " << time_ << "location = " << location_.transpose();
std::cout << s << "{'time':" << time_
<< ", 'location': " << location_.transpose() << "}";
}
/* ************************************************************************* */
bool Event::equals(const Event& other, double tol) const {
return std::abs(time_ - other.time_) < tol
&& traits<Point3>::Equals(location_, other.location_, tol);
return std::abs(time_ - other.time_) < tol &&
traits<Point3>::Equals(location_, other.location_, tol);
}
/* ************************************************************************* */
} //\ namespace gtsam
} // namespace gtsam

98
gtsam_unstable/geometry/Event.h
View File

@ -20,42 +20,43 @@
#pragma once
#include <gtsam/geometry/Point3.h>
#include <gtsam_unstable/dllexport.h>
#include <cmath>
#include <iosfwd>
#include <gtsam_unstable/dllexport.h>
#include <string>
namespace gtsam {
/// A space-time event
/**
* A space-time event models an event that happens at a certain 3D location, at
* a certain time. One use for it is in sound-based or UWB-ranging tracking or
* SLAM, where we have "time of arrival" measurements at a set of sensors. The
* TOA functor below provides a measurement function for those applications.
*/
class Event {
double time_; ///< Time event was generated
Point3 location_; ///< Location at time event was generated
double time_; ///< Time event was generated
Point3 location_; ///< Location at time event was generated
public:
public:
enum { dimension = 4 };
/// Default Constructor
Event() :
time_(0), location_(0,0,0) {
}
Event() : time_(0), location_(0, 0, 0) {}
/// Constructor from time and location
Event(double t, const Point3& p) :
time_(t), location_(p) {
}
Event(double t, const Point3& p) : time_(t), location_(p) {}
/// Constructor with doubles
Event(double t, double x, double y, double z) :
time_(t), location_(x, y, z) {
}
Event(double t, double x, double y, double z)
: time_(t), location_(x, y, z) {}
double time() const { return time_;}
Point3 location() const { return location_;}
double time() const { return time_; }
Point3 location() const { return location_; }
// TODO we really have to think of a better way to do linear arguments
double height(OptionalJacobian<1,4> H = boost::none) const {
static const Matrix14 JacobianZ = (Matrix14() << 0,0,0,1).finished();
// TODO(frank) we really have to think of a better way to do linear arguments
double height(OptionalJacobian<1, 4> H = boost::none) const {
static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished();
if (H) *H = JacobianZ;
return location_.z();
}
@ -64,7 +65,8 @@ public:
GTSAM_UNSTABLE_EXPORT void print(const std::string& s = "") const;
/** equals with an tolerance */
GTSAM_UNSTABLE_EXPORT bool equals(const Event& other, double tol = 1e-9) const;
GTSAM_UNSTABLE_EXPORT bool equals(const Event& other,
double tol = 1e-9) const;
/// Updates a with tangent space delta
inline Event retract(const Vector4& v) const {
@ -73,28 +75,44 @@ public:
/// Returns inverse retraction
inline Vector4 localCoordinates(const Event& q) const {
return Vector4::Zero(); // TODO
}
/// Time of arrival to given microphone
double toa(const Point3& microphone, //
OptionalJacobian<1, 4> H1 = boost::none, //
OptionalJacobian<1, 3> H2 = boost::none) const {
static const double Speed = 330;
Matrix13 D1, D2;
double distance = gtsam::distance3(location_, microphone, D1, D2);
if (H1)
// derivative of toa with respect to event
*H1 << 1.0, D1 / Speed;
if (H2)
// derivative of toa with respect to microphone location
*H2 << D2 / Speed;
return time_ + distance / Speed;
return Vector4::Zero(); // TODO(frank) implement!
}
};
// Define GTSAM traits
template<>
template <>
struct traits<Event> : internal::Manifold<Event> {};
} //\ namespace gtsam
/// Time of arrival to given sensor
class TimeOfArrival {
const double speed_; ///< signal speed
public:
typedef double result_type;
/// Constructor with optional speed of signal, in m/sec
explicit TimeOfArrival(double speed = 330) : speed_(speed) {}
/// Calculate time of arrival
double measure(const Event& event, const Point3& sensor) const {
double distance = gtsam::distance3(event.location(), sensor);
return event.time() + distance / speed_;
}
/// Calculate time of arrival, with derivatives
double operator()(const Event& event, const Point3& sensor, //
OptionalJacobian<1, 4> H1 = boost::none, //
OptionalJacobian<1, 3> H2 = boost::none) const {
Matrix13 D1, D2;
double distance = gtsam::distance3(event.location(), sensor, D1, D2);
if (H1)
// derivative of toa with respect to event
*H1 << 1.0, D1 / speed_;
if (H2)
// derivative of toa with respect to sensor location
*H2 << D2 / speed_;
return event.time() + distance / speed_;
}
};
} // namespace gtsam

42
gtsam_unstable/geometry/tests/testEvent.cpp
View File

@ -17,10 +17,12 @@
* @date December 2014
*/
#include <gtsam_unstable/geometry/Event.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/Expression.h>
#include <gtsam_unstable/geometry/Event.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std;
@ -30,56 +32,59 @@ using namespace gtsam;
static const double ms = 1e-3;
static const double cm = 1e-2;
typedef Eigen::Matrix<double, 1, 1> Vector1;
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms));
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 microphoneAt0(0,0,0);
static const Point3 microphoneAt0(0, 0, 0);
static const double kSpeedOfSound = 340;
static const TimeOfArrival kToa(kSpeedOfSound);
//*****************************************************************************
TEST( Event, Constructor ) {
TEST(Event, Constructor) {
const double t = 0;
Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm);
}
//*****************************************************************************
TEST( Event, Toa1 ) {
TEST(Event, Toa1) {
Event event(0, 1, 0, 0);
double expected = 1. / 330;
EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9);
double expected = 1. / kSpeedOfSound;
EXPECT_DOUBLES_EQUAL(expected, kToa(event, microphoneAt0), 1e-9);
}
//*****************************************************************************
TEST( Event, Toa2 ) {
double expectedTOA = timeOfEvent + 1. / 330;
EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9);
TEST(Event, Toa2) {
double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
EXPECT_DOUBLES_EQUAL(expectedTOA, kToa(exampleEvent, microphoneAt0), 1e-9);
}
//*************************************************************************
TEST (Event, Derivatives) {
TEST(Event, Derivatives) {
Matrix14 actualH1;
Matrix13 actualH2;
exampleEvent.toa(microphoneAt0, actualH1, actualH2);
kToa(exampleEvent, microphoneAt0, actualH1, actualH2);
Matrix expectedH1 = numericalDerivative11<double, Event>(
boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none),
boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none),
exampleEvent);
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
Matrix expectedH2 = numericalDerivative11<double, Point3>(
boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none),
boost::bind(kToa, exampleEvent, _1, boost::none, boost::none),
microphoneAt0);
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
}
//*****************************************************************************
TEST( Event, Expression ) {
TEST(Event, Expression) {
Key key = 12;
Expression<Event> event_(key);
Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
Expression<double> expression(&Event::toa, event_, knownMicrophone_);
Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
Expression<double> expression(kToa, event_, knownMicrophone_);
Values values;
values.insert(key, exampleEvent);
double expectedTOA = timeOfEvent + 1. / 330;
double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9);
}
@ -97,4 +102,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
//*****************************************************************************

5
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
View File

@ -9,7 +9,8 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam_unstable/geometry/InvDepthCamera3.h>
@ -18,7 +19,7 @@ using namespace gtsam;
static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, *K);
PinholeCamera<Cal3_S2> level_camera(level_pose, *K);
/* ************************************************************************* */
TEST( InvDepthFactor, Project1) {

24
gtsam_unstable/gtsam_unstable.h
View File

@ -377,6 +377,30 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> RangeFactorRTV;
#include <gtsam_unstable/geometry/Event.h>
class Event {
Event();
Event(double t, const gtsam::Point3& p);
Event(double t, double x, double y, double z);
double time() const;
gtsam::Point3 location() const;
double height() const;
void print(string s) const;
};
class TimeOfArrival {
TimeOfArrival();
TimeOfArrival(double speed);
double measure(const gtsam::Event& event, const gtsam::Point3& sensor) const;
};
#include <gtsam_unstable/slam/TOAFactor.h>
virtual class TOAFactor : gtsam::NonlinearFactor {
// For now, because of overload issues, we only expose constructor with known sensor coordinates:
TOAFactor(size_t key1, gtsam::Point3 sensor, double measured,
const gtsam::noiseModel::Base* noiseModel);
static void InsertEvent(size_t key, const gtsam::Event& event, gtsam::Values* values);
};
#include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::PoseRTV}>

2
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -21,7 +21,7 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <boost/optional.hpp>
namespace gtsam {

48
gtsam_unstable/slam/TOAFactor.h
View File

@ -17,33 +17,51 @@
* @date December 2014
*/
#pragma once
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam_unstable/geometry/Event.h>
namespace gtsam {
/// A "Time of Arrival" factor - so little code seems hardly worth it :-)
class TOAFactor: public ExpressionFactor<double> {
class TOAFactor : public ExpressionFactor<double> {
typedef Expression<double> Double_;
public:
public:
/**
* Constructor
* @param some expression yielding an event
* @param microphone_ expression yielding a microphone location
* @param toaMeasurement time of arrival at microphone
* Most general constructor with two expressions
* @param eventExpression expression yielding an event
* @param sensorExpression expression yielding a sensor location
* @param toaMeasurement time of arrival at sensor
* @param model noise model
* @param speed optional speed of signal, in m/sec
*/
TOAFactor(const Expression<Event>& eventExpression,
const Expression<Point3>& microphone_, double toaMeasurement,
const SharedNoiseModel& model) :
ExpressionFactor<double>(model, toaMeasurement,
Double_(&Event::toa, eventExpression, microphone_)) {
}
const Expression<Point3>& sensorExpression, double toaMeasurement,
const SharedNoiseModel& model, double speed = 330)
: ExpressionFactor<double>(
model, toaMeasurement,
Double_(TimeOfArrival(speed), eventExpression, sensorExpression)) {}
/**
* Constructor with fixed sensor
* @param eventExpression expression yielding an event
* @param sensor a known sensor location
* @param toaMeasurement time of arrival at sensor
* @param model noise model
* @param toa optional time of arrival functor
*/
TOAFactor(const Expression<Event>& eventExpression, const Point3& sensor,
double toaMeasurement, const SharedNoiseModel& model,
double speed = 330)
: TOAFactor(eventExpression, Expression<Point3>(sensor), toaMeasurement,
model, speed) {}
static void InsertEvent(Key key, const Event& event,
boost::shared_ptr<Values> values) {
values->insert(key, event);
}
};
} //\ namespace gtsam
} // namespace gtsam

79
gtsam_unstable/slam/serialization.cpp
View File

@ -31,20 +31,21 @@
using namespace gtsam;
// Creating as many permutations of factors as possible
typedef PriorFactor<LieVector> PriorFactorLieVector;
typedef PriorFactor<LieMatrix> PriorFactorLieMatrix;
typedef PriorFactor<Point2> PriorFactorPoint2;
typedef PriorFactor<StereoPoint2> PriorFactorStereoPoint2;
typedef PriorFactor<Point3> PriorFactorPoint3;
typedef PriorFactor<Rot2> PriorFactorRot2;
typedef PriorFactor<Rot3> PriorFactorRot3;
typedef PriorFactor<Pose2> PriorFactorPose2;
typedef PriorFactor<Pose3> PriorFactorPose3;
typedef PriorFactor<Cal3_S2> PriorFactorCal3_S2;
typedef PriorFactor<Cal3DS2> PriorFactorCal3DS2;
typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera;
typedef PriorFactor<SimpleCamera> PriorFactorSimpleCamera;
typedef PriorFactor<StereoCamera> PriorFactorStereoCamera;
typedef PriorFactor<LieVector> PriorFactorLieVector;
typedef PriorFactor<LieMatrix> PriorFactorLieMatrix;
typedef PriorFactor<Point2> PriorFactorPoint2;
typedef PriorFactor<StereoPoint2> PriorFactorStereoPoint2;
typedef PriorFactor<Point3> PriorFactorPoint3;
typedef PriorFactor<Rot2> PriorFactorRot2;
typedef PriorFactor<Rot3> PriorFactorRot3;
typedef PriorFactor<Pose2> PriorFactorPose2;
typedef PriorFactor<Pose3> PriorFactorPose3;
typedef PriorFactor<Cal3_S2> PriorFactorCal3_S2;
typedef PriorFactor<Cal3DS2> PriorFactorCal3DS2;
typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera;
typedef PriorFactor<SimpleCamera> PriorFactorSimpleCamera;
typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
typedef PriorFactor<StereoCamera> PriorFactorStereoCamera;
typedef BetweenFactor<LieVector> BetweenFactorLieVector;
typedef BetweenFactor<LieMatrix> BetweenFactorLieMatrix;
@ -55,29 +56,32 @@ typedef BetweenFactor<Rot3> BetweenFactorRot3;
typedef BetweenFactor<Pose2> BetweenFactorPose2;
typedef BetweenFactor<Pose3> BetweenFactorPose3;
typedef NonlinearEquality<LieVector> NonlinearEqualityLieVector;
typedef NonlinearEquality<LieMatrix> NonlinearEqualityLieMatrix;
typedef NonlinearEquality<Point2> NonlinearEqualityPoint2;
typedef NonlinearEquality<StereoPoint2> NonlinearEqualityStereoPoint2;
typedef NonlinearEquality<Point3> NonlinearEqualityPoint3;
typedef NonlinearEquality<Rot2> NonlinearEqualityRot2;
typedef NonlinearEquality<Rot3> NonlinearEqualityRot3;
typedef NonlinearEquality<Pose2> NonlinearEqualityPose2;
typedef NonlinearEquality<Pose3> NonlinearEqualityPose3;
typedef NonlinearEquality<Cal3_S2> NonlinearEqualityCal3_S2;
typedef NonlinearEquality<Cal3DS2> NonlinearEqualityCal3DS2;
typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera;
typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera;
typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera;
typedef NonlinearEquality<LieVector> NonlinearEqualityLieVector;
typedef NonlinearEquality<LieMatrix> NonlinearEqualityLieMatrix;
typedef NonlinearEquality<Point2> NonlinearEqualityPoint2;
typedef NonlinearEquality<StereoPoint2> NonlinearEqualityStereoPoint2;
typedef NonlinearEquality<Point3> NonlinearEqualityPoint3;
typedef NonlinearEquality<Rot2> NonlinearEqualityRot2;
typedef NonlinearEquality<Rot3> NonlinearEqualityRot3;
typedef NonlinearEquality<Pose2> NonlinearEqualityPose2;
typedef NonlinearEquality<Pose3> NonlinearEqualityPose3;
typedef NonlinearEquality<Cal3_S2> NonlinearEqualityCal3_S2;
typedef NonlinearEquality<Cal3DS2> NonlinearEqualityCal3DS2;
typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera;
typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera;
typedef NonlinearEquality<PinholeCameraCal3_S2> NonlinearEqualityPinholeCameraCal3_S2;
typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera;
typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D;
typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint;
typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraPoint;
typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D;
typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint;
typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraPoint;
typedef RangeFactor<PinholeCameraCal3_S2, Point3> RangeFactorPinholeCameraCal3_S2Point;
typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2;
typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
@ -85,7 +89,7 @@ typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorCal3_S2;
typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
@ -126,6 +130,7 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3DS2);
GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo);
GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera);
GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2);
GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
/* Create GUIDs for factors */

6
gtsam_unstable/slam/tests/testInvDepthFactor3.cpp
View File

@ -7,7 +7,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -23,7 +23,7 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
// camera pose at (0,0,1) looking straight along the x-axis.
Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, *K);
PinholeCamera<Cal3_S2> level_camera(level_pose, *K);
typedef InvDepthFactor3<Pose3, Vector5, double> InverseDepthFactor;
typedef NonlinearEquality<Pose3> PoseConstraint;
@ -64,7 +64,7 @@ TEST( InvDepthFactor, optimize) {
// add a camera 2 meters to the right
Pose3 right_pose = level_pose * Pose3(Rot3(), Point3(2,0,0));
SimpleCamera right_camera(right_pose, *K);
PinholeCamera<Cal3_S2> right_camera(right_pose, *K);
// projection measurement of landmark into right camera
// this measurement disagrees with the depth initialization

6
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -523,9 +523,9 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
SimpleCamera cam1(cameraPose1, *K); // with camera poses
SimpleCamera cam2(cameraPose2, *K);
SimpleCamera cam3(cameraPose3, *K);
PinholeCamera<Cal3_S2> cam1(cameraPose1, *K); // with camera poses
PinholeCamera<Cal3_S2> cam2(cameraPose2, *K);
PinholeCamera<Cal3_S2> cam3(cameraPose3, *K);
// create arbitrary body_Pose_sensor (transforms from sensor to body)
Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //

72
gtsam_unstable/slam/tests/testTOAFactor.cpp
View File

@ -17,15 +17,16 @@
* @date December 2014
*/
#include <gtsam_unstable/geometry/Event.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam_unstable/geometry/Event.h>
#include <gtsam_unstable/slam/TOAFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/format.hpp>
#include <boost/bind.hpp>
#include <boost/format.hpp>
using namespace std;
using namespace gtsam;
@ -43,83 +44,59 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 microphoneAt0(0,0,0);
static const Point3 sensorAt0(0, 0, 0);
//*****************************************************************************
TEST( TOAFactor, NewWay ) {
TEST(TOAFactor, NewWay) {
Key key = 12;
Event_ eventExpression(key);
Point3_ microphoneConstant(microphoneAt0); // constant expression
double measurement = 7;
Double_ expression(&Event::toa, eventExpression, microphoneConstant);
ExpressionFactor<double> factor(model, measurement, expression);
TOAFactor factor(key, sensorAt0, measurement, model);
}
//*****************************************************************************
TEST( TOAFactor, WholeEnchilada ) {
static const bool verbose = false;
// Create microphones
TEST(TOAFactor, WholeEnchilada) {
// Create sensors
const double height = 0.5;
vector<Point3> microphones;
microphones.push_back(Point3(0, 0, height));
microphones.push_back(Point3(403 * cm, 0, height));
microphones.push_back(Point3(403 * cm, 403 * cm, height));
microphones.push_back(Point3(0, 403 * cm, 2 * height));
EXPECT_LONGS_EQUAL(4, microphones.size());
// microphones.push_back(Point3(200 * cm, 200 * cm, height));
vector<Point3> sensors;
sensors.push_back(Point3(0, 0, height));
sensors.push_back(Point3(403 * cm, 0, height));
sensors.push_back(Point3(403 * cm, 403 * cm, height));
sensors.push_back(Point3(0, 403 * cm, 2 * height));
EXPECT_LONGS_EQUAL(4, sensors.size());
// sensors.push_back(Point3(200 * cm, 200 * cm, height));
// Create a ground truth point
const double timeOfEvent = 0;
Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);
// Simulate simulatedTOA
size_t K = microphones.size();
size_t K = sensors.size();
vector<double> simulatedTOA(K);
TimeOfArrival toa;
for (size_t i = 0; i < K; i++) {
simulatedTOA[i] = groundTruthEvent.toa(microphones[i]);
if (verbose) {
cout << "mic" << i << " = " << microphones[i] << endl;
cout << "z" << i << " = " << simulatedTOA[i] / ms << endl;
}
simulatedTOA[i] = toa(groundTruthEvent, sensors[i]);
}
// Now, estimate using non-linear optimization
ExpressionFactorGraph graph;
NonlinearFactorGraph graph;
Key key = 12;
Event_ eventExpression(key);
for (size_t i = 0; i < K; i++) {
Point3_ microphone_i(microphones[i]); // constant expression
Double_ predictTOA(&Event::toa, eventExpression, microphone_i);
graph.addExpressionFactor(predictTOA, simulatedTOA[i], model);
graph.emplace_shared<TOAFactor>(key, sensors[i], simulatedTOA[i], model);
}
/// Print the graph
if (verbose)
GTSAM_PRINT(graph);
// Create initial estimate
Values initialEstimate;
//Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
// Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
Vector4 delta;
delta << 0.1, 0.1, -0.1, 0.1;
Event estimatedEvent = groundTruthEvent.retract(delta);
initialEstimate.insert(key, estimatedEvent);
// Print
if (verbose)
initialEstimate.print("Initial Estimate:\n");
// Optimize using Levenberg-Marquardt optimization.
LevenbergMarquardtParams params;
params.setAbsoluteErrorTol(1e-10);
if (verbose)
params.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
Values result = optimizer.optimize();
if (verbose)
result.print("Final Result:\n");
EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
}
@ -129,4 +106,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
//*****************************************************************************

1
matlab/+gtsam/Contents.m
View File

@ -94,6 +94,7 @@
% Rot2 - class Rot2, see Doxygen page for details
% Rot3 - class Rot3, see Doxygen page for details
% SimpleCamera - class SimpleCamera, see Doxygen page for details
% PinholeCameraCal3_S2 - class PinholeCameraCal3_S2, see Doxygen page for details
% StereoPoint2 - class StereoPoint2, see Doxygen page for details
%
%% SLAM and SFM

2
matlab/+gtsam/VisualISAMGenerateData.m
View File

@ -31,7 +31,7 @@ data.K = truth.K;
for i=1:options.nrCameras
theta = (i-1)*2*pi/options.nrCameras;
t = Point3([r*cos(theta), r*sin(theta), height]');
truth.cameras{i} = SimpleCamera.Lookat(t, Point3, Point3([0,0,1]'), truth.K);
truth.cameras{i} = PinholeCameraCal3_S2.Lookat(t, Point3, Point3([0,0,1]'), truth.K);
% Create measurements
for j=1:nrPoints
% All landmarks seen in every frame

2
matlab/+gtsam/cylinderSampleProjection.m
View File

@ -13,7 +13,7 @@ function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinder
import gtsam.*
camera = SimpleCamera(pose, K);
camera = PinholeCameraCal3_S2(pose, K);
%% memory allocation
cylinderNum = length(cylinders);

4
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -92,7 +92,7 @@ if options.enableTests
cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
end
camera = SimpleCamera.Lookat(Point3(10, 50, 10), ...
camera = PinholeCameraCal3_S2.Lookat(Point3(10, 50, 10), ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.monoK);
@ -154,7 +154,7 @@ while 1
%t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ...
% 15, 10]');
camera = SimpleCamera.Lookat(t, ...
camera = PinholeCameraCal3_S2.Lookat(t, ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.camera.monoK);
cameraPoses{end+1} = camera.pose;

4
matlab/gtsam_examples/SBAExample.m
View File

@ -46,7 +46,7 @@ end
%% Add Gaussian priors for a pose and a landmark to constrain the system
cameraPriorNoise = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas);
firstCamera = SimpleCamera(truth.cameras{1}.pose, truth.K);
firstCamera = PinholeCameraCal3_S2(truth.cameras{1}.pose, truth.K);
graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise));
pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
@ -60,7 +60,7 @@ graph.print(sprintf('\nFactor graph:\n'));
initialEstimate = Values;
for i=1:size(truth.cameras,2)
pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1));
camera_i = SimpleCamera(pose_i, truth.K);
camera_i = PinholeCameraCal3_S2(pose_i, truth.K);
initialEstimate.insert(symbol('c',i), camera_i);
end
for j=1:size(truth.points,2)

4
matlab/gtsam_tests/testTriangulation.m
View File

@ -18,11 +18,11 @@ sharedCal = Cal3_S2(1500, 1200, 0, 640, 480);
%% Looking along X-axis, 1 meter above ground plane (x-y)
upright = Rot3.Ypr(-pi / 2, 0., -pi / 2);
pose1 = Pose3(upright, Point3(0, 0, 1));
camera1 = SimpleCamera(pose1, sharedCal);
camera1 = PinholeCameraCal3_S2(pose1, sharedCal);
%% create second camera 1 meter to the right of first camera
pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)));
camera2 = SimpleCamera(pose2, sharedCal);
camera2 = PinholeCameraCal3_S2(pose2, sharedCal);
%% landmark ~5 meters infront of camera
landmark =Point3 (5, 0.5, 1.2);

4
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
View File

@ -97,8 +97,8 @@ if options.useRealData == 1
% Create the camera based on the current pose and the pose of the
% camera in the body
cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera);
camera = SimpleCamera(cameraPose, metadata.camera.calibration);
%camera = SimpleCamera(currentPose, metadata.camera.calibration);
camera = PinholeCameraCal3_S2(cameraPose, metadata.camera.calibration);
%camera = PinholeCameraCal3_S2(currentPose, metadata.camera.calibration);
% Record measurements if the landmark is within visual range
for j=1:length(metadata.camera.gtLandmarkPoints)

2
matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m
View File

@ -108,7 +108,7 @@ for i=1:20
% generate some camera measurements
cam_pose = initial.atPose3(i).compose(actual_transform);
% gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
cam = PinholeCameraCal3_S2(cam_pose,K);
i
% result
for j=1:nrPoints

2
matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
View File

@ -182,7 +182,7 @@ for i=1:steps
% generate some camera measurements
cam_pose = currentIMUPoseGlobal.compose(actual_transform);
% gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
cam = PinholeCameraCal3_S2(cam_pose,K);
i
% result
for j=1:nrPoints

2
matlab/unstable_examples/TransformProjectionFactorExample.m
View File

@ -73,7 +73,7 @@ for i=1:20
% generate some camera measurements
cam_pose = initial.atPose3(i).compose(actual_transform);
gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
cam = PinholeCameraCal3_S2(cam_pose,K);
i
for j=1:nrPoints
% All landmarks seen in every frame

2
matlab/unstable_examples/TransformProjectionFactorExampleISAM.m
View File

@ -98,7 +98,7 @@ for i=1:20
% generate some camera measurements
cam_pose = initial.atPose3(i).compose(actual_transform);
% gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
cam = PinholeCameraCal3_S2(cam_pose,K);
i
% result
for j=1:nrPoints

2
matlab/unstable_examples/project_landmarks.m
View File

@ -4,7 +4,7 @@ function [ measurements ] = project_landmarks( pose, landmarks, K )
import gtsam.*;
camera = SimpleCamera(pose,K);
camera = PinholeCameraCal3_S2(pose,K);
measurements = Values;
for i=1:size(landmarks)-1

6
tests/testNonlinearEquality.cpp
View File

@ -28,7 +28,7 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <CppUnitLite/TestHarness.h>
@ -527,10 +527,10 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
Rot3 faceTowardsY(Point3(1, 0, 0), Point3(0, 0, -1), Point3(0, 1, 0));
Pose3 poseLeft(faceTowardsY, Point3(0, 0, 0)); // origin, left camera
SimpleCamera leftCamera(poseLeft, K);
PinholeCamera<Cal3_S2> leftCamera(poseLeft, K);
Pose3 poseRight(faceTowardsY, Point3(2, 0, 0)); // 2 units to the right
SimpleCamera rightCamera(poseRight, K);
PinholeCamera<Cal3_S2> rightCamera(poseRight, K);
Point3 landmark(1, 5, 0); //centered between the cameras, 5 units away

83
tests/testSerializationSLAM.cpp
View File

@ -47,7 +47,7 @@
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/base/serializationTestHelpers.h>
@ -57,20 +57,21 @@ using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
// Creating as many permutations of factors as possible
typedef PriorFactor<LieVector> PriorFactorLieVector;
typedef PriorFactor<LieMatrix> PriorFactorLieMatrix;
typedef PriorFactor<Point2> PriorFactorPoint2;
typedef PriorFactor<StereoPoint2> PriorFactorStereoPoint2;
typedef PriorFactor<Point3> PriorFactorPoint3;
typedef PriorFactor<Rot2> PriorFactorRot2;
typedef PriorFactor<Rot3> PriorFactorRot3;
typedef PriorFactor<Pose2> PriorFactorPose2;
typedef PriorFactor<Pose3> PriorFactorPose3;
typedef PriorFactor<Cal3_S2> PriorFactorCal3_S2;
typedef PriorFactor<Cal3DS2> PriorFactorCal3DS2;
typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera;
typedef PriorFactor<SimpleCamera> PriorFactorSimpleCamera;
typedef PriorFactor<StereoCamera> PriorFactorStereoCamera;
typedef PriorFactor<LieVector> PriorFactorLieVector;
typedef PriorFactor<LieMatrix> PriorFactorLieMatrix;
typedef PriorFactor<Point2> PriorFactorPoint2;
typedef PriorFactor<StereoPoint2> PriorFactorStereoPoint2;
typedef PriorFactor<Point3> PriorFactorPoint3;
typedef PriorFactor<Rot2> PriorFactorRot2;
typedef PriorFactor<Rot3> PriorFactorRot3;
typedef PriorFactor<Pose2> PriorFactorPose2;
typedef PriorFactor<Pose3> PriorFactorPose3;
typedef PriorFactor<Cal3_S2> PriorFactorCal3_S2;
typedef PriorFactor<Cal3DS2> PriorFactorCal3DS2;
typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera;
typedef PriorFactor<SimpleCamera> PriorFactorSimpleCamera;
typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
typedef PriorFactor<StereoCamera> PriorFactorStereoCamera;
typedef BetweenFactor<LieVector> BetweenFactorLieVector;
typedef BetweenFactor<LieMatrix> BetweenFactorLieMatrix;
@ -81,29 +82,32 @@ typedef BetweenFactor<Rot3> BetweenFactorRot3;
typedef BetweenFactor<Pose2> BetweenFactorPose2;
typedef BetweenFactor<Pose3> BetweenFactorPose3;
typedef NonlinearEquality<LieVector> NonlinearEqualityLieVector;
typedef NonlinearEquality<LieMatrix> NonlinearEqualityLieMatrix;
typedef NonlinearEquality<Point2> NonlinearEqualityPoint2;
typedef NonlinearEquality<StereoPoint2> NonlinearEqualityStereoPoint2;
typedef NonlinearEquality<Point3> NonlinearEqualityPoint3;
typedef NonlinearEquality<Rot2> NonlinearEqualityRot2;
typedef NonlinearEquality<Rot3> NonlinearEqualityRot3;
typedef NonlinearEquality<Pose2> NonlinearEqualityPose2;
typedef NonlinearEquality<Pose3> NonlinearEqualityPose3;
typedef NonlinearEquality<Cal3_S2> NonlinearEqualityCal3_S2;
typedef NonlinearEquality<Cal3DS2> NonlinearEqualityCal3DS2;
typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera;
typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera;
typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera;
typedef NonlinearEquality<LieVector> NonlinearEqualityLieVector;
typedef NonlinearEquality<LieMatrix> NonlinearEqualityLieMatrix;
typedef NonlinearEquality<Point2> NonlinearEqualityPoint2;
typedef NonlinearEquality<StereoPoint2> NonlinearEqualityStereoPoint2;
typedef NonlinearEquality<Point3> NonlinearEqualityPoint3;
typedef NonlinearEquality<Rot2> NonlinearEqualityRot2;
typedef NonlinearEquality<Rot3> NonlinearEqualityRot3;
typedef NonlinearEquality<Pose2> NonlinearEqualityPose2;
typedef NonlinearEquality<Pose3> NonlinearEqualityPose3;
typedef NonlinearEquality<Cal3_S2> NonlinearEqualityCal3_S2;
typedef NonlinearEquality<Cal3DS2> NonlinearEqualityCal3DS2;
typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera;
typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera;
typedef NonlinearEquality<PinholeCameraCal3_S2> NonlinearEqualityPinholeCameraCal3_S2;
typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera;
typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D;
typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint;
typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraPoint;
typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D;
typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint;
typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraPoint;
typedef RangeFactor<PinholeCameraCal3_S2, Point3> RangeFactorPinholeCameraCal3_S2Point;
typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2;
typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
@ -111,7 +115,7 @@ typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorCal3_S2;
typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
@ -158,6 +162,7 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3DS2);
GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo);
GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera);
GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2);
GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
/* Create GUIDs for factors */
@ -294,7 +299,7 @@ TEST (testSerializationSLAM, factors) {
Cal3DS2 cal3ds2(1.0, 2.0, 3.0, 4.0, 5.0,6.0, 7.0, 8.0, 9.0);
Cal3_S2Stereo cal3_s2stereo(1.0, 2.0, 3.0, 4.0, 5.0, 1.0);
CalibratedCamera calibratedCamera(pose3);
SimpleCamera simpleCamera(pose3, cal3_s2);
PinholeCamera<Cal3_S2> simpleCamera(pose3, cal3_s2);
StereoCamera stereoCamera(pose3, boost::make_shared<Cal3_S2Stereo>(cal3_s2stereo));

2
tests/testVisualISAM2.cpp
View File

@ -59,7 +59,7 @@ TEST(testVisualISAM2, all)
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j)
{
SimpleCamera camera(poses[i], *K);
PinholeCamera<Cal3_S2> camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);

1
wrap/python/pybind11 Submodule

@ -0,0 +1 @@
Subproject commit b3bf248eec9cad8260753c982e1ae6cb72fff470