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

Merge pull request #258 from borglab/deprecate-simplecamera 

Replace SimpleCamera with PinholeCameraCal3_S2
release/4.3a0
Varun Agrawal 2020-03-23 08:08:18 -04:00 committed by GitHub
parent 748a27db38 87c338a18b
commit ca4daa0894
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
44 changed files with 227 additions and 201 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
import gtsam.utils.plot as gtsam_plot 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): def X(key):
@ -69,8 +74,8 @@ PARAMS.setUse2ndOrderCoriolis(False)
PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1) BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
gtsam.Point3(0.05, -0.10, 0.20)) Point3(0.05, -0.10, 0.20))
def IMU_example(): def IMU_example():
@ -78,10 +83,10 @@ def IMU_example():
# Start with a camera on x-axis looking at origin # Start with a camera on x-axis looking at origin
radius = 30 radius = 30
up = gtsam.Point3(0, 0, 1) up = Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0) target = Point3(0, 0, 0)
position = gtsam.Point3(radius, 0, 0) position = Point3(radius, 0, 0)
camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2()) camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
pose_0 = camera.pose() pose_0 = camera.pose()
# Create the set of ground-truth landmarks and poses # Create the set of ground-truth landmarks and poses
@ -90,37 +95,37 @@ def IMU_example():
angular_velocity_vector = vector3(0, -angular_velocity, 0) angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = gtsam.ConstantTwistScenario( scenario = ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0) angular_velocity_vector, linear_velocity_vector, pose_0)
# Create a factor graph # Create a factor graph
newgraph = gtsam.NonlinearFactorGraph() newgraph = NonlinearFactorGraph()
# Create (incremental) ISAM2 solver # Create (incremental) ISAM2 solver
isam = gtsam.ISAM2() isam = ISAM2()
# Create the initial estimate to the solution # Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth # 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. # 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 # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noise = gtsam.noiseModel_Diagonal.Sigmas( noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) 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 # Add imu priors
biasKey = gtsam.symbol(ord('b'), 0) biasKey = gtsam.symbol(ord('b'), 0)
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
biasnoise) biasnoise)
newgraph.push_back(biasprior) newgraph.push_back(biasprior)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
# Calculate with correct initial velocity # Calculate with correct initial velocity
n_velocity = vector3(0, angular_velocity * radius, 0) 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) newgraph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity) initialEstimate.insert(V(0), n_velocity)
@ -141,7 +146,7 @@ def IMU_example():
# Add Bias variables periodically # Add Bias variables periodically
if i % 5 == 0: if i % 5 == 0:
biasKey += 1 biasKey += 1
factor = gtsam.BetweenFactorConstantBias( factor = BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
newgraph.add(factor) newgraph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
@ -154,8 +159,7 @@ def IMU_example():
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
# Add Imu Factor # Add Imu Factor
imufac = gtsam.ImuFactor( imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac) newgraph.add(imufac)
# insert new velocity, which is wrong # insert new velocity, which is wrong
@ -168,7 +172,7 @@ def IMU_example():
ISAM2_plot(result) ISAM2_plot(result)
# reset # reset
newgraph = gtsam.NonlinearFactorGraph() newgraph = NonlinearFactorGraph()
initialEstimate.clear() initialEstimate.clear()

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

@ -16,7 +16,7 @@ from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
GenericProjectionFactorCal3_S2, NonlinearFactorGraph, GenericProjectionFactorCal3_S2, NonlinearFactorGraph,
Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, Point3, Pose3, PriorFactorPoint3, PriorFactorPose3,
Rot3, SimpleCamera, Values) Rot3, PinholeCameraCal3_S2, Values)
def symbol(name: str, index: int) -> int: def symbol(name: str, index: int) -> int:
@ -75,7 +75,7 @@ def main():
# Simulated measurements from each camera pose, adding them to the factor graph # Simulated measurements from each camera pose, adding them to the factor graph
for i, pose in enumerate(poses): for i, pose in enumerate(poses):
camera = SimpleCamera(pose, K) camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points): for j, point in enumerate(points):
measurement = camera.project(point) measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2( 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, from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
NonlinearFactorGraph, NonlinearISAM, Point3, Pose3, NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3, PriorFactorPoint3, PriorFactorPose3, Rot3,
SimpleCamera, Values) PinholeCameraCal3_S2, Values)
def symbol(name: str, index: int) -> int: def symbol(name: str, index: int) -> int:
@ -54,7 +54,7 @@ def main():
# Loop over the different poses, adding the observations to iSAM incrementally # Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses): for i, pose in enumerate(poses):
camera = SimpleCamera(pose, K) camera = PinholeCameraCal3_S2(pose, K)
# Add factors for each landmark observation # Add factors for each landmark observation
for j, point in enumerate(points): for j, point in enumerate(points):
measurement = camera.project(point) 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 See LICENSE for the license information
SimpleCamera unit tests. PinholeCameraCal3_S2 unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python) Author: Frank Dellaert & Duy Nguyen Ta (Python)
""" """
import math import math
@ -14,7 +14,7 @@ import unittest
import numpy as np import numpy as np
import gtsam 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 from gtsam.utils.test_case import GtsamTestCase
K = Cal3_S2(625, 625, 0, 0, 0) K = Cal3_S2(625, 625, 0, 0, 0)
@ -23,14 +23,14 @@ class TestSimpleCamera(GtsamTestCase):
def test_constructor(self): def test_constructor(self):
pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) 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.calibration(), K, 1e-9)
self.gtsamAssertEquals(camera.pose(), pose1, 1e-9) self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)
def test_level2(self): def test_level2(self):
# Create a level camera, looking in Y-direction # Create a level camera, looking in Y-direction
pose2 = Pose2(0.4,0.3,math.pi/2.0) 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 # expected
x = Point3(1,0,0) x = Point3(1,0,0)

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

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

