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

replaced/appended all calls to SimpleCamera with PinholeCameraCal3_S2

release/4.3a0
Varun Agrawal 2020-02-21 19:42:55 -05:00
parent 56ca889913
commit d9923fc3cc
44 changed files with 180 additions and 164 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -81,7 +81,7 @@ def IMU_example():
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())
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, gtsam.Cal3_S2())
pose_0 = camera.pose()
# Create the set of ground-truth landmarks and poses

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)

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

@ -99,7 +99,7 @@ def generate_data(options):
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,
truth.cameras[i] = gtsam.PinholeCameraCal3_S2.Lookat(t,
gtsam.Point3(),
gtsam.Point3(0, 0, 1),
truth.K)

4
examples/CameraResectioning.cpp
View File

@ -18,7 +18,7 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <boost/make_shared.hpp>
using namespace gtsam;
@ -47,7 +47,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_;
}
};

2
examples/ISAM2Example_SmartFactor.cpp
View File

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

4
examples/ImuFactorExample2.cpp
View File

@ -1,5 +1,5 @@
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/ImuFactor.h>
@ -34,7 +34,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 PinholeCamera<Cal3_S2> 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,

16
gtsam.h
View File

@ -2133,7 +2133,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);
@ -2454,7 +2454,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;
@ -2492,7 +2492,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;
@ -2515,7 +2515,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);
@ -2541,9 +2541,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>
@ -2634,7 +2634,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;
@ -3134,7 +3134,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");

11
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,11 @@ 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);
RangeFactor<PinholeCamera<Cal3_S2>,Point3> factor1(poseKey, pointKey, measurement, model);
RangeFactor<PinholeCamera<Cal3_S2>,Pose3> factor2(poseKey, pointKey, measurement, model);
RangeFactor<PinholeCamera<Cal3_S2>,PinholeCamera<Cal3_S2>> 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 {

7
gtsam_unstable/slam/serialization.cpp
View File

@ -44,6 +44,7 @@ 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;
@ -68,6 +69,7 @@ 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;
@ -76,8 +78,10 @@ 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

11
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>
@ -70,6 +70,7 @@ 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;
@ -94,6 +95,7 @@ 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;
@ -102,8 +104,10 @@ 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);