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.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()

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

@ -16,7 +16,7 @@ from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
GenericProjectionFactorCal3_S2, NonlinearFactorGraph,
Point3, Pose3, PriorFactorPoint3, PriorFactorPose3,
Rot3, SimpleCamera, Values)
Rot3, PinholeCameraCal3_S2, Values)
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
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

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

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) {

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 {

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(); //

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);