5
examples/CameraResectioning.cpp
View File

@ -18,7 +18,8 @@
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.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> #include <boost/make_shared.hpp>
using namespace gtsam; using namespace gtsam;
@ -47,7 +48,7 @@ public:
/// evaluate the error /// evaluate the error
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H = virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const { boost::none) const {
SimpleCamera camera(pose, *K_); PinholeCamera<Cal3_S2> camera(pose, *K_);
return camera.project(P_, H, boost::none, boost::none) - p_; 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) * @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/nonlinear/ISAM2.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.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/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/ImuFactor.h>
@ -34,7 +35,7 @@ int main(int argc, char* argv[]) {
double radius = 30; double radius = 30;
const Point3 up(0, 0, 1), target(0, 0, 0); const Point3 up(0, 0, 1), target(0, 0, 0);
const Point3 position(radius, 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(); const auto pose_0 = camera.pose();
// Now, create a constant-twist scenario that makes the camera orbit the // 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 // Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) { 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) { for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); 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> #include <gtsam/nonlinear/ExpressionFactorGraph.h>
// Header order is close to far // Header order is close to far
#include <examples/SFMdata.h> #include "SFMdata.h"
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/DoglegOptimizer.h> #include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/Values.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 // Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) { for (size_t i = 0; i < poses.size(); ++i) {
Pose3_ x('x', 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) { for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
// Below an expression for the prediction of the measurement: // 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 * @brief Simple example for the structure-from-motion problems
* @author Duy-Nguyen Ta * @author Duy-Nguyen Ta
*/ */
@ -30,7 +30,8 @@
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
// We will also need a camera object to hold calibration information and perform projections. // 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() { 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); noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
for (size_t i = 0; i < poses.size(); ++i) { for (size_t i = 0; i < poses.size(); ++i) {
for (size_t j = 0; j < points.size(); ++j) { 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]); Point2 measurement = camera.project(points[j]);
// The only real difference with the Visual SLAM example is that here we use a // 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 // 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) { for (size_t i = 0; i < poses.size(); ++i) {
// Add factors for each landmark observation // Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) { 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]); Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >( graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(
measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); 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 // Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) { for (size_t j = 0; j < points.size(); ++j) {
// Create ground truth measurement // Create ground truth measurement
SimpleCamera camera(poses[i], *K); PinholeCamera<Cal3_S2> camera(poses[i], *K);
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
// Add measurement // Add measurement
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, noise, 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> #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 { virtual class GenericValue : gtsam::Value {
void serializable() const; 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::Cal3DS2& cal3ds2);
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); 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::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, const gtsam::imuBias::ConstantBias& constant_bias);
void insert(size_t j, Vector vector); void insert(size_t j, Vector vector);
void insert(size_t j, Matrix matrix); void insert(size_t j, Matrix matrix);
@ -2459,7 +2459,7 @@ class ISAM2 {
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::Cal3Bundler, gtsam::EssentialMatrix,
gtsam::SimpleCamera, Vector, Matrix}> gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, Vector, Matrix}>
VALUE calculateEstimate(size_t key) const; VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const; gtsam::Values calculateBestEstimate() const;
Matrix marginalCovariance(size_t key) const; Matrix marginalCovariance(size_t key) const;
@ -2497,7 +2497,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h> #include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.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 { virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const; T prior() const;
@ -2520,7 +2520,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
#include <gtsam/nonlinear/NonlinearEquality.h> #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 { virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation // Constructor - forces exact evaluation
NonlinearEquality(size_t j, const T& feasible); 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::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3; typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint; 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::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> #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); GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey);
gtsam::Point2 measured() const; 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 // due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2; //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
@ -3153,7 +3153,7 @@ namespace utilities {
void perturbPoint2(gtsam::Values& values, double sigma, int seed); void perturbPoint2(gtsam::Values& values, double sigma, int seed);
void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed);
void perturbPoint3(gtsam::Values& values, double sigma, 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);
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); 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); 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 // 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: // For classes with overloaded range methods, such as PinholeCamera, this can even be templated:
// template <typename T> struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {}; // template <typename T> struct Range<PinholeCamera, T> : HasRange<PinholeCamera, T, double> {};
template <class A1, typename A2, class RT> template <class A1, typename A2, class RT>
struct HasRange { struct HasRange {
typedef RT result_type; typedef RT result_type;

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

@ -17,7 +17,7 @@
*/ */
#include <gtsam/geometry/triangulation.h> #include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/StereoCamera.h> #include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/CameraSet.h> #include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Cal3Bundler.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 // 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)); 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); Point2 z3 = camera3.project(landmark);
poses += pose3; poses += pose3;
@ -168,7 +168,7 @@ TEST( triangulation, fourPoses) {
// 4. Test failure: Add a 4th camera facing the wrong way // 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)); 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 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
@ -185,17 +185,17 @@ TEST( triangulation, fourPoses) {
TEST( triangulation, fourPoses_distinct_Ks) { TEST( triangulation, fourPoses_distinct_Ks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480); Cal3_S2 K1(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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 // create second camera 1 meter to the right of first camera
Cal3_S2 K2(1600, 1300, 0, 650, 440); 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 // 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(landmark);
CameraSet<SimpleCamera> cameras; CameraSet<PinholeCamera<Cal3_S2> > cameras;
Point2Vector measurements; Point2Vector measurements;
cameras += camera1, camera2; cameras += camera1, camera2;
@ -216,7 +216,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
// 3. Add a slightly rotated third camera above, again with measurement noise // 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)); 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); Cal3_S2 K3(700, 500, 0, 640, 480);
SimpleCamera camera3(pose3, K3); PinholeCamera<Cal3_S2> camera3(pose3, K3);
Point2 z3 = camera3.project(landmark); Point2 z3 = camera3.project(landmark);
cameras += camera3; cameras += camera3;
@ -234,7 +234,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
// 4. Test failure: Add a 4th camera facing the wrong way // 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)); Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Cal3_S2 K4(700, 500, 0, 640, 480); Cal3_S2 K4(700, 500, 0, 640, 480);
SimpleCamera camera4(pose4, K4); PinholeCamera<Cal3_S2> camera4(pose4, K4);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
@ -250,17 +250,17 @@ TEST( triangulation, fourPoses_distinct_Ks) {
TEST( triangulation, outliersAndFarLandmarks) { TEST( triangulation, outliersAndFarLandmarks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480); Cal3_S2 K1(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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 // create second camera 1 meter to the right of first camera
Cal3_S2 K2(1600, 1300, 0, 650, 440); 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 // 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(landmark);
CameraSet<SimpleCamera> cameras; CameraSet<PinholeCamera<Cal3_S2> > cameras;
Point2Vector measurements; Point2Vector measurements;
cameras += camera1, camera2; cameras += camera1, camera2;
@ -280,7 +280,7 @@ TEST( triangulation, outliersAndFarLandmarks) {
// 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER) // 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)); 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); Cal3_S2 K3(700, 500, 0, 640, 480);
SimpleCamera camera3(pose3, K3); PinholeCamera<Cal3_S2> camera3(pose3, K3);
Point2 z3 = camera3.project(landmark); Point2 z3 = camera3.project(landmark);
cameras += camera3; cameras += camera3;
@ -302,7 +302,7 @@ TEST( triangulation, outliersAndFarLandmarks) {
//****************************************************************************** //******************************************************************************
TEST( triangulation, twoIdenticalPoses) { TEST( triangulation, twoIdenticalPoses) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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 // 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark); 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. // Expressions wrap trees of functions that can evaluate their own derivatives.
// The meta-functions below are useful to specify the type of those functions. // 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: // 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> template<class A1>
struct UnaryFunction { struct UnaryFunction {
typedef boost::function< typedef boost::function<

4
gtsam/nonlinear/utilities.h
View File

@ -28,7 +28,7 @@
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <exception> #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 /// 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) { const Vector& J, const Matrix& Z, double depth) {
if (Z.rows() != 2) if (Z.rows() != 2)
throw std::invalid_argument("insertBackProjections: Z must be 2*K"); 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/sam/RangeFactor.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.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/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h> #include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/base/TestableAssertions.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) { TEST( RangeFactor, Camera) {
RangeFactor<SimpleCamera,Point3> factor1(poseKey, pointKey, measurement, model); using Camera = PinholeCamera<Cal3_S2>;
RangeFactor<SimpleCamera,Pose3> factor2(poseKey, pointKey, measurement, model);
RangeFactor<SimpleCamera,SimpleCamera> factor3(poseKey, pointKey, measurement, model); 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/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/EssentialMatrix.h> #include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <iostream> #include <iostream>
namespace gtsam { namespace gtsam {

3
gtsam/slam/ProjectionFactor.h
View File

@ -21,7 +21,8 @@
#pragma once #pragma once
#include <gtsam/nonlinear/NonlinearFactor.h> #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> #include <boost/optional.hpp>
namespace gtsam { namespace gtsam {

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

@ -13,6 +13,7 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/CalibratedCamera.h> #include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.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/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/StereoCamera.h> #include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/nonlinear/Expression.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) // 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 Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); 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 // landmark ~5 meters infront of camera
static const Point3 landmark(5, 0.5, 1.2); static const Point3 landmark(5, 0.5, 1.2);
@ -52,7 +52,7 @@ TEST( triangulation, TriangulationFactor ) {
Key pointKey(1); Key pointKey(1);
SharedNoiseModel model; SharedNoiseModel model;
typedef TriangulationFactor<SimpleCamera> Factor; typedef TriangulationFactor<PinholeCamera<Cal3_S2> > Factor;
Factor factor(camera1, z1, model, pointKey); Factor factor(camera1, z1, model, pointKey);
// Use the factor to calculate the Jacobians // Use the factor to calculate the Jacobians

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

@ -9,7 +9,8 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.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> #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)); 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)); 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) { TEST( InvDepthFactor, Project1) {

2
gtsam_unstable/slam/MultiProjectionFactor.h
View File

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

79
gtsam_unstable/slam/serialization.cpp
View File

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

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

@ -7,7 +7,7 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.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. // 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)); 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 InvDepthFactor3<Pose3, Vector5, double> InverseDepthFactor;
typedef NonlinearEquality<Pose3> PoseConstraint; typedef NonlinearEquality<Pose3> PoseConstraint;
@ -64,7 +64,7 @@ TEST( InvDepthFactor, optimize) {
// add a camera 2 meters to the right // add a camera 2 meters to the right
Pose3 right_pose = level_pose * Pose3(Rot3(), Point3(2,0,0)); 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 // projection measurement of landmark into right camera
// this measurement disagrees with the depth initialization // 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 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
SimpleCamera cam1(cameraPose1, *K); // with camera poses PinholeCamera<Cal3_S2> cam1(cameraPose1, *K); // with camera poses
SimpleCamera cam2(cameraPose2, *K); PinholeCamera<Cal3_S2> cam2(cameraPose2, *K);
SimpleCamera cam3(cameraPose3, *K); PinholeCamera<Cal3_S2> cam3(cameraPose3, *K);
// create arbitrary body_Pose_sensor (transforms from sensor to body) // 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(); // Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //

1
matlab/+gtsam/Contents.m
View File

@ -94,6 +94,7 @@
% Rot2 - class Rot2, see Doxygen page for details % Rot2 - class Rot2, see Doxygen page for details
% Rot3 - class Rot3, see Doxygen page for details % Rot3 - class Rot3, see Doxygen page for details
% SimpleCamera - class SimpleCamera, 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 % StereoPoint2 - class StereoPoint2, see Doxygen page for details
% %
%% SLAM and SFM %% SLAM and SFM

2
matlab/+gtsam/VisualISAMGenerateData.m
View File

@ -31,7 +31,7 @@ data.K = truth.K;
for i=1:options.nrCameras for i=1:options.nrCameras
theta = (i-1)*2*pi/options.nrCameras; theta = (i-1)*2*pi/options.nrCameras;
t = Point3([r*cos(theta), r*sin(theta), height]'); 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 % Create measurements
for j=1:nrPoints for j=1:nrPoints
% All landmarks seen in every frame % 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.* import gtsam.*
camera = SimpleCamera(pose, K); camera = PinholeCameraCal3_S2(pose, K);
%% memory allocation %% memory allocation
cylinderNum = length(cylinders); 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)); cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
end 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(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.monoK); Point3([0,0,1]'), options.monoK);
@ -154,7 +154,7 @@ while 1
%t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ... %t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ...
% 15, 10]'); % 15, 10]');
camera = SimpleCamera.Lookat(t, ... camera = PinholeCameraCal3_S2.Lookat(t, ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.camera.monoK); Point3([0,0,1]'), options.camera.monoK);
cameraPoses{end+1} = camera.pose; 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 %% Add Gaussian priors for a pose and a landmark to constrain the system
cameraPriorNoise = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas); 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)); graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise));
pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
@ -60,7 +60,7 @@ graph.print(sprintf('\nFactor graph:\n'));
initialEstimate = Values; initialEstimate = Values;
for i=1:size(truth.cameras,2) for i=1:size(truth.cameras,2)
pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1)); 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); initialEstimate.insert(symbol('c',i), camera_i);
end end
for j=1:size(truth.points,2) 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) %% Looking along X-axis, 1 meter above ground plane (x-y)
upright = Rot3.Ypr(-pi / 2, 0., -pi / 2); upright = Rot3.Ypr(-pi / 2, 0., -pi / 2);
pose1 = Pose3(upright, Point3(0, 0, 1)); 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 %% create second camera 1 meter to the right of first camera
pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))); 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 ~5 meters infront of camera
landmark =Point3 (5, 0.5, 1.2); 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 % Create the camera based on the current pose and the pose of the
% camera in the body % camera in the body
cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera); cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera);
camera = SimpleCamera(cameraPose, metadata.camera.calibration); camera = PinholeCameraCal3_S2(cameraPose, metadata.camera.calibration);
%camera = SimpleCamera(currentPose, metadata.camera.calibration); %camera = PinholeCameraCal3_S2(currentPose, metadata.camera.calibration);
% Record measurements if the landmark is within visual range % Record measurements if the landmark is within visual range
for j=1:length(metadata.camera.gtLandmarkPoints) 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 % generate some camera measurements
cam_pose = initial.atPose3(i).compose(actual_transform); cam_pose = initial.atPose3(i).compose(actual_transform);
% gtsam.plotPose3(cam_pose); % gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K); cam = PinholeCameraCal3_S2(cam_pose,K);
i i
% result % result
for j=1:nrPoints for j=1:nrPoints

2
matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
View File

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

2
matlab/unstable_examples/TransformProjectionFactorExample.m
View File

@ -73,7 +73,7 @@ for i=1:20
% generate some camera measurements % generate some camera measurements
cam_pose = initial.atPose3(i).compose(actual_transform); cam_pose = initial.atPose3(i).compose(actual_transform);
gtsam.plotPose3(cam_pose); gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K); cam = PinholeCameraCal3_S2(cam_pose,K);
i i
for j=1:nrPoints for j=1:nrPoints
% All landmarks seen in every frame % 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 % generate some camera measurements
cam_pose = initial.atPose3(i).compose(actual_transform); cam_pose = initial.atPose3(i).compose(actual_transform);
% gtsam.plotPose3(cam_pose); % gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K); cam = PinholeCameraCal3_S2(cam_pose,K);
i i
% result % result
for j=1:nrPoints 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.*; import gtsam.*;
camera = SimpleCamera(pose,K); camera = PinholeCameraCal3_S2(pose,K);
measurements = Values; measurements = Values;
for i=1:size(landmarks)-1 for i=1:size(landmarks)-1

6
tests/testNonlinearEquality.cpp
View File

@ -28,7 +28,7 @@
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <CppUnitLite/TestHarness.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)); Rot3 faceTowardsY(Point3(1, 0, 0), Point3(0, 0, -1), Point3(0, 1, 0));
Pose3 poseLeft(faceTowardsY, Point3(0, 0, 0)); // origin, left camera 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 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 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/Cal3DS2.h>
#include <gtsam/geometry/Cal3_S2Stereo.h> #include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/CalibratedCamera.h> #include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/StereoCamera.h> #include <gtsam/geometry/StereoCamera.h>
#include <gtsam/base/serializationTestHelpers.h> #include <gtsam/base/serializationTestHelpers.h>
@ -57,20 +57,21 @@ using namespace gtsam;
using namespace gtsam::serializationTestHelpers; using namespace gtsam::serializationTestHelpers;
// Creating as many permutations of factors as possible // Creating as many permutations of factors as possible
typedef PriorFactor<LieVector> PriorFactorLieVector; typedef PriorFactor<LieVector> PriorFactorLieVector;
typedef PriorFactor<LieMatrix> PriorFactorLieMatrix; typedef PriorFactor<LieMatrix> PriorFactorLieMatrix;
typedef PriorFactor<Point2> PriorFactorPoint2; typedef PriorFactor<Point2> PriorFactorPoint2;
typedef PriorFactor<StereoPoint2> PriorFactorStereoPoint2; typedef PriorFactor<StereoPoint2> PriorFactorStereoPoint2;
typedef PriorFactor<Point3> PriorFactorPoint3; typedef PriorFactor<Point3> PriorFactorPoint3;
typedef PriorFactor<Rot2> PriorFactorRot2; typedef PriorFactor<Rot2> PriorFactorRot2;
typedef PriorFactor<Rot3> PriorFactorRot3; typedef PriorFactor<Rot3> PriorFactorRot3;
typedef PriorFactor<Pose2> PriorFactorPose2; typedef PriorFactor<Pose2> PriorFactorPose2;
typedef PriorFactor<Pose3> PriorFactorPose3; typedef PriorFactor<Pose3> PriorFactorPose3;
typedef PriorFactor<Cal3_S2> PriorFactorCal3_S2; typedef PriorFactor<Cal3_S2> PriorFactorCal3_S2;
typedef PriorFactor<Cal3DS2> PriorFactorCal3DS2; typedef PriorFactor<Cal3DS2> PriorFactorCal3DS2;
typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera; typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera;
typedef PriorFactor<SimpleCamera> PriorFactorSimpleCamera; typedef PriorFactor<SimpleCamera> PriorFactorSimpleCamera;
typedef PriorFactor<StereoCamera> PriorFactorStereoCamera; typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
typedef PriorFactor<StereoCamera> PriorFactorStereoCamera;
typedef BetweenFactor<LieVector> BetweenFactorLieVector; typedef BetweenFactor<LieVector> BetweenFactorLieVector;
typedef BetweenFactor<LieMatrix> BetweenFactorLieMatrix; typedef BetweenFactor<LieMatrix> BetweenFactorLieMatrix;
@ -81,29 +82,32 @@ typedef BetweenFactor<Rot3> BetweenFactorRot3;
typedef BetweenFactor<Pose2> BetweenFactorPose2; typedef BetweenFactor<Pose2> BetweenFactorPose2;
typedef BetweenFactor<Pose3> BetweenFactorPose3; typedef BetweenFactor<Pose3> BetweenFactorPose3;
typedef NonlinearEquality<LieVector> NonlinearEqualityLieVector; typedef NonlinearEquality<LieVector> NonlinearEqualityLieVector;
typedef NonlinearEquality<LieMatrix> NonlinearEqualityLieMatrix; typedef NonlinearEquality<LieMatrix> NonlinearEqualityLieMatrix;
typedef NonlinearEquality<Point2> NonlinearEqualityPoint2; typedef NonlinearEquality<Point2> NonlinearEqualityPoint2;
typedef NonlinearEquality<StereoPoint2> NonlinearEqualityStereoPoint2; typedef NonlinearEquality<StereoPoint2> NonlinearEqualityStereoPoint2;
typedef NonlinearEquality<Point3> NonlinearEqualityPoint3; typedef NonlinearEquality<Point3> NonlinearEqualityPoint3;
typedef NonlinearEquality<Rot2> NonlinearEqualityRot2; typedef NonlinearEquality<Rot2> NonlinearEqualityRot2;
typedef NonlinearEquality<Rot3> NonlinearEqualityRot3; typedef NonlinearEquality<Rot3> NonlinearEqualityRot3;
typedef NonlinearEquality<Pose2> NonlinearEqualityPose2; typedef NonlinearEquality<Pose2> NonlinearEqualityPose2;
typedef NonlinearEquality<Pose3> NonlinearEqualityPose3; typedef NonlinearEquality<Pose3> NonlinearEqualityPose3;
typedef NonlinearEquality<Cal3_S2> NonlinearEqualityCal3_S2; typedef NonlinearEquality<Cal3_S2> NonlinearEqualityCal3_S2;
typedef NonlinearEquality<Cal3DS2> NonlinearEqualityCal3DS2; typedef NonlinearEquality<Cal3DS2> NonlinearEqualityCal3DS2;
typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera; typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera;
typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera; typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera;
typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera; typedef NonlinearEquality<PinholeCameraCal3_S2> NonlinearEqualityPinholeCameraCal3_S2;
typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera;
typedef RangeFactor<Pose2, Point2> RangeFactor2D; typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D; typedef RangeFactor<Pose3, Point3> RangeFactor3D;
typedef RangeFactor<Pose2, Pose2> RangeFactorPose2; typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
typedef RangeFactor<Pose3, Pose3> RangeFactorPose3; typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint; typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint;
typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraPoint; typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraPoint;
typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera; typedef RangeFactor<PinholeCameraCal3_S2, Point3> RangeFactorPinholeCameraCal3_S2Point;
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera; 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<Pose2, Point2> BearingRangeFactor2D;
typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D; 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, Cal3_S2> GenericProjectionFactorCal3_S2;
typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2; 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::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2; 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::Cal3_S2Stereo);
GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera);
GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2);
GTSAM_VALUE_EXPORT(gtsam::StereoCamera); GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
/* Create GUIDs for factors */ /* 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); 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); Cal3_S2Stereo cal3_s2stereo(1.0, 2.0, 3.0, 4.0, 5.0, 1.0);
CalibratedCamera calibratedCamera(pose3); 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)); 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 // Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) 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]); Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>( graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);