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

applied formatting to modified files

release/4.3a0
lcarlone 2021-11-07 16:31:48 -05:00
parent 78a8b7dc0e
commit 78a4075a54
7 changed files with 458 additions and 420 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
gtsam/geometry/SphericalCamera.cpp
View File

@ -23,14 +23,12 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
bool SphericalCamera::equals(const SphericalCamera &camera, double tol) const {
bool SphericalCamera::equals(const SphericalCamera& camera, double tol) const {
return pose_.equals(camera.pose(), tol);
}
/* ************************************************************************* */
void SphericalCamera::print(const string& s) const {
pose_.print(s + ".pose");
}
void SphericalCamera::print(const string& s) const { pose_.print(s + ".pose"); }
/* ************************************************************************* */
pair<Unit3, bool> SphericalCamera::projectSafe(const Point3& pw) const {
@ -41,37 +39,33 @@ pair<Unit3, bool> SphericalCamera::projectSafe(const Point3& pw) const {
/* ************************************************************************* */
Unit3 SphericalCamera::project2(const Point3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const {
OptionalJacobian<2, 3> Dpoint) const {
Matrix36 Dtf_pose;
Matrix3 Dtf_point; // calculated by transformTo if needed
const Point3 pc = pose().transformTo(pw, Dpose ? &Dtf_pose : 0, Dpoint ? &Dtf_point : 0);
Matrix3 Dtf_point; // calculated by transformTo if needed
const Point3 pc =
pose().transformTo(pw, Dpose ? &Dtf_pose : 0, Dpoint ? &Dtf_point : 0);
if (pc.norm() <= 1e-8)
throw("point cannot be at the center of the camera");
if (pc.norm() <= 1e-8) throw("point cannot be at the center of the camera");
Matrix23 Dunit; // calculated by FromPoint3 if needed
Matrix23 Dunit; // calculated by FromPoint3 if needed
Unit3 pu = Unit3::FromPoint3(Point3(pc), Dpoint ? &Dunit : 0);
if (Dpose)
*Dpose = Dunit * Dtf_pose; //2x3 * 3x6 = 2x6
if (Dpoint)
*Dpoint = Dunit * Dtf_point; //2x3 * 3x3 = 2x3
if (Dpose) *Dpose = Dunit * Dtf_pose; // 2x3 * 3x6 = 2x6
if (Dpoint) *Dpoint = Dunit * Dtf_point; // 2x3 * 3x3 = 2x3
return pu;
}
/* ************************************************************************* */
Unit3 SphericalCamera::project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 2> Dpoint) const {
OptionalJacobian<2, 2> Dpoint) const {
Matrix23 Dtf_rot;
Matrix2 Dtf_point; // calculated by transformTo if needed
const Unit3 pu = pose().rotation().unrotate(pwu, Dpose ? &Dtf_rot : 0, Dpoint ? &Dtf_point : 0);
Matrix2 Dtf_point; // calculated by transformTo if needed
const Unit3 pu = pose().rotation().unrotate(pwu, Dpose ? &Dtf_rot : 0,
Dpoint ? &Dtf_point : 0);
if (Dpose)
*Dpose << Dtf_rot, Matrix::Zero(2,3); //2x6 (translation part is zero)
if (Dpoint)
*Dpoint = Dtf_point; //2x2
*Dpose << Dtf_rot, Matrix::Zero(2, 3); // 2x6 (translation part is zero)
if (Dpoint) *Dpoint = Dtf_point; // 2x2
return pu;
}
@ -87,7 +81,8 @@ Unit3 SphericalCamera::backprojectPointAtInfinity(const Unit3& p) const {
/* ************************************************************************* */
Unit3 SphericalCamera::project(const Point3& point,
OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
OptionalJacobian<2, 6> Dcamera,
OptionalJacobian<2, 3> Dpoint) const {
return project2(point, Dcamera, Dpoint);
}
@ -102,10 +97,8 @@ Vector2 SphericalCamera::reprojectionError(
Matrix22 H_error;
Unit3 projected = project2(point, H_project_pose, H_project_point);
Vector2 error = measured.errorVector(projected, boost::none, H_error);
if (Dpose)
*Dpose = H_error * H_project_pose;
if (Dpoint)
*Dpoint = H_error * H_project_point;
if (Dpose) *Dpose = H_error * H_project_pose;
if (Dpoint) *Dpoint = H_error * H_project_point;
return error;
} else {
return measured.errorVector(project2(point, Dpose, Dpoint));
@ -113,4 +106,4 @@ Vector2 SphericalCamera::reprojectionError(
}
/* ************************************************************************* */
}
} // namespace gtsam

90
gtsam/geometry/SphericalCamera.h
View File

@ -18,13 +18,14 @@
#pragma once
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/concepts.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/base/concepts.h>
#include <gtsam/dllexport.h>
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
@ -32,11 +33,11 @@ namespace gtsam {
class GTSAM_EXPORT EmptyCal {
public:
enum { dimension = 0 };
EmptyCal(){}
EmptyCal() {}
virtual ~EmptyCal() = default;
using shared_ptr = boost::shared_ptr<EmptyCal>;
void print(const std::string& s) const {
std::cout << "empty calibration: " << s << std::endl;
std::cout << "empty calibration: " << s << std::endl;
}
};
@ -46,56 +47,41 @@ class GTSAM_EXPORT EmptyCal {
* \nosubgrouping
*/
class GTSAM_EXPORT SphericalCamera {
public:
enum {
dimension = 6
};
enum { dimension = 6 };
typedef Unit3 Measurement;
typedef std::vector<Unit3> MeasurementVector;
typedef EmptyCal CalibrationType;
private:
Pose3 pose_; ///< 3D pose of camera
protected:
EmptyCal::shared_ptr emptyCal_;
public:
/// @}
/// @name Standard Constructors
/// @{
/// Default constructor
SphericalCamera()
: pose_(Pose3::identity()),
emptyCal_(boost::make_shared<EmptyCal>()) {
}
: pose_(Pose3::identity()), emptyCal_(boost::make_shared<EmptyCal>()) {}
/// Constructor with pose
explicit SphericalCamera(const Pose3& pose)
: pose_(pose),
emptyCal_(boost::make_shared<EmptyCal>()) {
}
: pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
/// Constructor with empty intrinsics (needed for smart factors)
explicit SphericalCamera(const Pose3& pose,
const boost::shared_ptr<EmptyCal>& cal)
: pose_(pose),
emptyCal_(boost::make_shared<EmptyCal>()) {
}
: pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
/// @}
/// @name Advanced Constructors
/// @{
explicit SphericalCamera(const Vector& v)
: pose_(Pose3::Expmap(v)) {
}
explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {}
/// Default destructor
virtual ~SphericalCamera() = default;
@ -106,16 +92,14 @@ class GTSAM_EXPORT SphericalCamera {
}
/// return calibration
const EmptyCal& calibration() const {
return *emptyCal_;
}
const EmptyCal& calibration() const { return *emptyCal_; }
/// @}
/// @name Testable
/// @{
/// assert equality up to a tolerance
bool equals(const SphericalCamera &camera, double tol = 1e-9) const;
bool equals(const SphericalCamera& camera, double tol = 1e-9) const;
/// print
virtual void print(const std::string& s = "SphericalCamera") const;
@ -125,19 +109,13 @@ class GTSAM_EXPORT SphericalCamera {
/// @{
/// return pose, constant version
const Pose3& pose() const {
return pose_;
}
const Pose3& pose() const { return pose_; }
/// get rotation
const Rot3& rotation() const {
return pose_.rotation();
}
const Rot3& rotation() const { return pose_.rotation(); }
/// get translation
const Point3& translation() const {
return pose_.translation();
}
const Point3& translation() const { return pose_.translation(); }
// /// return pose, with derivative
// const Pose3& getPose(OptionalJacobian<6, 6> H) const;
@ -200,7 +178,8 @@ class GTSAM_EXPORT SphericalCamera {
/// for Canonical
static SphericalCamera identity() {
return SphericalCamera(Pose3::identity()); // assumes that the default constructor is valid
return SphericalCamera(
Pose3::identity()); // assumes that the default constructor is valid
}
/// for Linear Triangulation
@ -210,36 +189,29 @@ class GTSAM_EXPORT SphericalCamera {
/// for Nonlinear Triangulation
Vector defaultErrorWhenTriangulatingBehindCamera() const {
return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(0.0);
return Eigen::Matrix<double, traits<Point2>::dimension, 1>::Constant(0.0);
}
/// @deprecated
size_t dim() const {
return 6;
}
size_t dim() const { return 6; }
/// @deprecated
static size_t Dim() {
return 6;
}
/// @deprecated
static size_t Dim() { return 6; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(pose_);
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(pose_);
}
};
// end of class SphericalCamera
template<>
struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {
};
template <>
struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {};
template<>
struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {
};
template <>
struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {};
}
} // namespace gtsam

116
gtsam/geometry/tests/testSphericalCamera.cpp
View File

@ -16,11 +16,10 @@
* @author Luca Carlone
*/
#include <gtsam/geometry/SphericalCamera.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/SphericalCamera.h>
#include <cmath>
#include <iostream>
@ -31,64 +30,65 @@ using namespace gtsam;
typedef SphericalCamera Camera;
//static const Cal3_S2 K(625, 625, 0, 0, 0);
// static const Cal3_S2 K(625, 625, 0, 0, 0);
//
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()),
Point3(0, 0, 0.5));
static const Camera camera(pose);
//
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
static const Camera camera1(pose1);
static const Point3 point1(-0.08,-0.08, 0.0);
static const Point3 point1(-0.08, -0.08, 0.0);
static const Point3 point2(-0.08, 0.08, 0.0);
static const Point3 point3( 0.08, 0.08, 0.0);
static const Point3 point4( 0.08,-0.08, 0.0);
static const Point3 point3(0.08, 0.08, 0.0);
static const Point3 point4(0.08, -0.08, 0.0);
// manually computed in matlab
static const Unit3 bearing1(-0.156054862928174, 0.156054862928174, 0.975342893301088);
static const Unit3 bearing2(-0.156054862928174,-0.156054862928174,0.975342893301088);
static const Unit3 bearing3(0.156054862928174,-0.156054862928174,0.975342893301088);
static const Unit3 bearing4(0.156054862928174, 0.156054862928174, 0.975342893301088);
static const Unit3 bearing1(-0.156054862928174, 0.156054862928174,
0.975342893301088);
static const Unit3 bearing2(-0.156054862928174, -0.156054862928174,
0.975342893301088);
static const Unit3 bearing3(0.156054862928174, -0.156054862928174,
0.975342893301088);
static const Unit3 bearing4(0.156054862928174, 0.156054862928174,
0.975342893301088);
static double depth = 0.512640224719052;
/* ************************************************************************* */
TEST( SphericalCamera, constructor)
{
EXPECT(assert_equal( pose, camera.pose()));
TEST(SphericalCamera, constructor) {
EXPECT(assert_equal(pose, camera.pose()));
}
/* ************************************************************************* */
TEST( SphericalCamera, project)
{
TEST(SphericalCamera, project) {
// expected from manual calculation in Matlab
EXPECT(assert_equal( camera.project(point1), bearing1 ));
EXPECT(assert_equal( camera.project(point2), bearing2 ));
EXPECT(assert_equal( camera.project(point3), bearing3 ));
EXPECT(assert_equal( camera.project(point4), bearing4 ));
EXPECT(assert_equal(camera.project(point1), bearing1));
EXPECT(assert_equal(camera.project(point2), bearing2));
EXPECT(assert_equal(camera.project(point3), bearing3));
EXPECT(assert_equal(camera.project(point4), bearing4));
}
/* ************************************************************************* */
TEST( SphericalCamera, backproject)
{
EXPECT(assert_equal( camera.backproject(bearing1, depth), point1));
EXPECT(assert_equal( camera.backproject(bearing2, depth), point2));
EXPECT(assert_equal( camera.backproject(bearing3, depth), point3));
EXPECT(assert_equal( camera.backproject(bearing4, depth), point4));
TEST(SphericalCamera, backproject) {
EXPECT(assert_equal(camera.backproject(bearing1, depth), point1));
EXPECT(assert_equal(camera.backproject(bearing2, depth), point2));
EXPECT(assert_equal(camera.backproject(bearing3, depth), point3));
EXPECT(assert_equal(camera.backproject(bearing4, depth), point4));
}
/* ************************************************************************* */
TEST( SphericalCamera, backproject2)
{
Point3 origin(0,0,0);
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
TEST(SphericalCamera, backproject2) {
Point3 origin(0, 0, 0);
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin));
Point3 actual = camera.backproject(Unit3(0,0,1), 1.);
Point3 actual = camera.backproject(Unit3(0, 0, 1), 1.);
Point3 expected(0., 1., 0.);
pair<Unit3, bool> x = camera.projectSafe(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Unit3(0,0,1), x.first));
EXPECT(assert_equal(Unit3(0, 0, 1), x.first));
EXPECT(x.second);
}
@ -98,45 +98,45 @@ static Unit3 project3(const Pose3& pose, const Point3& point) {
}
/* ************************************************************************* */
TEST( SphericalCamera, Dproject)
{
TEST(SphericalCamera, Dproject) {
Matrix Dpose, Dpoint;
Unit3 result = camera.project(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project3, pose, point1);
Matrix numerical_pose = numericalDerivative21(project3, pose, point1);
Matrix numerical_point = numericalDerivative22(project3, pose, point1);
EXPECT(assert_equal(bearing1, result));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
static Vector2 reprojectionError2(const Pose3& pose, const Point3& point, const Unit3& measured) {
return Camera(pose).reprojectionError(point,measured);
static Vector2 reprojectionError2(const Pose3& pose, const Point3& point,
const Unit3& measured) {
return Camera(pose).reprojectionError(point, measured);
}
/* ************************************************************************* */
TEST( SphericalCamera, reprojectionError) {
TEST(SphericalCamera, reprojectionError) {
Matrix Dpose, Dpoint;
Vector2 result = camera.reprojectionError(point1, bearing1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative31(reprojectionError2, pose,
point1, bearing1);
Matrix numerical_point = numericalDerivative32(reprojectionError2, pose,
point1, bearing1);
Matrix numerical_pose =
numericalDerivative31(reprojectionError2, pose, point1, bearing1);
Matrix numerical_point =
numericalDerivative32(reprojectionError2, pose, point1, bearing1);
EXPECT(assert_equal(Vector2(0.0, 0.0), result));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
TEST( SphericalCamera, reprojectionError_noisy) {
TEST(SphericalCamera, reprojectionError_noisy) {
Matrix Dpose, Dpoint;
Unit3 bearing_noisy = bearing1.retract(Vector2(0.01, 0.05));
Vector2 result = camera.reprojectionError(point1, bearing_noisy, Dpose,
Dpoint);
Matrix numerical_pose = numericalDerivative31(reprojectionError2, pose,
point1, bearing_noisy);
Matrix numerical_point = numericalDerivative32(reprojectionError2, pose,
point1, bearing_noisy);
Vector2 result =
camera.reprojectionError(point1, bearing_noisy, Dpose, Dpoint);
Matrix numerical_pose =
numericalDerivative31(reprojectionError2, pose, point1, bearing_noisy);
Matrix numerical_point =
numericalDerivative32(reprojectionError2, pose, point1, bearing_noisy);
EXPECT(assert_equal(Vector2(-0.050282, 0.00833482), result, 1e-5));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
@ -144,20 +144,20 @@ TEST( SphericalCamera, reprojectionError_noisy) {
/* ************************************************************************* */
// Add a test with more arbitrary rotation
TEST( SphericalCamera, Dproject2)
{
TEST(SphericalCamera, Dproject2) {
static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Camera camera(pose1);
Matrix Dpose, Dpoint;
camera.project2(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project3, pose1, point1);
Matrix numerical_pose = numericalDerivative21(project3, pose1, point1);
Matrix numerical_point = numericalDerivative22(project3, pose1, point1);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -17,17 +17,16 @@
* @author Luca Carlone
*/
#include <gtsam/geometry/triangulation.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/SphericalCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/StereoFactor.h>
#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>
@ -38,7 +37,7 @@ using namespace boost::assign;
// Some common constants
static const boost::shared_ptr<Cal3_S2> sharedCal = //
static const boost::shared_ptr<Cal3_S2> sharedCal = //
boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
// Looking along X-axis, 1 meter above ground plane (x-y)
@ -59,8 +58,7 @@ Point2 z2 = camera2.project(landmark);
//******************************************************************************
// Simple test with a well-behaved two camera situation
TEST( triangulation, twoPoses) {
TEST(triangulation, twoPoses) {
vector<Pose3> poses;
Point2Vector measurements;
@ -71,36 +69,36 @@ TEST( triangulation, twoPoses) {
// 1. Test simple DLT, perfect in no noise situation
bool optimize = false;
boost::optional<Point3> actual1 = //
boost::optional<Point3> actual1 = //
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7));
// 2. test with optimization on, same answer
optimize = true;
boost::optional<Point3> actual2 = //
boost::optional<Point3> actual2 = //
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7));
// 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
// 3. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814)
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
optimize = false;
boost::optional<Point3> actual3 = //
boost::optional<Point3> actual3 = //
triangulatePoint3(poses, sharedCal, 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 = //
boost::optional<Point3> actual4 = //
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
}
//******************************************************************************
// Similar, but now with Bundler calibration
TEST( triangulation, twoPosesBundler) {
boost::shared_ptr<Cal3Bundler> bundlerCal = //
TEST(triangulation, twoPosesBundler) {
boost::shared_ptr<Cal3Bundler> bundlerCal = //
boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480);
PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal);
PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal);
@ -118,7 +116,7 @@ TEST( triangulation, twoPosesBundler) {
bool optimize = true;
double rank_tol = 1e-9;
boost::optional<Point3> actual = //
boost::optional<Point3> actual = //
triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual, 1e-7));
@ -126,28 +124,29 @@ TEST( triangulation, twoPosesBundler) {
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> actual2 = //
boost::optional<Point3> actual2 = //
triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4));
}
//******************************************************************************
TEST( triangulation, fourPoses) {
TEST(triangulation, fourPoses) {
vector<Pose3> poses;
Point2Vector measurements;
poses += pose1, pose2;
measurements += z1, z2;
boost::optional<Point3> actual = triangulatePoint3(poses, sharedCal,
measurements);
boost::optional<Point3> actual =
triangulatePoint3(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
// 2. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814)
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> actual2 = //
boost::optional<Point3> actual2 = //
triangulatePoint3(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *actual2, 1e-2));
@ -159,13 +158,13 @@ TEST( triangulation, fourPoses) {
poses += pose3;
measurements += z3 + Point2(0.1, -0.1);
boost::optional<Point3> triangulated_3cameras = //
boost::optional<Point3> triangulated_3cameras = //
triangulatePoint3(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses,
sharedCal, measurements, 1e-9, true);
boost::optional<Point3> triangulated_3cameras_opt =
triangulatePoint3(poses, sharedCal, measurements, 1e-9, true);
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way
@ -179,12 +178,12 @@ TEST( triangulation, fourPoses) {
measurements += Point2(400, 400);
CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
TriangulationCheiralityException);
TriangulationCheiralityException);
#endif
}
//******************************************************************************
TEST( triangulation, fourPoses_distinct_Ks) {
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)
PinholeCamera<Cal3_S2> camera1(pose1, K1);
@ -197,21 +196,22 @@ TEST( triangulation, fourPoses_distinct_Ks) {
Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark);
CameraSet<PinholeCamera<Cal3_S2> > cameras;
CameraSet<PinholeCamera<Cal3_S2>> cameras;
Point2Vector measurements;
cameras += camera1, camera2;
measurements += z1, z2;
boost::optional<Point3> actual = //
boost::optional<Point3> actual = //
triangulatePoint3(cameras, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
// 2. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814)
measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> actual2 = //
boost::optional<Point3> actual2 = //
triangulatePoint3(cameras, measurements);
EXPECT(assert_equal(landmark, *actual2, 1e-2));
@ -224,13 +224,13 @@ TEST( triangulation, fourPoses_distinct_Ks) {
cameras += camera3;
measurements += z3 + Point2(0.1, -0.1);
boost::optional<Point3> triangulated_3cameras = //
boost::optional<Point3> triangulated_3cameras = //
triangulatePoint3(cameras, measurements);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(cameras,
measurements, 1e-9, true);
boost::optional<Point3> triangulated_3cameras_opt =
triangulatePoint3(cameras, measurements, 1e-9, true);
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way
@ -244,12 +244,12 @@ TEST( triangulation, fourPoses_distinct_Ks) {
cameras += camera4;
measurements += Point2(400, 400);
CHECK_EXCEPTION(triangulatePoint3(cameras, measurements),
TriangulationCheiralityException);
TriangulationCheiralityException);
#endif
}
//******************************************************************************
TEST( triangulation, outliersAndFarLandmarks) {
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)
PinholeCamera<Cal3_S2> camera1(pose1, K1);
@ -262,24 +262,29 @@ TEST( triangulation, outliersAndFarLandmarks) {
Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark);
CameraSet<PinholeCamera<Cal3_S2> > cameras;
CameraSet<PinholeCamera<Cal3_S2>> cameras;
Point2Vector measurements;
cameras += camera1, camera2;
measurements += z1, z2;
double landmarkDistanceThreshold = 10; // landmark is closer than that
TriangulationParameters params(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold
TriangulationResult actual = triangulateSafe(cameras,measurements,params);
double landmarkDistanceThreshold = 10; // landmark is closer than that
TriangulationParameters params(
1.0, false, landmarkDistanceThreshold); // all default except
// landmarkDistanceThreshold
TriangulationResult actual = triangulateSafe(cameras, measurements, params);
EXPECT(assert_equal(landmark, *actual, 1e-2));
EXPECT(actual.valid());
landmarkDistanceThreshold = 4; // landmark is farther than that
TriangulationParameters params2(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold
actual = triangulateSafe(cameras,measurements,params2);
landmarkDistanceThreshold = 4; // landmark is farther than that
TriangulationParameters params2(
1.0, false, landmarkDistanceThreshold); // all default except
// landmarkDistanceThreshold
actual = triangulateSafe(cameras, measurements, params2);
EXPECT(actual.farPoint());
// 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER)
// 3. Add a slightly rotated third camera above with a wrong measurement
// (OUTLIER)
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
Cal3_S2 K3(700, 500, 0, 640, 480);
PinholeCamera<Cal3_S2> camera3(pose3, K3);
@ -288,21 +293,23 @@ TEST( triangulation, outliersAndFarLandmarks) {
cameras += camera3;
measurements += z3 + Point2(10, -10);
landmarkDistanceThreshold = 10; // landmark is closer than that
double outlierThreshold = 100; // loose, the outlier is going to pass
TriangulationParameters params3(1.0, false, landmarkDistanceThreshold,outlierThreshold);
actual = triangulateSafe(cameras,measurements,params3);
landmarkDistanceThreshold = 10; // landmark is closer than that
double outlierThreshold = 100; // loose, the outlier is going to pass
TriangulationParameters params3(1.0, false, landmarkDistanceThreshold,
outlierThreshold);
actual = triangulateSafe(cameras, measurements, params3);
EXPECT(actual.valid());
// now set stricter threshold for outlier rejection
outlierThreshold = 5; // tighter, the outlier is not going to pass
TriangulationParameters params4(1.0, false, landmarkDistanceThreshold,outlierThreshold);
actual = triangulateSafe(cameras,measurements,params4);
outlierThreshold = 5; // tighter, the outlier is not going to pass
TriangulationParameters params4(1.0, false, landmarkDistanceThreshold,
outlierThreshold);
actual = triangulateSafe(cameras, measurements, params4);
EXPECT(actual.outlier());
}
//******************************************************************************
TEST( triangulation, twoIdenticalPoses) {
TEST(triangulation, twoIdenticalPoses) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
@ -316,11 +323,11 @@ TEST( triangulation, twoIdenticalPoses) {
measurements += z1, z1;
CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
TriangulationUnderconstrainedException);
TriangulationUnderconstrainedException);
}
//******************************************************************************
TEST( triangulation, onePose) {
TEST(triangulation, onePose) {
// we expect this test to fail with a TriangulationUnderconstrainedException
// because there's only one camera observation
@ -328,28 +335,26 @@ TEST( triangulation, onePose) {
Point2Vector measurements;
poses += Pose3();
measurements += Point2(0,0);
measurements += Point2(0, 0);
CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
TriangulationUnderconstrainedException);
TriangulationUnderconstrainedException);
}
//******************************************************************************
TEST( triangulation, StereotriangulateNonlinear ) {
auto stereoK = boost::make_shared<Cal3_S2Stereo>(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612);
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
Matrix4 m1, m2;
m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779,
0.592783835, -0.77156583, 0.230856632, 66.2186159,
0.116517574, -0.201470143, -0.9725393, -4.28382528,
0, 0, 0, 1;
m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779, 0.592783835,
-0.77156583, 0.230856632, 66.2186159, 0.116517574, -0.201470143,
-0.9725393, -4.28382528, 0, 0, 0, 1;
m2 << -0.955959025, -0.29288915, -0.0189328569, 45.7169799,
-0.29277519, 0.947083213, 0.131587097, 65.843136,
-0.0206094928, 0.131334858, -0.991123524, -4.3525033,
0, 0, 0, 1;
m2 << -0.955959025, -0.29288915, -0.0189328569, 45.7169799, -0.29277519,
0.947083213, 0.131587097, 65.843136, -0.0206094928, 0.131334858,
-0.991123524, -4.3525033, 0, 0, 0, 1;
typedef CameraSet<StereoCamera> Cameras;
Cameras cameras;
@ -360,18 +365,19 @@ TEST( triangulation, StereotriangulateNonlinear ) {
measurements += StereoPoint2(226.936, 175.212, 424.469);
measurements += StereoPoint2(339.571, 285.547, 669.973);
Point3 initial = Point3(46.0536958, 66.4621179, -6.56285929); // error: 96.5715555191
Point3 initial =
Point3(46.0536958, 66.4621179, -6.56285929); // error: 96.5715555191
Point3 actual = triangulateNonlinear(cameras, measurements, initial);
Point3 expected(46.0484569, 66.4710686, -6.55046613); // error: 0.763510644187
Point3 expected(46.0484569, 66.4710686,
-6.55046613); // error: 0.763510644187
EXPECT(assert_equal(expected, actual, 1e-4));
// regular stereo factor comparison - expect very similar result as above
{
typedef GenericStereoFactor<Pose3,Point3> StereoFactor;
typedef GenericStereoFactor<Pose3, Point3> StereoFactor;
Values values;
values.insert(Symbol('x', 1), Pose3(m1));
@ -380,17 +386,19 @@ TEST( triangulation, StereotriangulateNonlinear ) {
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3));
graph.emplace_shared<StereoFactor>(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK);
graph.emplace_shared<StereoFactor>(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK);
graph.emplace_shared<StereoFactor>(measurements[0], unit, Symbol('x', 1),
Symbol('l', 1), stereoK);
graph.emplace_shared<StereoFactor>(measurements[1], unit, Symbol('x', 2),
Symbol('l', 1), stereoK);
const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9);
graph.addPrior(Symbol('x',1), Pose3(m1), posePrior);
graph.addPrior(Symbol('x',2), Pose3(m2), posePrior);
graph.addPrior(Symbol('x', 1), Pose3(m1), posePrior);
graph.addPrior(Symbol('x', 2), Pose3(m2), posePrior);
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l', 1)), 1e-4));
}
// use Triangulation Factor directly - expect same result as above
@ -401,13 +409,15 @@ TEST( triangulation, StereotriangulateNonlinear ) {
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3));
graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[0], measurements[0], unit, Symbol('l',1));
graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[1], measurements[1], unit, Symbol('l',1));
graph.emplace_shared<TriangulationFactor<StereoCamera>>(
cameras[0], measurements[0], unit, Symbol('l', 1));
graph.emplace_shared<TriangulationFactor<StereoCamera>>(
cameras[1], measurements[1], unit, Symbol('l', 1));
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l', 1)), 1e-4));
}
// use ExpressionFactor - expect same result as above
@ -418,11 +428,13 @@ TEST( triangulation, StereotriangulateNonlinear ) {
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3));
Expression<Point3> point_(Symbol('l',1));
Expression<Point3> point_(Symbol('l', 1));
Expression<StereoCamera> camera0_(cameras[0]);
Expression<StereoCamera> camera1_(cameras[1]);
Expression<StereoPoint2> project0_(camera0_, &StereoCamera::project2, point_);
Expression<StereoPoint2> project1_(camera1_, &StereoCamera::project2, point_);
Expression<StereoPoint2> project0_(camera0_, &StereoCamera::project2,
point_);
Expression<StereoPoint2> project1_(camera1_, &StereoCamera::project2,
point_);
graph.addExpressionFactor(unit, measurements[0], project0_);
graph.addExpressionFactor(unit, measurements[1], project1_);
@ -430,14 +442,13 @@ TEST( triangulation, StereotriangulateNonlinear ) {
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l', 1)), 1e-4));
}
}
//******************************************************************************
// Simple test with a well-behaved two camera situation
TEST( triangulation, twoPoses_sphericalCamera) {
TEST(triangulation, twoPoses_sphericalCamera) {
vector<Pose3> poses;
std::vector<Unit3> measurements;
@ -457,8 +468,9 @@ TEST( triangulation, twoPoses_sphericalCamera) {
double rank_tol = 1e-9;
// 1. Test linear triangulation via DLT
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices =
getCameraProjectionMatrices<SphericalCamera>(cameras);
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
projection_matrices =
getCameraProjectionMatrices<SphericalCamera>(cameras);
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
EXPECT(assert_equal(landmark, point, 1e-7));
@ -468,48 +480,60 @@ TEST( triangulation, twoPoses_sphericalCamera) {
// 3. Test simple DLT, now within triangulatePoint3
bool optimize = false;
boost::optional<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, optimize);
boost::optional<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7));
// 4. test with optimization on, same answer
optimize = true;
boost::optional<Point3> actual2 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, optimize);
boost::optional<Point3> actual2 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7));
// 5. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements.at(0) = u1.retract(Vector2(0.01, 0.05)); //note: perturbation smaller for Unit3
// 5. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814)
measurements.at(0) =
u1.retract(Vector2(0.01, 0.05)); // note: perturbation smaller for Unit3
measurements.at(1) = u2.retract(Vector2(-0.02, 0.03));
optimize = false;
boost::optional<Point3> actual3 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, optimize);
boost::optional<Point3> actual3 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(Point3(5.9432, 0.654319, 1.48192), *actual3, 1e-3));
// 6. Now with optimization on
optimize = true;
boost::optional<Point3> actual4 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, optimize);
boost::optional<Point3> actual4 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(Point3(5.9432, 0.654334, 1.48192), *actual4, 1e-3));
}
//******************************************************************************
TEST( triangulation, twoPoses_sphericalCamera_extremeFOV) {
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), Point3(0.0,0.0,0.0)); // with z pointing along x axis of global frame
Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(2.0,0.0,0.0)); // 2m in front of poseA
Point3 landmarkL(1.0,-1.0,0.0);// 1m to the right of both cameras, in front of poseA, behind poseB
Pose3 poseA = Pose3(
Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0.0, 0.0, 0.0)); // with z pointing along x axis of global frame
Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(2.0, 0.0, 0.0)); // 2m in front of poseA
Point3 landmarkL(
1.0, -1.0,
0.0); // 1m to the right of both cameras, in front of poseA, behind poseB
SphericalCamera cam1(poseA);
SphericalCamera cam2(poseB);
Unit3 u1 = cam1.project(landmarkL);
Unit3 u2 = cam2.project(landmarkL);
EXPECT(assert_equal(Unit3(Point3(1.0,0.0,1.0)), u1, 1e-7)); // in front and to the right of PoseA
EXPECT(assert_equal(Unit3(Point3(1.0,0.0,-1.0)), u2, 1e-7));// behind and to the right of PoseB
EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, 1.0)), u1,
1e-7)); // in front and to the right of PoseA
EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2,
1e-7)); // behind and to the right of PoseB
poses += pose1, pose2;
measurements += u1, u2;
@ -521,57 +545,65 @@ TEST( triangulation, twoPoses_sphericalCamera_extremeFOV) {
double rank_tol = 1e-9;
{
// 1. Test simple DLT, when 1 point is behind spherical camera
bool optimize = false;
// 1. Test simple DLT, when 1 point is behind spherical camera
bool optimize = false;
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize), TriangulationCheiralityException);
#else // otherwise project should not throw the exception
boost::optional<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmarkL, *actual1, 1e-7));
CHECK_EXCEPTION(triangulatePoint3<SphericalCamera>(cameras, measurements,
rank_tol, optimize),
TriangulationCheiralityException);
#else // otherwise project should not throw the exception
boost::optional<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(landmarkL, *actual1, 1e-7));
#endif
}
{
// 2. test with optimization on, same answer
bool optimize = true;
// 2. test with optimization on, same answer
bool optimize = true;
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize), TriangulationCheiralityException);
#else // otherwise project should not throw the exception
boost::optional<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmarkL, *actual1, 1e-7));
CHECK_EXCEPTION(triangulatePoint3<SphericalCamera>(cameras, measurements,
rank_tol, optimize),
TriangulationCheiralityException);
#else // otherwise project should not throw the exception
boost::optional<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(landmarkL, *actual1, 1e-7));
#endif
}
}
//******************************************************************************
TEST( triangulation, reprojectionError_cameraComparison) {
Pose3 poseA = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,0.0,0.0)); // with z pointing along x axis of global frame
Point3 landmarkL(5.0,0.0,0.0);// 1m in front of poseA
TEST(triangulation, reprojectionError_cameraComparison) {
Pose3 poseA = Pose3(
Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0.0, 0.0, 0.0)); // with z pointing along x axis of global frame
Point3 landmarkL(5.0, 0.0, 0.0); // 1m in front of poseA
SphericalCamera sphericalCamera(poseA);
Unit3 u = sphericalCamera.project(landmarkL);
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(60, 640, 480));
PinholePose<Cal3_S2> pinholeCamera(poseA,sharedK);
PinholePose<Cal3_S2> pinholeCamera(poseA, sharedK);
Vector2 px = pinholeCamera.project(landmarkL);
// add perturbation and compare error in both cameras
Vector2 px_noise(1.0,1.0); // 1px perturbation vertically and horizontally
Vector2 px_noise(1.0, 1.0); // 1px perturbation vertically and horizontally
Vector2 measured_px = px + px_noise;
Vector2 measured_px_calibrated = sharedK->calibrate(measured_px);
Unit3 measured_u = Unit3(measured_px_calibrated[0],measured_px_calibrated[1],1.0);
Unit3 measured_u =
Unit3(measured_px_calibrated[0], measured_px_calibrated[1], 1.0);
Vector2 actualErrorPinhole = pinholeCamera.reprojectionError(landmarkL,measured_px);
Vector2 expectedErrorPinhole = Vector2(-px_noise[0],-px_noise[1]);
EXPECT(assert_equal(expectedErrorPinhole, actualErrorPinhole, 1e-7)); //- sign due to definition of error
Vector2 actualErrorPinhole =
pinholeCamera.reprojectionError(landmarkL, measured_px);
Vector2 expectedErrorPinhole = Vector2(-px_noise[0], -px_noise[1]);
EXPECT(assert_equal(expectedErrorPinhole, actualErrorPinhole,
1e-7)); //- sign due to definition of error
Vector2 actualErrorSpherical = sphericalCamera.reprojectionError(landmarkL,measured_u);
Vector2 expectedErrorSpherical = Vector2(px_noise[0]/sharedK->fx(),px_noise[1]/sharedK->fy());
Vector2 actualErrorSpherical =
sphericalCamera.reprojectionError(landmarkL, measured_u);
Vector2 expectedErrorSpherical =
Vector2(px_noise[0] / sharedK->fx(), px_noise[1] / sharedK->fy());
EXPECT(assert_equal(expectedErrorSpherical, actualErrorSpherical, 1e-7));
}

