From d9923fc3cc36605ce52ab35a71bb4a69713d045f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 21 Feb 2020 19:42:55 -0500 Subject: [PATCH] replaced/appended all calls to SimpleCamera with PinholeCameraCal3_S2 --- cython/gtsam/examples/ImuFactorExample2.py | 2 +- cython/gtsam/examples/SFMExample.py | 4 +- cython/gtsam/examples/VisualISAMExample.py | 4 +- cython/gtsam/tests/test_SimpleCamera.py | 8 +- cython/gtsam/utils/visual_data_generator.py | 2 +- examples/CameraResectioning.cpp | 4 +- examples/ISAM2Example_SmartFactor.cpp | 2 +- examples/ImuFactorExample2.cpp | 4 +- examples/SFMExample.cpp | 2 +- examples/SFMExampleExpressions.cpp | 4 +- examples/SFMdata.h | 5 +- examples/SelfCalibrationExample.cpp | 2 +- examples/VisualISAM2Example.cpp | 2 +- examples/VisualISAMExample.cpp | 2 +- gtsam.h | 18 ++-- gtsam/geometry/BearingRange.h | 4 +- gtsam/geometry/tests/testTriangulation.cpp | 26 +++--- gtsam/nonlinear/Expression.h | 2 +- gtsam/nonlinear/utilities.h | 4 +- gtsam/sam/tests/testRangeFactor.cpp | 11 +-- gtsam/slam/EssentialMatrixFactor.h | 2 +- gtsam/slam/ProjectionFactor.h | 3 +- .../slam/tests/testEssentialMatrixFactor.cpp | 1 + gtsam/slam/tests/testTriangulationFactor.cpp | 6 +- .../geometry/tests/testInvDepthCamera3.cpp | 5 +- gtsam_unstable/slam/MultiProjectionFactor.h | 2 +- gtsam_unstable/slam/serialization.cpp | 79 +++++++++--------- .../slam/tests/testInvDepthFactor3.cpp | 6 +- .../testSmartStereoProjectionPoseFactor.cpp | 6 +- matlab/+gtsam/Contents.m | 1 + matlab/+gtsam/VisualISAMGenerateData.m | 2 +- matlab/+gtsam/cylinderSampleProjection.m | 2 +- matlab/gtsam_examples/CameraFlyingExample.m | 4 +- matlab/gtsam_examples/SBAExample.m | 4 +- matlab/gtsam_tests/testTriangulation.m | 4 +- .../covarianceAnalysisCreateTrajectory.m | 4 +- .../TransformCalProjectionFactorExampleISAM.m | 2 +- ...ansformCalProjectionFactorIMUExampleISAM.m | 2 +- .../TransformProjectionFactorExample.m | 2 +- .../TransformProjectionFactorExampleISAM.m | 2 +- matlab/unstable_examples/project_landmarks.m | 2 +- tests/testNonlinearEquality.cpp | 6 +- tests/testSerializationSLAM.cpp | 83 ++++++++++--------- tests/testVisualISAM2.cpp | 2 +- 44 files changed, 180 insertions(+), 164 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index 1f6732f49..4b86d4837 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -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 diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py index bf46f09d5..21b90a60f 100644 --- a/cython/gtsam/examples/SFMExample.py +++ b/cython/gtsam/examples/SFMExample.py @@ -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( diff --git a/cython/gtsam/examples/VisualISAMExample.py b/cython/gtsam/examples/VisualISAMExample.py index f4d81eaf7..b7b83e2a1 100644 --- a/cython/gtsam/examples/VisualISAMExample.py +++ b/cython/gtsam/examples/VisualISAMExample.py @@ -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) diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py index efdfec561..a3654a5f1 100644 --- a/cython/gtsam/tests/test_SimpleCamera.py +++ b/cython/gtsam/tests/test_SimpleCamera.py @@ -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) diff --git a/cython/gtsam/utils/visual_data_generator.py b/cython/gtsam/utils/visual_data_generator.py index 91194c565..7cd885d49 100644 --- a/cython/gtsam/utils/visual_data_generator.py +++ b/cython/gtsam/utils/visual_data_generator.py @@ -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) diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index e3408b67b..e93e6ffca 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include using namespace gtsam; @@ -47,7 +47,7 @@ public: /// evaluate the error virtual Vector evaluateError(const Pose3& pose, boost::optional H = boost::none) const { - SimpleCamera camera(pose, *K_); + PinholeCamera camera(pose, *K_); return camera.project(P_, H, boost::none, boost::none) - p_; } }; diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp index d8fee1516..46b9fcd47 100644 --- a/examples/ISAM2Example_SmartFactor.cpp +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -4,7 +4,7 @@ * @author Alexander (pumaking on BitBucket) */ -#include +#include #include #include #include diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 25a6adf51..4ebc342eb 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -1,5 +1,5 @@ -#include +#include #include #include #include @@ -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 camera = PinholeCamera::Lookat(position, target, up); const auto pose_0 = camera.pose(); // Now, create a constant-twist scenario that makes the camera orbit the diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 8c87fa315..16cd25dba 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -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 camera(poses[i], *K); for (size_t j = 0; j < points.size(); ++j) { Point2 measurement = camera.project(points[j]); graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); diff --git a/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp index 191664ef6..73b6f27f7 100644 --- a/examples/SFMExampleExpressions.cpp +++ b/examples/SFMExampleExpressions.cpp @@ -27,7 +27,7 @@ #include // Header order is close to far -#include +#include "SFMdata.h" #include #include #include @@ -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 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: diff --git a/examples/SFMdata.h b/examples/SFMdata.h index 5462868c9..04d3c9e47 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -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 // We will also need a camera object to hold calibration information and perform projections. -#include +#include +#include /* ************************************************************************* */ std::vector createPoints() { diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index ba097c074..1d26ea0aa 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -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 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 diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 35bc9bcf6..db9090de6 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -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 camera(poses[i], *K); Point2 measurement = camera.project(points[j]); graph.emplace_shared >( measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 7a58deeb7..b4086910b 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -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 camera(poses[i], *K); Point2 measurement = camera.project(points[j]); // Add measurement graph.emplace_shared >(measurement, noise, diff --git a/gtsam.h b/gtsam.h index c0ed4d20f..caf377863 100644 --- a/gtsam.h +++ b/gtsam.h @@ -281,7 +281,7 @@ virtual class Value { }; #include -template +template virtual class GenericValue : gtsam::Value { void serializable() const; }; @@ -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 + 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 #include -template +template 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 -template +template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); @@ -2541,9 +2541,9 @@ typedef gtsam::RangeFactor RangeFactor3D; typedef gtsam::RangeFactor RangeFactorPose2; typedef gtsam::RangeFactor RangeFactorPose3; typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; +typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; +typedef gtsam::RangeFactor RangeFactorSimpleCamera; #include @@ -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 GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; // due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //typedef gtsam::GeneralSFMFactor 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); diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 8214b0727..7c73f3cbd 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -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 struct Range : HasRange {}; +// For classes with overloaded range methods, such as PinholeCamera, this can even be templated: +// template struct Range : HasRange {}; template struct HasRange { typedef RT result_type; diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 97412f94d..d2f0c91df 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -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 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 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 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 camera2(pose2, K2); // 1. Project two landmarks into two cameras and triangulate Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); - CameraSet cameras; + CameraSet > 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 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 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 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 camera2(pose2, K2); // 1. Project two landmarks into two cameras and triangulate Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); - CameraSet cameras; + CameraSet > 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 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 camera1(pose1, *sharedCal); // 1. Project two landmarks into two cameras and triangulate Point2 z1 = camera1.project(landmark); diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 7d02d479c..9e8aa3f29 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -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::BinaryFunction::type + // Expression::BinaryFunction,Point3>::type template struct UnaryFunction { typedef boost::function< diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 867db70e0..cf9fb9482 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include @@ -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& camera, const Vector& J, const Matrix& Z, double depth) { if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 5d57886f5..0d19918f6 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -19,7 +19,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -353,11 +354,11 @@ TEST(RangeFactor, Point3) { } /* ************************************************************************* */ -// Do tests with SimpleCamera +// Do tests with PinholeCamera TEST( RangeFactor, Camera) { - RangeFactor factor1(poseKey, pointKey, measurement, model); - RangeFactor factor2(poseKey, pointKey, measurement, model); - RangeFactor factor3(poseKey, pointKey, measurement, model); + RangeFactor,Point3> factor1(poseKey, pointKey, measurement, model); + RangeFactor,Pose3> factor2(poseKey, pointKey, measurement, model); + RangeFactor,PinholeCamera> factor3(poseKey, pointKey, measurement, model); } /* ************************************************************************* */ diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 2e921bfef..8bd155a14 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -9,7 +9,7 @@ #include #include -#include +#include #include namespace gtsam { diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 4a8a6b138..856913aae 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -21,7 +21,8 @@ #pragma once #include -#include +#include +#include #include namespace gtsam { diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index b2d368b67..05a94dab9 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 1d2baefee..4bbef6530 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -39,7 +39,7 @@ static const boost::shared_ptr 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 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 Factor; + typedef TriangulationFactor > Factor; Factor factor(camera1, z1, model, pointKey); // Use the factor to calculate the Jacobians diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index 6ac3389ed..4c6251831 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -9,7 +9,8 @@ #include #include -#include +#include +#include #include @@ -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 level_camera(level_pose, *K); /* ************************************************************************* */ TEST( InvDepthFactor, Project1) { diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 3e1263bb9..5bef97bcf 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -21,7 +21,7 @@ #pragma once #include -#include +#include #include namespace gtsam { diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 17c95e614..5b4bd8929 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -31,20 +31,21 @@ using namespace gtsam; // Creating as many permutations of factors as possible -typedef PriorFactor PriorFactorLieVector; -typedef PriorFactor PriorFactorLieMatrix; -typedef PriorFactor PriorFactorPoint2; -typedef PriorFactor PriorFactorStereoPoint2; -typedef PriorFactor PriorFactorPoint3; -typedef PriorFactor PriorFactorRot2; -typedef PriorFactor PriorFactorRot3; -typedef PriorFactor PriorFactorPose2; -typedef PriorFactor PriorFactorPose3; -typedef PriorFactor PriorFactorCal3_S2; -typedef PriorFactor PriorFactorCal3DS2; -typedef PriorFactor PriorFactorCalibratedCamera; -typedef PriorFactor PriorFactorSimpleCamera; -typedef PriorFactor PriorFactorStereoCamera; +typedef PriorFactor PriorFactorLieVector; +typedef PriorFactor PriorFactorLieMatrix; +typedef PriorFactor PriorFactorPoint2; +typedef PriorFactor PriorFactorStereoPoint2; +typedef PriorFactor PriorFactorPoint3; +typedef PriorFactor PriorFactorRot2; +typedef PriorFactor PriorFactorRot3; +typedef PriorFactor PriorFactorPose2; +typedef PriorFactor PriorFactorPose3; +typedef PriorFactor PriorFactorCal3_S2; +typedef PriorFactor PriorFactorCal3DS2; +typedef PriorFactor PriorFactorCalibratedCamera; +typedef PriorFactor PriorFactorSimpleCamera; +typedef PriorFactor PriorFactorPinholeCameraCal3_S2; +typedef PriorFactor PriorFactorStereoCamera; typedef BetweenFactor BetweenFactorLieVector; typedef BetweenFactor BetweenFactorLieMatrix; @@ -55,29 +56,32 @@ typedef BetweenFactor BetweenFactorRot3; typedef BetweenFactor BetweenFactorPose2; typedef BetweenFactor BetweenFactorPose3; -typedef NonlinearEquality NonlinearEqualityLieVector; -typedef NonlinearEquality NonlinearEqualityLieMatrix; -typedef NonlinearEquality NonlinearEqualityPoint2; -typedef NonlinearEquality NonlinearEqualityStereoPoint2; -typedef NonlinearEquality NonlinearEqualityPoint3; -typedef NonlinearEquality NonlinearEqualityRot2; -typedef NonlinearEquality NonlinearEqualityRot3; -typedef NonlinearEquality NonlinearEqualityPose2; -typedef NonlinearEquality NonlinearEqualityPose3; -typedef NonlinearEquality NonlinearEqualityCal3_S2; -typedef NonlinearEquality NonlinearEqualityCal3DS2; -typedef NonlinearEquality NonlinearEqualityCalibratedCamera; -typedef NonlinearEquality NonlinearEqualitySimpleCamera; -typedef NonlinearEquality NonlinearEqualityStereoCamera; +typedef NonlinearEquality NonlinearEqualityLieVector; +typedef NonlinearEquality NonlinearEqualityLieMatrix; +typedef NonlinearEquality NonlinearEqualityPoint2; +typedef NonlinearEquality NonlinearEqualityStereoPoint2; +typedef NonlinearEquality NonlinearEqualityPoint3; +typedef NonlinearEquality NonlinearEqualityRot2; +typedef NonlinearEquality NonlinearEqualityRot3; +typedef NonlinearEquality NonlinearEqualityPose2; +typedef NonlinearEquality NonlinearEqualityPose3; +typedef NonlinearEquality NonlinearEqualityCal3_S2; +typedef NonlinearEquality NonlinearEqualityCal3DS2; +typedef NonlinearEquality NonlinearEqualityCalibratedCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef NonlinearEquality NonlinearEqualityPinholeCameraCal3_S2; +typedef NonlinearEquality NonlinearEqualityStereoCamera; -typedef RangeFactor RangeFactor2D; -typedef RangeFactor RangeFactor3D; -typedef RangeFactor RangeFactorPose2; -typedef RangeFactor RangeFactorPose3; -typedef RangeFactor RangeFactorCalibratedCameraPoint; -typedef RangeFactor RangeFactorSimpleCameraPoint; -typedef RangeFactor RangeFactorCalibratedCamera; -typedef RangeFactor RangeFactorSimpleCamera; +typedef RangeFactor RangeFactor2D; +typedef RangeFactor RangeFactor3D; +typedef RangeFactor RangeFactorPose2; +typedef RangeFactor RangeFactorPose3; +typedef RangeFactor RangeFactorCalibratedCameraPoint; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorPinholeCameraCal3_S2Point; +typedef RangeFactor RangeFactorCalibratedCamera; +typedef RangeFactor RangeFactorSimpleCamera; +typedef RangeFactor RangeFactorPinholeCameraCal3_S2; typedef BearingRangeFactor BearingRangeFactor2D; typedef BearingRangeFactor BearingRangeFactor3D; @@ -85,7 +89,7 @@ typedef BearingRangeFactor BearingRangeFactor3D; typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; typedef gtsam::GeneralSFMFactor2 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 */ diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index a74bfc3c6..14ad43ae2 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -7,7 +7,7 @@ #include -#include +#include #include #include #include @@ -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 level_camera(level_pose, *K); typedef InvDepthFactor3 InverseDepthFactor; typedef NonlinearEquality 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 right_camera(right_pose, *K); // projection measurement of landmark into right camera // this measurement disagrees with the depth initialization diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 0ac2c24ee..14ac52c45 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -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 cam1(cameraPose1, *K); // with camera poses + PinholeCamera cam2(cameraPose2, *K); + PinholeCamera 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(); // diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 10fd5e142..035e7e509 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -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 diff --git a/matlab/+gtsam/VisualISAMGenerateData.m b/matlab/+gtsam/VisualISAMGenerateData.m index ab47e92db..57f9439a4 100644 --- a/matlab/+gtsam/VisualISAMGenerateData.m +++ b/matlab/+gtsam/VisualISAMGenerateData.m @@ -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 diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index 71f72f6b9..2b913b52d 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -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); diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 36b7635e2..add2bc75a 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -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; diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m index 7f50f2db8..584ace09a 100644 --- a/matlab/gtsam_examples/SBAExample.m +++ b/matlab/gtsam_examples/SBAExample.m @@ -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) diff --git a/matlab/gtsam_tests/testTriangulation.m b/matlab/gtsam_tests/testTriangulation.m index d46493328..7116d3838 100644 --- a/matlab/gtsam_tests/testTriangulation.m +++ b/matlab/gtsam_tests/testTriangulation.m @@ -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); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 2cceea753..195b7ff69 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -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) diff --git a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m index c9e912ea4..129b8c176 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m @@ -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 diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m index fd4a17469..4557d711f 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m @@ -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 diff --git a/matlab/unstable_examples/TransformProjectionFactorExample.m b/matlab/unstable_examples/TransformProjectionFactorExample.m index 669073e56..79a96209d 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExample.m +++ b/matlab/unstable_examples/TransformProjectionFactorExample.m @@ -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 diff --git a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m index 8edcfade7..ca5b70c62 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m @@ -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 diff --git a/matlab/unstable_examples/project_landmarks.m b/matlab/unstable_examples/project_landmarks.m index 629c6d994..aaccc9248 100644 --- a/matlab/unstable_examples/project_landmarks.m +++ b/matlab/unstable_examples/project_landmarks.m @@ -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 diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 95843e5ab..d59666655 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include @@ -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 leftCamera(poseLeft, K); Pose3 poseRight(faceTowardsY, Point3(2, 0, 0)); // 2 units to the right - SimpleCamera rightCamera(poseRight, K); + PinholeCamera rightCamera(poseRight, K); Point3 landmark(1, 5, 0); //centered between the cameras, 5 units away diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 64e111e94..9222894a4 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include #include @@ -57,20 +57,21 @@ using namespace gtsam; using namespace gtsam::serializationTestHelpers; // Creating as many permutations of factors as possible -typedef PriorFactor PriorFactorLieVector; -typedef PriorFactor PriorFactorLieMatrix; -typedef PriorFactor PriorFactorPoint2; -typedef PriorFactor PriorFactorStereoPoint2; -typedef PriorFactor PriorFactorPoint3; -typedef PriorFactor PriorFactorRot2; -typedef PriorFactor PriorFactorRot3; -typedef PriorFactor PriorFactorPose2; -typedef PriorFactor PriorFactorPose3; -typedef PriorFactor PriorFactorCal3_S2; -typedef PriorFactor PriorFactorCal3DS2; -typedef PriorFactor PriorFactorCalibratedCamera; -typedef PriorFactor PriorFactorSimpleCamera; -typedef PriorFactor PriorFactorStereoCamera; +typedef PriorFactor PriorFactorLieVector; +typedef PriorFactor PriorFactorLieMatrix; +typedef PriorFactor PriorFactorPoint2; +typedef PriorFactor PriorFactorStereoPoint2; +typedef PriorFactor PriorFactorPoint3; +typedef PriorFactor PriorFactorRot2; +typedef PriorFactor PriorFactorRot3; +typedef PriorFactor PriorFactorPose2; +typedef PriorFactor PriorFactorPose3; +typedef PriorFactor PriorFactorCal3_S2; +typedef PriorFactor PriorFactorCal3DS2; +typedef PriorFactor PriorFactorCalibratedCamera; +typedef PriorFactor PriorFactorSimpleCamera; +typedef PriorFactor PriorFactorPinholeCameraCal3_S2; +typedef PriorFactor PriorFactorStereoCamera; typedef BetweenFactor BetweenFactorLieVector; typedef BetweenFactor BetweenFactorLieMatrix; @@ -81,29 +82,32 @@ typedef BetweenFactor BetweenFactorRot3; typedef BetweenFactor BetweenFactorPose2; typedef BetweenFactor BetweenFactorPose3; -typedef NonlinearEquality NonlinearEqualityLieVector; -typedef NonlinearEquality NonlinearEqualityLieMatrix; -typedef NonlinearEquality NonlinearEqualityPoint2; -typedef NonlinearEquality NonlinearEqualityStereoPoint2; -typedef NonlinearEquality NonlinearEqualityPoint3; -typedef NonlinearEquality NonlinearEqualityRot2; -typedef NonlinearEquality NonlinearEqualityRot3; -typedef NonlinearEquality NonlinearEqualityPose2; -typedef NonlinearEquality NonlinearEqualityPose3; -typedef NonlinearEquality NonlinearEqualityCal3_S2; -typedef NonlinearEquality NonlinearEqualityCal3DS2; -typedef NonlinearEquality NonlinearEqualityCalibratedCamera; -typedef NonlinearEquality NonlinearEqualitySimpleCamera; -typedef NonlinearEquality NonlinearEqualityStereoCamera; +typedef NonlinearEquality NonlinearEqualityLieVector; +typedef NonlinearEquality NonlinearEqualityLieMatrix; +typedef NonlinearEquality NonlinearEqualityPoint2; +typedef NonlinearEquality NonlinearEqualityStereoPoint2; +typedef NonlinearEquality NonlinearEqualityPoint3; +typedef NonlinearEquality NonlinearEqualityRot2; +typedef NonlinearEquality NonlinearEqualityRot3; +typedef NonlinearEquality NonlinearEqualityPose2; +typedef NonlinearEquality NonlinearEqualityPose3; +typedef NonlinearEquality NonlinearEqualityCal3_S2; +typedef NonlinearEquality NonlinearEqualityCal3DS2; +typedef NonlinearEquality NonlinearEqualityCalibratedCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef NonlinearEquality NonlinearEqualityPinholeCameraCal3_S2; +typedef NonlinearEquality NonlinearEqualityStereoCamera; -typedef RangeFactor RangeFactor2D; -typedef RangeFactor RangeFactor3D; -typedef RangeFactor RangeFactorPose2; -typedef RangeFactor RangeFactorPose3; -typedef RangeFactor RangeFactorCalibratedCameraPoint; -typedef RangeFactor RangeFactorSimpleCameraPoint; -typedef RangeFactor RangeFactorCalibratedCamera; -typedef RangeFactor RangeFactorSimpleCamera; +typedef RangeFactor RangeFactor2D; +typedef RangeFactor RangeFactor3D; +typedef RangeFactor RangeFactorPose2; +typedef RangeFactor RangeFactorPose3; +typedef RangeFactor RangeFactorCalibratedCameraPoint; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorPinholeCameraCal3_S2Point; +typedef RangeFactor RangeFactorCalibratedCamera; +typedef RangeFactor RangeFactorSimpleCamera; +typedef RangeFactor RangeFactorPinholeCameraCal3_S2; typedef BearingRangeFactor BearingRangeFactor2D; typedef BearingRangeFactor BearingRangeFactor3D; @@ -111,7 +115,7 @@ typedef BearingRangeFactor BearingRangeFactor3D; typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; typedef gtsam::GeneralSFMFactor2 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 simpleCamera(pose3, cal3_s2); StereoCamera stereoCamera(pose3, boost::make_shared(cal3_s2stereo)); diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp index 9c63e1aa8..182408004 100644 --- a/tests/testVisualISAM2.cpp +++ b/tests/testVisualISAM2.cpp @@ -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 camera(poses[i], *K); Point2 measurement = camera.project(points[j]); graph.emplace_shared>( measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);