Inherit constructors for CameraSets to switch to initializer lists.
Frank Dellaert
parent
c4fb764299
commit
d2f0cf5cc7
|
|
@ -35,56 +35,56 @@ template<class CAMERA>
|
|||
class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> > {
|
||||
|
||||
protected:
|
||||
using Base = std::vector<CAMERA, typename Eigen::aligned_allocator<CAMERA>>;
|
||||
|
||||
/**
|
||||
* 2D measurement and noise model for each of the m views
|
||||
* The order is kept the same as the keys that we use to create the factor.
|
||||
*/
|
||||
typedef typename CAMERA::Measurement Z;
|
||||
typedef typename CAMERA::MeasurementVector ZVector;
|
||||
/**
|
||||
* 2D measurement and noise model for each of the m views
|
||||
* The order is kept the same as the keys that we use to create the factor.
|
||||
*/
|
||||
typedef typename CAMERA::Measurement Z;
|
||||
typedef typename CAMERA::MeasurementVector ZVector;
|
||||
|
||||
static const int D = traits<CAMERA>::dimension; ///< Camera dimension
|
||||
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
|
||||
static const int D = traits<CAMERA>::dimension; ///< Camera dimension
|
||||
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
|
||||
|
||||
/// Make a vector of re-projection errors
|
||||
static Vector ErrorVector(const ZVector& predicted,
|
||||
const ZVector& measured) {
|
||||
/// Make a vector of re-projection errors
|
||||
static Vector ErrorVector(const ZVector& predicted, const ZVector& measured) {
|
||||
// Check size
|
||||
size_t m = predicted.size();
|
||||
if (measured.size() != m)
|
||||
throw std::runtime_error("CameraSet::errors: size mismatch");
|
||||
|
||||
// Check size
|
||||
size_t m = predicted.size();
|
||||
if (measured.size() != m)
|
||||
throw std::runtime_error("CameraSet::errors: size mismatch");
|
||||
|
||||
// Project and fill error vector
|
||||
Vector b(ZDim * m);
|
||||
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
|
||||
Vector bi = traits<Z>::Local(measured[i], predicted[i]);
|
||||
if(ZDim==3 && std::isnan(bi(1))){ // if it is a stereo point and the right pixel is missing (nan)
|
||||
bi(1) = 0;
|
||||
}
|
||||
b.segment<ZDim>(row) = bi;
|
||||
}
|
||||
return b;
|
||||
// Project and fill error vector
|
||||
Vector b(ZDim * m);
|
||||
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
|
||||
Vector bi = traits<Z>::Local(measured[i], predicted[i]);
|
||||
if (ZDim == 3 && std::isnan(bi(1))) { // if it is a stereo point and the
|
||||
// right pixel is missing (nan)
|
||||
bi(1) = 0;
|
||||
}
|
||||
b.segment<ZDim>(row) = bi;
|
||||
}
|
||||
return b;
|
||||
}
|
||||
|
||||
public:
|
||||
using Base::Base; // Inherit the vector constructors
|
||||
|
||||
/// Destructor
|
||||
virtual ~CameraSet() = default;
|
||||
/// Destructor
|
||||
virtual ~CameraSet() = default;
|
||||
|
||||
/// Definitions for blocks of F
|
||||
using MatrixZD = Eigen::Matrix<double, ZDim, D>;
|
||||
using FBlocks = std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>;
|
||||
/// Definitions for blocks of F
|
||||
using MatrixZD = Eigen::Matrix<double, ZDim, D>;
|
||||
using FBlocks = std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>;
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter useful for printing Symbols
|
||||
*/
|
||||
virtual void print(const std::string& s = "") const {
|
||||
std::cout << s << "CameraSet, cameras = \n";
|
||||
for (size_t k = 0; k < this->size(); ++k)
|
||||
this->at(k).print(s);
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter useful for printing Symbols
|
||||
*/
|
||||
virtual void print(const std::string& s = "") const {
|
||||
std::cout << s << "CameraSet, cameras = \n";
|
||||
for (size_t k = 0; k < this->size(); ++k) this->at(k).print(s);
|
||||
}
|
||||
|
||||
/// equals
|
||||
|
|
|
|||
|
|
@ -30,12 +30,8 @@
|
|||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/slam/StereoFactor.h>
|
||||
|
||||
#include <boost/assign.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace boost::assign;
|
||||
|
||||
// Some common constants
|
||||
|
||||
|
|
@ -51,34 +47,34 @@ static const PinholeCamera<Cal3_S2> kCamera1(kPose1, *kSharedCal);
|
|||
static const Pose3 kPose2 = kPose1 * Pose3(Rot3(), Point3(1, 0, 0));
|
||||
static const PinholeCamera<Cal3_S2> kCamera2(kPose2, *kSharedCal);
|
||||
|
||||
// landmark ~5 meters infront of camera
|
||||
static const std::vector<Pose3> kPoses = {kPose1, kPose2};
|
||||
|
||||
|
||||
// landmark ~5 meters in front of camera
|
||||
static const Point3 kLandmark(5, 0.5, 1.2);
|
||||
|
||||
// 1. Project two landmarks into two cameras and triangulate
|
||||
static const Point2 kZ1 = kCamera1.project(kLandmark);
|
||||
static const Point2 kZ2 = kCamera2.project(kLandmark);
|
||||
static const Point2Vector kMeasurements{kZ1, kZ2};
|
||||
|
||||
//******************************************************************************
|
||||
// Simple test with a well-behaved two camera situation
|
||||
TEST(triangulation, twoPoses) {
|
||||
vector<Pose3> poses;
|
||||
Point2Vector measurements;
|
||||
|
||||
poses += kPose1, kPose2;
|
||||
measurements += kZ1, kZ2;
|
||||
Point2Vector measurements = kMeasurements;
|
||||
|
||||
double rank_tol = 1e-9;
|
||||
|
||||
// 1. Test simple DLT, perfect in no noise situation
|
||||
bool optimize = false;
|
||||
boost::optional<Point3> actual1 = //
|
||||
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
|
||||
triangulatePoint3<Cal3_S2>(kPoses, kSharedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
|
||||
|
||||
// 2. test with optimization on, same answer
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual2 = //
|
||||
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
|
||||
triangulatePoint3<Cal3_S2>(kPoses, kSharedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
|
||||
|
||||
// 3. Add some noise and try again: result should be ~ (4.995,
|
||||
|
|
@ -87,13 +83,13 @@ TEST(triangulation, twoPoses) {
|
|||
measurements.at(1) += Point2(-0.2, 0.3);
|
||||
optimize = false;
|
||||
boost::optional<Point3> actual3 = //
|
||||
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
|
||||
triangulatePoint3<Cal3_S2>(kPoses, kSharedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4));
|
||||
|
||||
// 4. Now with optimization on
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual4 = //
|
||||
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
|
||||
triangulatePoint3<Cal3_S2>(kPoses, kSharedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
|
||||
}
|
||||
|
||||
|
|
@ -102,7 +98,7 @@ TEST(triangulation, twoCamerasUsingLOST) {
|
|||
cameras.push_back(kCamera1);
|
||||
cameras.push_back(kCamera2);
|
||||
|
||||
Point2Vector measurements = {kZ1, kZ2};
|
||||
Point2Vector measurements = kMeasurements;
|
||||
SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4);
|
||||
double rank_tol = 1e-9;
|
||||
|
||||
|
|
@ -175,25 +171,21 @@ TEST(triangulation, twoPosesCal3DS2) {
|
|||
Point2 z1Distorted = camera1Distorted.project(kLandmark);
|
||||
Point2 z2Distorted = camera2Distorted.project(kLandmark);
|
||||
|
||||
vector<Pose3> poses;
|
||||
Point2Vector measurements;
|
||||
|
||||
poses += kPose1, kPose2;
|
||||
measurements += z1Distorted, z2Distorted;
|
||||
Point2Vector measurements{z1Distorted, z2Distorted};
|
||||
|
||||
double rank_tol = 1e-9;
|
||||
|
||||
// 1. Test simple DLT, perfect in no noise situation
|
||||
bool optimize = false;
|
||||
boost::optional<Point3> actual1 = //
|
||||
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
|
||||
triangulatePoint3<Cal3DS2>(kPoses, sharedDistortedCal, measurements,
|
||||
rank_tol, optimize);
|
||||
EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
|
||||
|
||||
// 2. test with optimization on, same answer
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual2 = //
|
||||
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
|
||||
triangulatePoint3<Cal3DS2>(kPoses, sharedDistortedCal, measurements,
|
||||
rank_tol, optimize);
|
||||
EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
|
||||
|
||||
|
|
@ -203,14 +195,14 @@ TEST(triangulation, twoPosesCal3DS2) {
|
|||
measurements.at(1) += Point2(-0.2, 0.3);
|
||||
optimize = false;
|
||||
boost::optional<Point3> actual3 = //
|
||||
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
|
||||
triangulatePoint3<Cal3DS2>(kPoses, sharedDistortedCal, measurements,
|
||||
rank_tol, optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
|
||||
|
||||
// 4. Now with optimization on
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual4 = //
|
||||
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
|
||||
triangulatePoint3<Cal3DS2>(kPoses, sharedDistortedCal, measurements,
|
||||
rank_tol, optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
|
||||
}
|
||||
|
|
@ -232,25 +224,21 @@ TEST(triangulation, twoPosesFisheye) {
|
|||
Point2 z1Distorted = camera1Distorted.project(kLandmark);
|
||||
Point2 z2Distorted = camera2Distorted.project(kLandmark);
|
||||
|
||||
vector<Pose3> poses;
|
||||
Point2Vector measurements;
|
||||
|
||||
poses += kPose1, kPose2;
|
||||
measurements += z1Distorted, z2Distorted;
|
||||
Point2Vector measurements{z1Distorted, z2Distorted};
|
||||
|
||||
double rank_tol = 1e-9;
|
||||
|
||||
// 1. Test simple DLT, perfect in no noise situation
|
||||
bool optimize = false;
|
||||
boost::optional<Point3> actual1 = //
|
||||
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
|
||||
triangulatePoint3<Calibration>(kPoses, sharedDistortedCal, measurements,
|
||||
rank_tol, optimize);
|
||||
EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
|
||||
|
||||
// 2. test with optimization on, same answer
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual2 = //
|
||||
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
|
||||
triangulatePoint3<Calibration>(kPoses, sharedDistortedCal, measurements,
|
||||
rank_tol, optimize);
|
||||
EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
|
||||
|
||||
|
|
@ -260,14 +248,14 @@ TEST(triangulation, twoPosesFisheye) {
|
|||
measurements.at(1) += Point2(-0.2, 0.3);
|
||||
optimize = false;
|
||||
boost::optional<Point3> actual3 = //
|
||||
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
|
||||
triangulatePoint3<Calibration>(kPoses, sharedDistortedCal, measurements,
|
||||
rank_tol, optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
|
||||
|
||||
// 4. Now with optimization on
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual4 = //
|
||||
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
|
||||
triangulatePoint3<Calibration>(kPoses, sharedDistortedCal, measurements,
|
||||
rank_tol, optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
|
||||
}
|
||||
|
|
@ -284,17 +272,13 @@ TEST(triangulation, twoPosesBundler) {
|
|||
Point2 z1 = camera1.project(kLandmark);
|
||||
Point2 z2 = camera2.project(kLandmark);
|
||||
|
||||
vector<Pose3> poses;
|
||||
Point2Vector measurements;
|
||||
|
||||
poses += kPose1, kPose2;
|
||||
measurements += z1, z2;
|
||||
Point2Vector measurements{z1, z2};
|
||||
|
||||
bool optimize = true;
|
||||
double rank_tol = 1e-9;
|
||||
|
||||
boost::optional<Point3> actual = //
|
||||
triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol,
|
||||
triangulatePoint3<Cal3Bundler>(kPoses, bundlerCal, measurements, rank_tol,
|
||||
optimize);
|
||||
EXPECT(assert_equal(kLandmark, *actual, 1e-7));
|
||||
|
||||
|
|
@ -303,19 +287,15 @@ TEST(triangulation, twoPosesBundler) {
|
|||
measurements.at(1) += Point2(-0.2, 0.3);
|
||||
|
||||
boost::optional<Point3> actual2 = //
|
||||
triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol,
|
||||
triangulatePoint3<Cal3Bundler>(kPoses, bundlerCal, measurements, rank_tol,
|
||||
optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(triangulation, fourPoses) {
|
||||
vector<Pose3> poses;
|
||||
Point2Vector measurements;
|
||||
|
||||
poses += kPose1, kPose2;
|
||||
measurements += kZ1, kZ2;
|
||||
|
||||
Pose3Vector poses = kPoses;
|
||||
Point2Vector measurements = kMeasurements;
|
||||
boost::optional<Point3> actual =
|
||||
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
|
||||
EXPECT(assert_equal(kLandmark, *actual, 1e-2));
|
||||
|
|
@ -334,8 +314,8 @@ TEST(triangulation, fourPoses) {
|
|||
PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
|
||||
Point2 z3 = camera3.project(kLandmark);
|
||||
|
||||
poses += pose3;
|
||||
measurements += z3 + Point2(0.1, -0.1);
|
||||
poses.push_back(pose3);
|
||||
measurements.push_back(z3 + Point2(0.1, -0.1));
|
||||
|
||||
boost::optional<Point3> triangulated_3cameras = //
|
||||
triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
|
||||
|
|
@ -353,8 +333,8 @@ TEST(triangulation, fourPoses) {
|
|||
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
|
||||
CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException);
|
||||
|
||||
poses += pose4;
|
||||
measurements += Point2(400, 400);
|
||||
poses.push_back(pose4);
|
||||
measurements.emplace_back(400, 400);
|
||||
|
||||
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
|
||||
TriangulationCheiralityException);
|
||||
|
|
@ -368,10 +348,8 @@ TEST(triangulation, threePoses_robustNoiseModel) {
|
|||
PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
|
||||
Point2 z3 = camera3.project(kLandmark);
|
||||
|
||||
vector<Pose3> poses;
|
||||
Point2Vector measurements;
|
||||
poses += kPose1, kPose2, pose3;
|
||||
measurements += kZ1, kZ2, z3;
|
||||
const vector<Pose3> poses{kPose1, kPose2, pose3};
|
||||
Point2Vector measurements{kZ1, kZ2, z3};
|
||||
|
||||
// noise free, so should give exactly the landmark
|
||||
boost::optional<Point3> actual =
|
||||
|
|
@ -410,10 +388,9 @@ TEST(triangulation, fourPoses_robustNoiseModel) {
|
|||
PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
|
||||
Point2 z3 = camera3.project(kLandmark);
|
||||
|
||||
vector<Pose3> poses;
|
||||
Point2Vector measurements;
|
||||
poses += kPose1, kPose1, kPose2, pose3; // 2 measurements from pose 1
|
||||
measurements += kZ1, kZ1, kZ2, z3;
|
||||
const vector<Pose3> poses{kPose1, kPose1, kPose2, pose3};
|
||||
// 2 measurements from pose 1:
|
||||
Point2Vector measurements{kZ1, kZ1, kZ2, z3};
|
||||
|
||||
// noise free, so should give exactly the landmark
|
||||
boost::optional<Point3> actual =
|
||||
|
|
@ -463,11 +440,8 @@ TEST(triangulation, fourPoses_distinct_Ks) {
|
|||
Point2 z1 = camera1.project(kLandmark);
|
||||
Point2 z2 = camera2.project(kLandmark);
|
||||
|
||||
CameraSet<PinholeCamera<Cal3_S2>> cameras;
|
||||
Point2Vector measurements;
|
||||
|
||||
cameras += camera1, camera2;
|
||||
measurements += z1, z2;
|
||||
CameraSet<PinholeCamera<Cal3_S2>> cameras{camera1, camera2};
|
||||
Point2Vector measurements{z1, z2};
|
||||
|
||||
boost::optional<Point3> actual = //
|
||||
triangulatePoint3<Cal3_S2>(cameras, measurements);
|
||||
|
|
@ -488,8 +462,8 @@ TEST(triangulation, fourPoses_distinct_Ks) {
|
|||
PinholeCamera<Cal3_S2> camera3(pose3, K3);
|
||||
Point2 z3 = camera3.project(kLandmark);
|
||||
|
||||
cameras += camera3;
|
||||
measurements += z3 + Point2(0.1, -0.1);
|
||||
cameras.push_back(camera3);
|
||||
measurements.push_back(z3 + Point2(0.1, -0.1));
|
||||
|
||||
boost::optional<Point3> triangulated_3cameras = //
|
||||
triangulatePoint3<Cal3_S2>(cameras, measurements);
|
||||
|
|
@ -508,8 +482,8 @@ TEST(triangulation, fourPoses_distinct_Ks) {
|
|||
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
|
||||
CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException);
|
||||
|
||||
cameras += camera4;
|
||||
measurements += Point2(400, 400);
|
||||
cameras.push_back(camera4);
|
||||
measurements.emplace_back(400, 400);
|
||||
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(cameras, measurements),
|
||||
TriangulationCheiralityException);
|
||||
#endif
|
||||
|
|
@ -529,11 +503,8 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) {
|
|||
Point2 z1 = camera1.project(kLandmark);
|
||||
Point2 z2 = camera2.project(kLandmark);
|
||||
|
||||
CameraSet<PinholeCamera<Cal3DS2>> cameras;
|
||||
Point2Vector measurements;
|
||||
|
||||
cameras += camera1, camera2;
|
||||
measurements += z1, z2;
|
||||
const CameraSet<PinholeCamera<Cal3DS2>> cameras{camera1, camera2};
|
||||
const Point2Vector measurements{z1, z2};
|
||||
|
||||
boost::optional<Point3> actual = //
|
||||
triangulatePoint3<Cal3DS2>(cameras, measurements);
|
||||
|
|
@ -554,11 +525,8 @@ TEST(triangulation, outliersAndFarLandmarks) {
|
|||
Point2 z1 = camera1.project(kLandmark);
|
||||
Point2 z2 = camera2.project(kLandmark);
|
||||
|
||||
CameraSet<PinholeCamera<Cal3_S2>> cameras;
|
||||
Point2Vector measurements;
|
||||
|
||||
cameras += camera1, camera2;
|
||||
measurements += z1, z2;
|
||||
CameraSet<PinholeCamera<Cal3_S2>> cameras{camera1, camera2};
|
||||
Point2Vector measurements{z1, z2};
|
||||
|
||||
double landmarkDistanceThreshold = 10; // landmark is closer than that
|
||||
TriangulationParameters params(
|
||||
|
|
@ -582,8 +550,8 @@ TEST(triangulation, outliersAndFarLandmarks) {
|
|||
PinholeCamera<Cal3_S2> camera3(pose3, K3);
|
||||
Point2 z3 = camera3.project(kLandmark);
|
||||
|
||||
cameras += camera3;
|
||||
measurements += z3 + Point2(10, -10);
|
||||
cameras.push_back(camera3);
|
||||
measurements.push_back(z3 + Point2(10, -10));
|
||||
|
||||
landmarkDistanceThreshold = 10; // landmark is closer than that
|
||||
double outlierThreshold = 100; // loose, the outlier is going to pass
|
||||
|
|
@ -608,11 +576,8 @@ TEST(triangulation, twoIdenticalPoses) {
|
|||
// 1. Project two landmarks into two cameras and triangulate
|
||||
Point2 z1 = camera1.project(kLandmark);
|
||||
|
||||
vector<Pose3> poses;
|
||||
Point2Vector measurements;
|
||||
|
||||
poses += kPose1, kPose1;
|
||||
measurements += z1, z1;
|
||||
const vector<Pose3> poses{kPose1, kPose1};
|
||||
const Point2Vector measurements{z1, z1};
|
||||
|
||||
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
|
||||
TriangulationUnderconstrainedException);
|
||||
|
|
@ -623,22 +588,19 @@ TEST(triangulation, onePose) {
|
|||
// we expect this test to fail with a TriangulationUnderconstrainedException
|
||||
// because there's only one camera observation
|
||||
|
||||
vector<Pose3> poses;
|
||||
Point2Vector measurements;
|
||||
|
||||
poses += Pose3();
|
||||
measurements += Point2(0, 0);
|
||||
const vector<Pose3> poses{Pose3()};
|
||||
const Point2Vector measurements {{0,0}};
|
||||
|
||||
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
|
||||
TriangulationUnderconstrainedException);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(triangulation, StereotriangulateNonlinear) {
|
||||
TEST(triangulation, StereoTriangulateNonlinear) {
|
||||
auto stereoK = boost::make_shared<Cal3_S2Stereo>(1733.75, 1733.75, 0, 689.645,
|
||||
508.835, 0.0699612);
|
||||
|
||||
// two camera poses m1, m2
|
||||
// two camera kPoses m1, m2
|
||||
Matrix4 m1, m2;
|
||||
m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779, 0.592783835,
|
||||
-0.77156583, 0.230856632, 66.2186159, 0.116517574, -0.201470143,
|
||||
|
|
@ -648,14 +610,12 @@ TEST(triangulation, StereotriangulateNonlinear) {
|
|||
0.947083213, 0.131587097, 65.843136, -0.0206094928, 0.131334858,
|
||||
-0.991123524, -4.3525033, 0, 0, 0, 1;
|
||||
|
||||
typedef CameraSet<StereoCamera> Cameras;
|
||||
Cameras cameras;
|
||||
cameras.push_back(StereoCamera(Pose3(m1), stereoK));
|
||||
cameras.push_back(StereoCamera(Pose3(m2), stereoK));
|
||||
typedef CameraSet<StereoCamera> StereoCameras;
|
||||
const StereoCameras cameras{{Pose3(m1), stereoK}, {Pose3(m2), stereoK}};
|
||||
|
||||
StereoPoint2Vector measurements;
|
||||
measurements += StereoPoint2(226.936, 175.212, 424.469);
|
||||
measurements += StereoPoint2(339.571, 285.547, 669.973);
|
||||
measurements.push_back(StereoPoint2(226.936, 175.212, 424.469));
|
||||
measurements.push_back(StereoPoint2(339.571, 285.547, 669.973));
|
||||
|
||||
Point3 initial =
|
||||
Point3(46.0536958, 66.4621179, -6.56285929); // error: 96.5715555191
|
||||
|
|
@ -741,8 +701,6 @@ TEST(triangulation, StereotriangulateNonlinear) {
|
|||
//******************************************************************************
|
||||
// Simple test with a well-behaved two camera situation
|
||||
TEST(triangulation, twoPoses_sphericalCamera) {
|
||||
vector<Pose3> poses;
|
||||
std::vector<Unit3> measurements;
|
||||
|
||||
// Project landmark into two cameras and triangulate
|
||||
SphericalCamera cam1(kPose1);
|
||||
|
|
@ -750,8 +708,7 @@ TEST(triangulation, twoPoses_sphericalCamera) {
|
|||
Unit3 u1 = cam1.project(kLandmark);
|
||||
Unit3 u2 = cam2.project(kLandmark);
|
||||
|
||||
poses += kPose1, kPose2;
|
||||
measurements += u1, u2;
|
||||
std::vector<Unit3> measurements{u1, u2};
|
||||
|
||||
CameraSet<SphericalCamera> cameras;
|
||||
cameras.push_back(cam1);
|
||||
|
|
@ -803,9 +760,6 @@ TEST(triangulation, twoPoses_sphericalCamera) {
|
|||
|
||||
//******************************************************************************
|
||||
TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) {
|
||||
vector<Pose3> poses;
|
||||
std::vector<Unit3> measurements;
|
||||
|
||||
// Project landmark into two cameras and triangulate
|
||||
Pose3 poseA = Pose3(
|
||||
Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
|
||||
|
|
@ -825,8 +779,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) {
|
|||
EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2,
|
||||
1e-7)); // behind and to the right of PoseB
|
||||
|
||||
poses += kPose1, kPose2;
|
||||
measurements += u1, u2;
|
||||
const std::vector<Unit3> measurements{u1, u2};
|
||||
|
||||
CameraSet<SphericalCamera> cameras;
|
||||
cameras.push_back(cam1);
|
||||
|
|
|
|||
Loading…
Reference in New Issue