42
gtsam/slam/tests/smartFactorScenarios.h
View File

@ -17,12 +17,13 @@
*/
#pragma once
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SphericalCamera.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include "../SmartProjectionRigFactor.h"
using namespace std;
@ -45,7 +46,7 @@ Pose3 pose_above = level_pose * Pose3(Rot3(), Point3(0, -1, 0));
// Create a noise unit2 for the pixel error
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static double fov = 60; // degrees
static double fov = 60; // degrees
static size_t w = 640, h = 480;
/* ************************************************************************* */
@ -64,7 +65,7 @@ Camera cam2(pose_right, K2);
Camera cam3(pose_above, K2);
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
SmartProjectionParams params;
}
} // namespace vanilla
/* ************************************************************************* */
// default Cal3_S2 poses
@ -79,7 +80,7 @@ Camera level_camera_right(pose_right, sharedK);
Camera cam1(level_pose, sharedK);
Camera cam2(pose_right, sharedK);
Camera cam3(pose_above, sharedK);
}
} // namespace vanillaPose
/* ************************************************************************* */
// default Cal3_S2 poses
@ -94,7 +95,7 @@ Camera level_camera_right(pose_right, sharedK2);
Camera cam1(level_pose, sharedK2);
Camera cam2(pose_right, sharedK2);
Camera cam3(pose_above, sharedK2);
}
} // namespace vanillaPose2
/* *************************************************************************/
// Cal3Bundler cameras
@ -112,7 +113,7 @@ Camera cam1(level_pose, K);
Camera cam2(pose_right, K);
Camera cam3(pose_above, K);
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
}
} // namespace bundler
/* *************************************************************************/
// Cal3Bundler poses
@ -121,14 +122,15 @@ typedef PinholePose<Cal3Bundler> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor;
typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
static boost::shared_ptr<Cal3Bundler> sharedBundlerK(
new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
static boost::shared_ptr<Cal3Bundler> sharedBundlerK(new Cal3Bundler(500, 1e-3,
1e-3, 1000,
2000));
Camera level_camera(level_pose, sharedBundlerK);
Camera level_camera_right(pose_right, sharedBundlerK);
Camera cam1(level_pose, sharedBundlerK);
Camera cam2(pose_right, sharedBundlerK);
Camera cam3(pose_above, sharedBundlerK);
}
} // namespace bundlerPose
/* ************************************************************************* */
// sphericalCamera
@ -142,21 +144,22 @@ Camera level_camera_right(pose_right);
Camera cam1(level_pose);
Camera cam2(pose_right);
Camera cam3(pose_above);
}
} // namespace sphericalCamera
/* *************************************************************************/
template<class CAMERA>
template <class CAMERA>
CAMERA perturbCameraPose(const CAMERA& camera) {
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
Pose3 noise_pose =
Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3));
Pose3 cameraPose = camera.pose();
Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
return CAMERA(perturbedCameraPose, camera.calibration());
}
template<class CAMERA>
void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2,
const CAMERA& cam3, Point3 landmark, typename CAMERA::MeasurementVector& measurements_cam) {
template <class CAMERA>
void projectToMultipleCameras(
const CAMERA& cam1, const CAMERA& cam2, const CAMERA& cam3, Point3 landmark,
typename CAMERA::MeasurementVector& measurements_cam) {
typename CAMERA::Measurement cam1_uv1 = cam1.project(landmark);
typename CAMERA::Measurement cam2_uv1 = cam2.project(landmark);
typename CAMERA::Measurement cam3_uv1 = cam3.project(landmark);
@ -166,4 +169,3 @@ void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2,
}
/* ************************************************************************* */

220
gtsam/slam/tests/testSmartProjectionRigFactor.cpp
View File

@ -1195,8 +1195,9 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) {
// this factor is slightly slower (but comparable) to original
// SmartProjectionPoseFactor
//-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0)
//| -SmartRigFactor LINEARIZE: 0.05 CPU (10000 times, 0.057952 wall, 0.05 children, min: 0 max: 0)
//| -SmartPoseFactor LINEARIZE: 0.05 CPU (10000 times, 0.069647 wall, 0.05 children, min: 0 max: 0)
//| -SmartRigFactor LINEARIZE: 0.05 CPU (10000 times, 0.057952 wall, 0.05
// children, min: 0 max: 0) | -SmartPoseFactor LINEARIZE: 0.05 CPU (10000
// times, 0.069647 wall, 0.05 children, min: 0 max: 0)
/* *************************************************************************/
TEST(SmartProjectionRigFactor, timing) {
using namespace vanillaRig;
@ -1256,15 +1257,18 @@ TEST(SmartProjectionRigFactor, timing) {
#endif
/* *************************************************************************/
TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) {
TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
using namespace sphericalCamera;
Camera::MeasurementVector measurements_lmk1, measurements_lmk2, measurements_lmk3;
Camera::MeasurementVector measurements_lmk1, measurements_lmk2,
measurements_lmk3;
// Project three landmarks into three cameras
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1, measurements_lmk1);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2, measurements_lmk2);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3, measurements_lmk3);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1,
measurements_lmk1);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2,
measurements_lmk2);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3,
measurements_lmk3);
// create inputs
KeyVector keys;
@ -1280,13 +1284,16 @@ TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) {
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
params.setRankTolerance(0.1);
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params));
SmartFactorP::shared_ptr smartFactor1(
new SmartFactorP(model, cameraRig, params));
smartFactor1->add(measurements_lmk1, keys);
SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params));
SmartFactorP::shared_ptr smartFactor2(
new SmartFactorP(model, cameraRig, params));
smartFactor2->add(measurements_lmk2, keys);
SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params));
SmartFactorP::shared_ptr smartFactor3(
new SmartFactorP(model, cameraRig, params));
smartFactor3->add(measurements_lmk3, keys);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1305,12 +1312,13 @@ TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) {
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 100),
Point3(0.2, 0.2, 0.2)); // note: larger noise!
Point3(0.2, 0.2, 0.2)); // note: larger noise!
Values values;
values.insert(x1, level_pose);
values.insert(x2, pose_right);
// initialize third pose with some noise, we expect it to move back to original pose_above
// initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose_above * noise_pose);
DOUBLES_EQUAL(0.94148963675515274, graph.error(values), 1e-9);
@ -1324,12 +1332,13 @@ TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) {
#ifndef DISABLE_TIMING
#include <gtsam/base/timing.h>
// using spherical camera is slightly slower (but comparable) to PinholePose<Cal3_S2>
//| -SmartFactorP spherical LINEARIZE: 0.01 CPU (1000 times, 0.008178 wall, 0.01 children, min: 0 max: 0)
//| -SmartFactorP pinhole LINEARIZE: 0.01 CPU (1000 times, 0.005717 wall, 0.01 children, min: 0 max: 0)
// using spherical camera is slightly slower (but comparable) to
// PinholePose<Cal3_S2>
//| -SmartFactorP spherical LINEARIZE: 0.01 CPU (1000 times, 0.008178 wall,
// 0.01 children, min: 0 max: 0) | -SmartFactorP pinhole LINEARIZE: 0.01 CPU
//(1000 times, 0.005717 wall, 0.01 children, min: 0 max: 0)
/* *************************************************************************/
TEST( SmartProjectionFactorP, timing_sphericalCamera ) {
TEST(SmartProjectionFactorP, timing_sphericalCamera) {
// create common data
Rot3 R = Rot3::identity();
Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
@ -1359,7 +1368,7 @@ TEST( SmartProjectionFactorP, timing_sphericalCamera ) {
size_t nrTests = 1000;
for(size_t i = 0; i<nrTests; i++){
for (size_t i = 0; i < nrTests; i++) {
CameraSet<SphericalCamera> cameraRig; // single camera in the rig
cameraRig.push_back(SphericalCamera(body_P_sensorId, emptyK));
@ -1377,13 +1386,13 @@ TEST( SmartProjectionFactorP, timing_sphericalCamera ) {
gttoc_(SmartFactorP_spherical_LINEARIZE);
}
for(size_t i = 0; i<nrTests; i++){
for (size_t i = 0; i < nrTests; i++) {
CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig
cameraRig.push_back(PinholePose<Cal3_S2>(body_P_sensorId, sharedKSimple));
SmartProjectionRigFactor<PinholePose<Cal3_S2>>::shared_ptr smartFactorP2(
new SmartProjectionRigFactor<PinholePose<Cal3_S2>>(model, cameraRig,
params));
params));
smartFactorP2->add(measurements_lmk1[0], x1);
smartFactorP2->add(measurements_lmk1[1], x1);
@ -1399,17 +1408,21 @@ TEST( SmartProjectionFactorP, timing_sphericalCamera ) {
#endif
/* *************************************************************************/
TEST( SmartProjectionFactorP, 2poses_rankTol ) {
Pose3 poseA = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,0.0,0.0)); // with z pointing along x axis of global frame
Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,-0.1,0.0)); // 10cm to the right of poseA
Point3 landmarkL = Point3(5.0,0.0,0.0); // 5m in front of poseA
TEST(SmartProjectionFactorP, 2poses_rankTol) {
Pose3 poseA = Pose3(
Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0.0, 0.0, 0.0)); // with z pointing along x axis of global frame
Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0.0, -0.1, 0.0)); // 10cm to the right of poseA
Point3 landmarkL = Point3(5.0, 0.0, 0.0); // 5m in front of poseA
// triangulate from a stereo with 10cm baseline, assuming standard calibration
{// default rankTol = 1 gives a valid point (compare with calibrated and spherical cameras below)
using namespace vanillaPose; // pinhole with Cal3_S2 calibration
{ // default rankTol = 1 gives a valid point (compare with calibrated and
// spherical cameras below)
using namespace vanillaPose; // pinhole with Cal3_S2 calibration
Camera cam1(poseA,sharedK);
Camera cam2(poseB,sharedK);
Camera cam1(poseA, sharedK);
Camera cam2(poseB, sharedK);
SmartProjectionParams params(
gtsam::HESSIAN,
@ -1419,7 +1432,8 @@ TEST( SmartProjectionFactorP, 2poses_rankTol ) {
CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig
cameraRig.push_back(PinholePose<Cal3_S2>(Pose3::identity(), sharedK));
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig,params));
SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params));
smartFactor1->add(cam1.project(landmarkL), x1);
smartFactor1->add(cam2.project(landmarkL), x2);
@ -1433,16 +1447,19 @@ TEST( SmartProjectionFactorP, 2poses_rankTol ) {
// get point
TriangulationResult point = smartFactor1->point();
EXPECT(point.valid()); // valid triangulation
EXPECT(point.valid()); // valid triangulation
EXPECT(assert_equal(landmarkL, *point, 1e-7));
}
// triangulate from a stereo with 10cm baseline, assuming canonical calibration
{// default rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a point 5m away and 10cm baseline
using namespace vanillaPose; // pinhole with Cal3_S2 calibration
static Cal3_S2::shared_ptr canonicalK(new Cal3_S2(1.0,1.0,0.0,0.0,0.0)); //canonical camera
// triangulate from a stereo with 10cm baseline, assuming canonical
// calibration
{ // default rankTol = 1 or 0.1 gives a degenerate point, which is
// undesirable for a point 5m away and 10cm baseline
using namespace vanillaPose; // pinhole with Cal3_S2 calibration
static Cal3_S2::shared_ptr canonicalK(
new Cal3_S2(1.0, 1.0, 0.0, 0.0, 0.0)); // canonical camera
Camera cam1(poseA,canonicalK);
Camera cam2(poseB,canonicalK);
Camera cam1(poseA, canonicalK);
Camera cam2(poseB, canonicalK);
SmartProjectionParams params(
gtsam::HESSIAN,
@ -1452,7 +1469,8 @@ TEST( SmartProjectionFactorP, 2poses_rankTol ) {
CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig
cameraRig.push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig,params));
SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params));
smartFactor1->add(cam1.project(landmarkL), x1);
smartFactor1->add(cam2.project(landmarkL), x2);
@ -1466,15 +1484,18 @@ TEST( SmartProjectionFactorP, 2poses_rankTol ) {
// get point
TriangulationResult point = smartFactor1->point();
EXPECT(point.degenerate()); // valid triangulation
EXPECT(point.degenerate()); // valid triangulation
}
// triangulate from a stereo with 10cm baseline, assuming canonical calibration
{// smaller rankTol = 0.01 gives a valid point (compare with calibrated and spherical cameras below)
using namespace vanillaPose; // pinhole with Cal3_S2 calibration
static Cal3_S2::shared_ptr canonicalK(new Cal3_S2(1.0,1.0,0.0,0.0,0.0)); //canonical camera
// triangulate from a stereo with 10cm baseline, assuming canonical
// calibration
{ // smaller rankTol = 0.01 gives a valid point (compare with calibrated and
// spherical cameras below)
using namespace vanillaPose; // pinhole with Cal3_S2 calibration
static Cal3_S2::shared_ptr canonicalK(
new Cal3_S2(1.0, 1.0, 0.0, 0.0, 0.0)); // canonical camera
Camera cam1(poseA,canonicalK);
Camera cam2(poseB,canonicalK);
Camera cam1(poseA, canonicalK);
Camera cam2(poseB, canonicalK);
SmartProjectionParams params(
gtsam::HESSIAN,
@ -1484,7 +1505,8 @@ TEST( SmartProjectionFactorP, 2poses_rankTol ) {
CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig
cameraRig.push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig,params));
SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params));
smartFactor1->add(cam1.project(landmarkL), x1);
smartFactor1->add(cam2.project(landmarkL), x2);
@ -1498,19 +1520,22 @@ TEST( SmartProjectionFactorP, 2poses_rankTol ) {
// get point
TriangulationResult point = smartFactor1->point();
EXPECT(point.valid()); // valid triangulation
EXPECT(point.valid()); // valid triangulation
EXPECT(assert_equal(landmarkL, *point, 1e-7));
}
}
/* *************************************************************************/
TEST( SmartProjectionFactorP, 2poses_sphericalCamera_rankTol ) {
TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) {
typedef SphericalCamera Camera;
typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
static EmptyCal::shared_ptr emptyK;
Pose3 poseA = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,0.0,0.0)); // with z pointing along x axis of global frame
Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,-0.1,0.0)); // 10cm to the right of poseA
Point3 landmarkL = Point3(5.0,0.0,0.0); // 5m in front of poseA
Pose3 poseA = Pose3(
Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0.0, 0.0, 0.0)); // with z pointing along x axis of global frame
Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0.0, -0.1, 0.0)); // 10cm to the right of poseA
Point3 landmarkL = Point3(5.0, 0.0, 0.0); // 5m in front of poseA
Camera cam1(poseA);
Camera cam2(poseB);
@ -1519,73 +1544,82 @@ TEST( SmartProjectionFactorP, 2poses_sphericalCamera_rankTol ) {
cameraRig.push_back(SphericalCamera(Pose3::identity(), emptyK));
// TRIANGULATION TEST WITH DEFAULT RANK TOL
{// rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a point 5m away and 10cm baseline
{ // rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a
// point 5m away and 10cm baseline
SmartProjectionParams params(
gtsam::HESSIAN,
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
params.setRankTolerance(0.1);
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig,params));
smartFactor1->add(cam1.project(landmarkL), x1);
smartFactor1->add(cam2.project(landmarkL), x2);
SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params));
smartFactor1->add(cam1.project(landmarkL), x1);
smartFactor1->add(cam2.project(landmarkL), x2);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
Values groundTruth;
groundTruth.insert(x1, poseA);
groundTruth.insert(x2, poseB);
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
Values groundTruth;
groundTruth.insert(x1, poseA);
groundTruth.insert(x2, poseB);
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
// get point
TriangulationResult point = smartFactor1->point();
EXPECT(point.degenerate()); // not enough parallax
// get point
TriangulationResult point = smartFactor1->point();
EXPECT(point.degenerate()); // not enough parallax
}
// SAME TEST WITH SMALLER RANK TOL
{// rankTol = 0.01 gives a valid point
// By playing with this test, we can show we can triangulate also with a baseline of 5cm (even for points
// far away, >100m), but the test fails when the baseline becomes 1cm. This suggests using
// rankTol = 0.01 and setting a reasonable max landmark distance to obtain best results.
SmartProjectionParams params(
{ // rankTol = 0.01 gives a valid point
// By playing with this test, we can show we can triangulate also with a
// baseline of 5cm (even for points far away, >100m), but the test fails
// when the baseline becomes 1cm. This suggests using rankTol = 0.01 and
// setting a reasonable max landmark distance to obtain best results.
SmartProjectionParams params(
gtsam::HESSIAN,
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
params.setRankTolerance(0.01);
params.setRankTolerance(0.01);
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig,params));
smartFactor1->add(cam1.project(landmarkL), x1);
smartFactor1->add(cam2.project(landmarkL), x2);
SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params));
smartFactor1->add(cam1.project(landmarkL), x1);
smartFactor1->add(cam2.project(landmarkL), x2);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
Values groundTruth;
groundTruth.insert(x1, poseA);
groundTruth.insert(x2, poseB);
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
Values groundTruth;
groundTruth.insert(x1, poseA);
groundTruth.insert(x2, poseB);
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
// get point
TriangulationResult point = smartFactor1->point();
EXPECT(point.valid()); // valid triangulation
EXPECT(assert_equal(landmarkL, *point, 1e-7));
// get point
TriangulationResult point = smartFactor1->point();
EXPECT(point.valid()); // valid triangulation
EXPECT(assert_equal(landmarkL, *point, 1e-7));
}
}
/* ************************************************************************* */
//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
//BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
//BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
// "gtsam_noiseModel_Constrained");
// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal,
// "gtsam_noiseModel_Diagonal");
// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian,
// "gtsam_noiseModel_Gaussian");
// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
// "gtsam_noiseModel_Isotropic");
// BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
// BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
//
// SERIALIZATION TEST FAILS: to be fixed
//TEST(SmartProjectionFactorP, serialize) {
// TEST(SmartProjectionFactorP, serialize) {
// using namespace vanillaPose;
// using namespace gtsam::serializationTestHelpers;
// SmartProjectionParams params(
// gtsam::HESSIAN,
// gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
// gtsam::ZERO_ON_DEGENERACY); // only config that works with rig
// factors
// params.setRankTolerance(rankTol);
//
// CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig
@ -1598,7 +1632,7 @@ TEST( SmartProjectionFactorP, 2poses_sphericalCamera_rankTol ) {
// EXPECT(equalsBinary(factor));
//}
//
//TEST(SmartProjectionFactorP, serialize2) {
// TEST(SmartProjectionFactorP, serialize2) {
// using namespace vanillaPose;
// using namespace gtsam::serializationTestHelpers;
// SmartProjectionParams params(

55
gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
View File

@ -1352,31 +1352,34 @@ namespace sphericalCameraRS {
typedef SphericalCamera Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactorRollingShutter<Camera> SmartFactorRS_spherical;
Pose3 interp_pose1 = interpolate<Pose3>(level_pose,pose_right,interp_factor1);
Pose3 interp_pose2 = interpolate<Pose3>(pose_right,pose_above,interp_factor2);
Pose3 interp_pose3 = interpolate<Pose3>(pose_above,level_pose,interp_factor3);
Pose3 interp_pose1 = interpolate<Pose3>(level_pose, pose_right, interp_factor1);
Pose3 interp_pose2 = interpolate<Pose3>(pose_right, pose_above, interp_factor2);
Pose3 interp_pose3 = interpolate<Pose3>(pose_above, level_pose, interp_factor3);
static EmptyCal::shared_ptr emptyK;
Camera cam1(interp_pose1, emptyK);
Camera cam2(interp_pose2, emptyK);
Camera cam3(interp_pose3, emptyK);
}
} // namespace sphericalCameraRS
/* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_sphericalCameras ) {
TEST(SmartProjectionPoseFactorRollingShutter,
optimization_3poses_sphericalCameras) {
using namespace sphericalCameraRS;
std::vector<Unit3> measurements_lmk1, measurements_lmk2, measurements_lmk3;
// Project three landmarks into three cameras
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1, measurements_lmk1);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2, measurements_lmk2);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3, measurements_lmk3);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1,
measurements_lmk1);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2,
measurements_lmk2);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3,
measurements_lmk3);
// create inputs
std::vector<std::pair<Key,Key>> key_pairs;
key_pairs.push_back(std::make_pair(x1,x2));
key_pairs.push_back(std::make_pair(x2,x3));
key_pairs.push_back(std::make_pair(x3,x1));
std::vector<std::pair<Key, Key>> key_pairs;
key_pairs.push_back(std::make_pair(x1, x2));
key_pairs.push_back(std::make_pair(x2, x3));
key_pairs.push_back(std::make_pair(x3, x1));
std::vector<double> interp_factors;
interp_factors.push_back(interp_factor1);
@ -1392,15 +1395,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_sphericalCame
cameraRig.push_back(Camera(Pose3::identity(), emptyK));
SmartFactorRS_spherical::shared_ptr smartFactor1(
new SmartFactorRS_spherical(model,cameraRig,params));
new SmartFactorRS_spherical(model, cameraRig, params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS_spherical::shared_ptr smartFactor2(
new SmartFactorRS_spherical(model,cameraRig,params));
new SmartFactorRS_spherical(model, cameraRig, params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS_spherical::shared_ptr smartFactor3(
new SmartFactorRS_spherical(model,cameraRig,params));
new SmartFactorRS_spherical(model, cameraRig, params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1418,20 +1421,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_sphericalCame
groundTruth.insert(x3, pose_above);
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
// Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, level_pose);
values.insert(x2, pose_right);
// initialize third pose with some noise, we expect it to move back to original pose_above
// initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose_above * noise_pose);
EXPECT( // check that the pose is actually noisy
assert_equal(
Pose3(
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
EXPECT( // check that the pose is actually noisy
assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
-0.0313952598, -0.000986635786, 0.0314107591,
-0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)),
values.at<Pose3>(x3)));
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);