commit
c7781961e6
|
@ -312,6 +312,16 @@ public:
|
|||
return range(camera.pose(), Dcamera, Dother);
|
||||
}
|
||||
|
||||
/// for Linear Triangulation
|
||||
Matrix34 cameraProjectionMatrix() const {
|
||||
return K_.K() * PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4);
|
||||
}
|
||||
|
||||
/// for Nonlinear Triangulation
|
||||
Vector defaultErrorWhenTriangulatingBehindCamera() const {
|
||||
return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
|
|
|
@ -121,6 +121,13 @@ public:
|
|||
return _project(pw, Dpose, Dpoint, Dcal);
|
||||
}
|
||||
|
||||
/// project a 3D point from world coordinates into the image
|
||||
Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 3> Dpoint = boost::none,
|
||||
OptionalJacobian<2, DimK> Dcal = boost::none) const {
|
||||
return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured);
|
||||
}
|
||||
|
||||
/// project a point at infinity from world coordinates into the image
|
||||
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 2> Dpoint = boost::none,
|
||||
|
@ -159,7 +166,6 @@ public:
|
|||
return result;
|
||||
}
|
||||
|
||||
|
||||
/// backproject a 2-dimensional point to a 3-dimensional point at infinity
|
||||
Unit3 backprojectPointAtInfinity(const Point2& p) const {
|
||||
const Point2 pn = calibration().calibrate(p);
|
||||
|
@ -410,6 +416,16 @@ public:
|
|||
return PinholePose(); // assumes that the default constructor is valid
|
||||
}
|
||||
|
||||
/// for Linear Triangulation
|
||||
Matrix34 cameraProjectionMatrix() const {
|
||||
Matrix34 P = Matrix34(PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4));
|
||||
return K_->K() * P;
|
||||
}
|
||||
|
||||
/// for Nonlinear Triangulation
|
||||
Vector defaultErrorWhenTriangulatingBehindCamera() const {
|
||||
return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());;
|
||||
}
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
|
|
@ -0,0 +1,109 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SphericalCamera.h
|
||||
* @brief Calibrated camera with spherical projection
|
||||
* @date Aug 26, 2021
|
||||
* @author Luca Carlone
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/SphericalCamera.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
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"); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Unit3, bool> SphericalCamera::projectSafe(const Point3& pw) const {
|
||||
const Point3 pc = pose().transformTo(pw);
|
||||
Unit3 pu = Unit3::FromPoint3(pc);
|
||||
return make_pair(pu, pc.norm() > 1e-8);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Unit3 SphericalCamera::project2(const Point3& pw, OptionalJacobian<2, 6> Dpose,
|
||||
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);
|
||||
|
||||
if (pc.norm() <= 1e-8) throw("point cannot be at the center of the camera");
|
||||
|
||||
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
|
||||
return pu;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Unit3 SphericalCamera::project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose,
|
||||
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);
|
||||
|
||||
if (Dpose)
|
||||
*Dpose << Dtf_rot, Matrix::Zero(2, 3); // 2x6 (translation part is zero)
|
||||
if (Dpoint) *Dpoint = Dtf_point; // 2x2
|
||||
return pu;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 SphericalCamera::backproject(const Unit3& pu, const double depth) const {
|
||||
return pose().transformFrom(depth * pu);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Unit3 SphericalCamera::backprojectPointAtInfinity(const Unit3& p) const {
|
||||
return pose().rotation().rotate(p);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Unit3 SphericalCamera::project(const Point3& point,
|
||||
OptionalJacobian<2, 6> Dcamera,
|
||||
OptionalJacobian<2, 3> Dpoint) const {
|
||||
return project2(point, Dcamera, Dpoint);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector2 SphericalCamera::reprojectionError(
|
||||
const Point3& point, const Unit3& measured, OptionalJacobian<2, 6> Dpose,
|
||||
OptionalJacobian<2, 3> Dpoint) const {
|
||||
// project point
|
||||
if (Dpose || Dpoint) {
|
||||
Matrix26 H_project_pose;
|
||||
Matrix23 H_project_point;
|
||||
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;
|
||||
return error;
|
||||
} else {
|
||||
return measured.errorVector(project2(point, Dpose, Dpoint));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
|
@ -0,0 +1,225 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SphericalCamera.h
|
||||
* @brief Calibrated camera with spherical projection
|
||||
* @date Aug 26, 2021
|
||||
* @author Luca Carlone
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#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 {
|
||||
|
||||
/**
|
||||
* Empty calibration. Only needed to play well with other cameras
|
||||
* (e.g., when templating functions wrt cameras), since other cameras
|
||||
* have constuctors in the form ‘camera(pose,calibration)’
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT EmptyCal {
|
||||
public:
|
||||
enum { dimension = 0 };
|
||||
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;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* A spherical camera class that has a Pose3 and measures bearing vectors.
|
||||
* The camera has an ‘Empty’ calibration and the only 6 dof are the pose
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT SphericalCamera {
|
||||
public:
|
||||
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>()) {}
|
||||
|
||||
/// Constructor with pose
|
||||
explicit SphericalCamera(const Pose3& pose)
|
||||
: 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>()) {}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {}
|
||||
|
||||
/// Default destructor
|
||||
virtual ~SphericalCamera() = default;
|
||||
|
||||
/// return shared pointer to calibration
|
||||
const boost::shared_ptr<EmptyCal>& sharedCalibration() const {
|
||||
return emptyCal_;
|
||||
}
|
||||
|
||||
/// return calibration
|
||||
const EmptyCal& calibration() const { return *emptyCal_; }
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const SphericalCamera& camera, double tol = 1e-9) const;
|
||||
|
||||
/// print
|
||||
virtual void print(const std::string& s = "SphericalCamera") const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// return pose, constant version
|
||||
const Pose3& pose() const { return pose_; }
|
||||
|
||||
/// get rotation
|
||||
const Rot3& rotation() const { return pose_.rotation(); }
|
||||
|
||||
/// get translation
|
||||
const Point3& translation() const { return pose_.translation(); }
|
||||
|
||||
// /// return pose, with derivative
|
||||
// const Pose3& getPose(OptionalJacobian<6, 6> H) const;
|
||||
|
||||
/// @}
|
||||
/// @name Transformations and measurement functions
|
||||
/// @{
|
||||
|
||||
/// Project a point into the image and check depth
|
||||
std::pair<Unit3, bool> projectSafe(const Point3& pw) const;
|
||||
|
||||
/** Project point into the image
|
||||
* (note: there is no CheiralityException for a spherical camera)
|
||||
* @param point 3D point in world coordinates
|
||||
* @return the intrinsic coordinates of the projected point
|
||||
*/
|
||||
Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 3> Dpoint = boost::none) const;
|
||||
|
||||
/** Project point into the image
|
||||
* (note: there is no CheiralityException for a spherical camera)
|
||||
* @param point 3D direction in world coordinates
|
||||
* @return the intrinsic coordinates of the projected point
|
||||
*/
|
||||
Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 2> Dpoint = boost::none) const;
|
||||
|
||||
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
|
||||
Point3 backproject(const Unit3& p, const double depth) const;
|
||||
|
||||
/// backproject point at infinity
|
||||
Unit3 backprojectPointAtInfinity(const Unit3& p) const;
|
||||
|
||||
/** Project point into the image
|
||||
* (note: there is no CheiralityException for a spherical camera)
|
||||
* @param point 3D point in world coordinates
|
||||
* @return the intrinsic coordinates of the projected point
|
||||
*/
|
||||
Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 3> Dpoint = boost::none) const;
|
||||
|
||||
/** Compute reprojection error for a given 3D point in world coordinates
|
||||
* @param point 3D point in world coordinates
|
||||
* @return the tangent space error between the projection and the measurement
|
||||
*/
|
||||
Vector2 reprojectionError(const Point3& point, const Unit3& measured,
|
||||
OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 3> Dpoint = boost::none) const;
|
||||
/// @}
|
||||
|
||||
/// move a cameras according to d
|
||||
SphericalCamera retract(const Vector6& d) const {
|
||||
return SphericalCamera(pose().retract(d));
|
||||
}
|
||||
|
||||
/// return canonical coordinate
|
||||
Vector6 localCoordinates(const SphericalCamera& p) const {
|
||||
return pose().localCoordinates(p.pose());
|
||||
}
|
||||
|
||||
/// for Canonical
|
||||
static SphericalCamera identity() {
|
||||
return SphericalCamera(
|
||||
Pose3::identity()); // assumes that the default constructor is valid
|
||||
}
|
||||
|
||||
/// for Linear Triangulation
|
||||
Matrix34 cameraProjectionMatrix() const {
|
||||
return Matrix34(pose_.inverse().matrix().block(0, 0, 3, 4));
|
||||
}
|
||||
|
||||
/// for Nonlinear Triangulation
|
||||
Vector defaultErrorWhenTriangulatingBehindCamera() const {
|
||||
return Eigen::Matrix<double, traits<Point2>::dimension, 1>::Constant(0.0);
|
||||
}
|
||||
|
||||
/// @deprecated
|
||||
size_t dim() const { 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_);
|
||||
}
|
||||
};
|
||||
// end of class SphericalCamera
|
||||
|
||||
template <>
|
||||
struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {};
|
||||
|
||||
template <>
|
||||
struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {};
|
||||
|
||||
} // namespace gtsam
|
|
@ -170,6 +170,11 @@ public:
|
|||
OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
|
||||
boost::none) const;
|
||||
|
||||
/// for Nonlinear Triangulation
|
||||
Vector defaultErrorWhenTriangulatingBehindCamera() const {
|
||||
return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());;
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
|
|
@ -0,0 +1,163 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SphericalCamera.h
|
||||
* @brief Calibrated camera with spherical projection
|
||||
* @date Aug 26, 2021
|
||||
* @author Luca Carlone
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/SphericalCamera.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std::placeholders;
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef SphericalCamera Camera;
|
||||
|
||||
// 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 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 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);
|
||||
|
||||
// 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 double depth = 0.512640224719052;
|
||||
/* ************************************************************************* */
|
||||
TEST(SphericalCamera, constructor) {
|
||||
EXPECT(assert_equal(pose, camera.pose()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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
|
||||
Camera camera(Pose3(rot, origin));
|
||||
|
||||
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(x.second);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Unit3 project3(const Pose3& pose, const Point3& point) {
|
||||
return Camera(pose).project(point);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SphericalCamera, Dproject) {
|
||||
Matrix Dpose, Dpoint;
|
||||
Unit3 result = camera.project(point1, Dpose, Dpoint);
|
||||
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_point, Dpoint, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Vector2 reprojectionError2(const Pose3& pose, const Point3& point,
|
||||
const Unit3& measured) {
|
||||
return Camera(pose).reprojectionError(point, measured);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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);
|
||||
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) {
|
||||
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);
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Add a test with more arbitrary rotation
|
||||
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_point = numericalDerivative22(project3, pose1, point1);
|
||||
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
|
||||
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
|
@ -10,22 +10,23 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* testTriangulation.cpp
|
||||
*
|
||||
* Created on: July 30th, 2013
|
||||
* Author: cbeall3
|
||||
* @file testTriangulation.cpp
|
||||
* @brief triangulation utilities
|
||||
* @date July 30th, 2013
|
||||
* @author Chris Beall (cbeall3)
|
||||
* @author Luca Carlone
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/triangulation.h>
|
||||
#include <gtsam/geometry/PinholeCamera.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/nonlinear/ExpressionFactor.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/triangulation.h>
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/slam/StereoFactor.h>
|
||||
|
||||
#include <boost/assign.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
|
@ -36,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)
|
||||
|
@ -57,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;
|
||||
|
||||
|
@ -69,36 +69,36 @@ TEST( triangulation, twoPoses) {
|
|||
|
||||
// 1. Test simple DLT, perfect in no noise situation
|
||||
bool optimize = false;
|
||||
boost::optional<Point3> actual1 = //
|
||||
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
|
||||
boost::optional<Point3> actual1 = //
|
||||
triangulatePoint3<Cal3_S2>(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 = //
|
||||
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
|
||||
boost::optional<Point3> actual2 = //
|
||||
triangulatePoint3<Cal3_S2>(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 = //
|
||||
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
|
||||
boost::optional<Point3> actual3 = //
|
||||
triangulatePoint3<Cal3_S2>(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 = //
|
||||
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
|
||||
boost::optional<Point3> actual4 = //
|
||||
triangulatePoint3<Cal3_S2>(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);
|
||||
|
@ -116,37 +116,38 @@ TEST( triangulation, twoPosesBundler) {
|
|||
bool optimize = true;
|
||||
double rank_tol = 1e-9;
|
||||
|
||||
boost::optional<Point3> actual = //
|
||||
triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
|
||||
boost::optional<Point3> actual = //
|
||||
triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(landmark, *actual, 1e-7));
|
||||
|
||||
// Add some noise and try again
|
||||
measurements.at(0) += Point2(0.1, 0.5);
|
||||
measurements.at(1) += Point2(-0.2, 0.3);
|
||||
|
||||
boost::optional<Point3> actual2 = //
|
||||
triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
|
||||
boost::optional<Point3> actual2 = //
|
||||
triangulatePoint3<Cal3Bundler>(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<Cal3_S2>(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 = //
|
||||
triangulatePoint3(poses, sharedCal, measurements);
|
||||
boost::optional<Point3> actual2 = //
|
||||
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
|
||||
EXPECT(assert_equal(landmark, *actual2, 1e-2));
|
||||
|
||||
// 3. Add a slightly rotated third camera above, again with measurement noise
|
||||
|
@ -157,13 +158,13 @@ TEST( triangulation, fourPoses) {
|
|||
poses += pose3;
|
||||
measurements += z3 + Point2(0.1, -0.1);
|
||||
|
||||
boost::optional<Point3> triangulated_3cameras = //
|
||||
triangulatePoint3(poses, sharedCal, measurements);
|
||||
boost::optional<Point3> triangulated_3cameras = //
|
||||
triangulatePoint3<Cal3_S2>(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<Cal3_S2>(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
|
||||
|
@ -176,13 +177,13 @@ TEST( triangulation, fourPoses) {
|
|||
poses += pose4;
|
||||
measurements += Point2(400, 400);
|
||||
|
||||
CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
|
||||
TriangulationCheiralityException);
|
||||
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
|
||||
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);
|
||||
|
@ -195,22 +196,23 @@ 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 = //
|
||||
triangulatePoint3(cameras, measurements);
|
||||
boost::optional<Point3> actual = //
|
||||
triangulatePoint3<Cal3_S2>(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 = //
|
||||
triangulatePoint3(cameras, measurements);
|
||||
boost::optional<Point3> actual2 = //
|
||||
triangulatePoint3<Cal3_S2>(cameras, measurements);
|
||||
EXPECT(assert_equal(landmark, *actual2, 1e-2));
|
||||
|
||||
// 3. Add a slightly rotated third camera above, again with measurement noise
|
||||
|
@ -222,13 +224,13 @@ TEST( triangulation, fourPoses_distinct_Ks) {
|
|||
cameras += camera3;
|
||||
measurements += z3 + Point2(0.1, -0.1);
|
||||
|
||||
boost::optional<Point3> triangulated_3cameras = //
|
||||
triangulatePoint3(cameras, measurements);
|
||||
boost::optional<Point3> triangulated_3cameras = //
|
||||
triangulatePoint3<Cal3_S2>(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<Cal3_S2>(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
|
||||
|
@ -241,13 +243,13 @@ TEST( triangulation, fourPoses_distinct_Ks) {
|
|||
|
||||
cameras += camera4;
|
||||
measurements += Point2(400, 400);
|
||||
CHECK_EXCEPTION(triangulatePoint3(cameras, measurements),
|
||||
TriangulationCheiralityException);
|
||||
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(cameras, measurements),
|
||||
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);
|
||||
|
@ -260,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);
|
||||
|
@ -286,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);
|
||||
|
||||
|
@ -313,12 +322,12 @@ TEST( triangulation, twoIdenticalPoses) {
|
|||
poses += pose1, pose1;
|
||||
measurements += z1, z1;
|
||||
|
||||
CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
|
||||
TriangulationUnderconstrainedException);
|
||||
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
|
||||
TriangulationUnderconstrainedException);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST( triangulation, onePose) {
|
||||
TEST(triangulation, onePose) {
|
||||
// we expect this test to fail with a TriangulationUnderconstrainedException
|
||||
// because there's only one camera observation
|
||||
|
||||
|
@ -326,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);
|
||||
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
|
||||
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;
|
||||
|
@ -358,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 actual = triangulateNonlinear<StereoCamera>(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));
|
||||
|
@ -378,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
|
||||
|
@ -399,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
|
||||
|
@ -416,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_);
|
||||
|
@ -428,10 +442,172 @@ 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) {
|
||||
vector<Pose3> poses;
|
||||
std::vector<Unit3> measurements;
|
||||
|
||||
// Project landmark into two cameras and triangulate
|
||||
SphericalCamera cam1(pose1);
|
||||
SphericalCamera cam2(pose2);
|
||||
Unit3 u1 = cam1.project(landmark);
|
||||
Unit3 u2 = cam2.project(landmark);
|
||||
|
||||
poses += pose1, pose2;
|
||||
measurements += u1, u2;
|
||||
|
||||
CameraSet<SphericalCamera> cameras;
|
||||
cameras.push_back(cam1);
|
||||
cameras.push_back(cam2);
|
||||
|
||||
double rank_tol = 1e-9;
|
||||
|
||||
// 1. Test linear triangulation via DLT
|
||||
auto projection_matrices = projectionMatricesFromCameras(cameras);
|
||||
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
|
||||
EXPECT(assert_equal(landmark, point, 1e-7));
|
||||
|
||||
// 2. Test nonlinear triangulation
|
||||
point = triangulateNonlinear<SphericalCamera>(cameras, measurements, point);
|
||||
EXPECT(assert_equal(landmark, point, 1e-7));
|
||||
|
||||
// 3. Test simple DLT, now within triangulatePoint3
|
||||
bool optimize = false;
|
||||
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);
|
||||
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
|
||||
measurements.at(1) = u2.retract(Vector2(-0.02, 0.03));
|
||||
optimize = false;
|
||||
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);
|
||||
EXPECT(assert_equal(Point3(5.9432, 0.654334, 1.48192), *actual4, 1e-3));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
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
|
||||
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
|
||||
|
||||
poses += pose1, pose2;
|
||||
measurements += u1, u2;
|
||||
|
||||
CameraSet<SphericalCamera> cameras;
|
||||
cameras.push_back(cam1);
|
||||
cameras.push_back(cam2);
|
||||
|
||||
double rank_tol = 1e-9;
|
||||
|
||||
{
|
||||
// 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));
|
||||
#endif
|
||||
}
|
||||
{
|
||||
// 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));
|
||||
#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
|
||||
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);
|
||||
Vector2 px = pinholeCamera.project(landmarkL);
|
||||
|
||||
// add perturbation and compare error in both cameras
|
||||
Vector2 px_noise(1.0, 2.0); // px 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 expected_measured_u =
|
||||
Unit3(px_noise[0] / sharedK->fx(), px_noise[1] / sharedK->fy(), 1.0);
|
||||
EXPECT(assert_equal(expected_measured_u, measured_u, 1e-7));
|
||||
|
||||
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);
|
||||
// expectedError: not easy to calculate, since it involves the unit3 basis
|
||||
Vector2 expectedErrorSpherical(-0.00360842, 0.00180419);
|
||||
EXPECT(assert_equal(expectedErrorSpherical, actualErrorSpherical, 1e-7));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
@ -53,15 +53,57 @@ Vector4 triangulateHomogeneousDLT(
|
|||
return v;
|
||||
}
|
||||
|
||||
Point3 triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
Vector4 triangulateHomogeneousDLT(
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const std::vector<Unit3>& measurements, double rank_tol) {
|
||||
|
||||
// number of cameras
|
||||
size_t m = projection_matrices.size();
|
||||
|
||||
// Allocate DLT matrix
|
||||
Matrix A = Matrix::Zero(m * 2, 4);
|
||||
|
||||
for (size_t i = 0; i < m; i++) {
|
||||
size_t row = i * 2;
|
||||
const Matrix34& projection = projection_matrices.at(i);
|
||||
const Point3& p = measurements.at(i).point3(); // to get access to x,y,z of the bearing vector
|
||||
|
||||
// build system of equations
|
||||
A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
|
||||
A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1);
|
||||
}
|
||||
int rank;
|
||||
double error;
|
||||
Vector v;
|
||||
boost::tie(rank, error, v) = DLT(A, rank_tol);
|
||||
|
||||
if (rank < 3)
|
||||
throw(TriangulationUnderconstrainedException());
|
||||
|
||||
return v;
|
||||
}
|
||||
|
||||
Point3 triangulateDLT(
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const Point2Vector& measurements, double rank_tol) {
|
||||
|
||||
Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
|
||||
|
||||
Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
|
||||
rank_tol);
|
||||
// Create 3D point from homogeneous coordinates
|
||||
return Point3(v.head<3>() / v[3]);
|
||||
}
|
||||
|
||||
Point3 triangulateDLT(
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const std::vector<Unit3>& measurements, double rank_tol) {
|
||||
|
||||
// contrary to previous triangulateDLT, this is now taking Unit3 inputs
|
||||
Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
|
||||
rank_tol);
|
||||
// Create 3D point from homogeneous coordinates
|
||||
return Point3(v.head<3>() / v[3]);
|
||||
}
|
||||
|
||||
///
|
||||
/**
|
||||
* Optimize for triangulation
|
||||
|
@ -71,7 +113,7 @@ Point3 triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matri
|
|||
* @return refined Point3
|
||||
*/
|
||||
Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
|
||||
Key landmarkKey) {
|
||||
Key landmarkKey) {
|
||||
// Maybe we should consider Gauss-Newton?
|
||||
LevenbergMarquardtParams params;
|
||||
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
|
|
|
@ -24,6 +24,7 @@
|
|||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/CameraSet.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/SphericalCamera.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
@ -59,6 +60,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
|
|||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const Point2Vector& measurements, double rank_tol = 1e-9);
|
||||
|
||||
/**
|
||||
* Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors
|
||||
* (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
|
||||
* @param projection_matrices Projection matrices (K*P^-1)
|
||||
* @param measurements Unit3 bearing measurements
|
||||
* @param rank_tol SVD rank tolerance
|
||||
* @return Triangulated point, in homogeneous coordinates
|
||||
*/
|
||||
GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const std::vector<Unit3>& measurements, double rank_tol = 1e-9);
|
||||
|
||||
/**
|
||||
* DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
|
||||
* @param projection_matrices Projection matrices (K*P^-1)
|
||||
|
@ -71,6 +84,14 @@ GTSAM_EXPORT Point3 triangulateDLT(
|
|||
const Point2Vector& measurements,
|
||||
double rank_tol = 1e-9);
|
||||
|
||||
/**
|
||||
* overload of previous function to work with Unit3 (projected to canonical camera)
|
||||
*/
|
||||
GTSAM_EXPORT Point3 triangulateDLT(
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const std::vector<Unit3>& measurements,
|
||||
double rank_tol = 1e-9);
|
||||
|
||||
/**
|
||||
* Create a factor graph with projection factors from poses and one calibration
|
||||
* @param poses Camera poses
|
||||
|
@ -180,26 +201,27 @@ Point3 triangulateNonlinear(
|
|||
return optimize(graph, values, Symbol('p', 0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a 3*4 camera projection matrix from calibration and pose.
|
||||
* Functor for partial application on calibration
|
||||
* @param pose The camera pose
|
||||
* @param cal The calibration
|
||||
* @return Returns a Matrix34
|
||||
*/
|
||||
template<class CAMERA>
|
||||
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
|
||||
projectionMatricesFromCameras(const CameraSet<CAMERA> &cameras) {
|
||||
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
|
||||
for (const CAMERA &camera: cameras) {
|
||||
projection_matrices.push_back(camera.cameraProjectionMatrix());
|
||||
}
|
||||
return projection_matrices;
|
||||
}
|
||||
|
||||
// overload, assuming pinholePose
|
||||
template<class CALIBRATION>
|
||||
struct CameraProjectionMatrix {
|
||||
CameraProjectionMatrix(const CALIBRATION& calibration) :
|
||||
K_(calibration.K()) {
|
||||
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses(
|
||||
const std::vector<Pose3> &poses, boost::shared_ptr<CALIBRATION> sharedCal) {
|
||||
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
|
||||
for (size_t i = 0; i < poses.size(); i++) {
|
||||
PinholePose<CALIBRATION> camera(poses.at(i), sharedCal);
|
||||
projection_matrices.push_back(camera.cameraProjectionMatrix());
|
||||
}
|
||||
Matrix34 operator()(const Pose3& pose) const {
|
||||
return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0);
|
||||
}
|
||||
private:
|
||||
const Matrix3 K_;
|
||||
public:
|
||||
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
return projection_matrices;
|
||||
}
|
||||
|
||||
/**
|
||||
* Function to triangulate 3D landmark point from an arbitrary number
|
||||
|
@ -224,10 +246,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
|
|||
throw(TriangulationUnderconstrainedException());
|
||||
|
||||
// construct projection matrices from poses & calibration
|
||||
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
|
||||
CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
|
||||
for(const Pose3& pose: poses)
|
||||
projection_matrices.push_back(createP(pose));
|
||||
auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
|
||||
|
||||
// Triangulate linearly
|
||||
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
|
||||
|
@ -274,11 +293,7 @@ Point3 triangulatePoint3(
|
|||
throw(TriangulationUnderconstrainedException());
|
||||
|
||||
// construct projection matrices from poses & calibration
|
||||
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
|
||||
for(const CAMERA& camera: cameras)
|
||||
projection_matrices.push_back(
|
||||
CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(
|
||||
camera.pose()));
|
||||
auto projection_matrices = projectionMatricesFromCameras(cameras);
|
||||
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
|
||||
|
||||
// The n refine using non-linear optimization
|
||||
|
@ -474,8 +489,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
|
|||
#endif
|
||||
// Check reprojection error
|
||||
if (params.dynamicOutlierRejectionThreshold > 0) {
|
||||
const Point2& zi = measured.at(i);
|
||||
Point2 reprojectionError(camera.project(point) - zi);
|
||||
const typename CAMERA::Measurement& zi = measured.at(i);
|
||||
Point2 reprojectionError = camera.reprojectionError(point, zi);
|
||||
maxReprojError = std::max(maxReprojError, reprojectionError.norm());
|
||||
}
|
||||
i += 1;
|
||||
|
@ -503,6 +518,6 @@ using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
|
|||
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
|
||||
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
|
||||
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
|
||||
|
||||
using CameraSetSpherical = CameraSet<SphericalCamera>;
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
|
@ -54,6 +54,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
typedef SmartProjectionFactor<CAMERA> Base;
|
||||
typedef SmartProjectionRigFactor<CAMERA> This;
|
||||
typedef typename CAMERA::CalibrationType CALIBRATION;
|
||||
typedef typename CAMERA::Measurement MEASUREMENT;
|
||||
typedef typename CAMERA::MeasurementVector MEASUREMENTS;
|
||||
|
||||
static const int DimPose = 6; ///< Pose3 dimension
|
||||
static const int ZDim = 2; ///< Measurement dimension
|
||||
|
@ -118,7 +120,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
* @param cameraId ID of the camera in the rig taking the measurement (default
|
||||
* 0)
|
||||
*/
|
||||
void add(const Point2& measured, const Key& poseKey,
|
||||
void add(const MEASUREMENT& measured, const Key& poseKey,
|
||||
const size_t& cameraId = 0) {
|
||||
// store measurement and key
|
||||
this->measured_.push_back(measured);
|
||||
|
@ -144,7 +146,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
* @param cameraIds IDs of the cameras in the rig taking each measurement
|
||||
* (same order as the measurements)
|
||||
*/
|
||||
void add(const Point2Vector& measurements, const KeyVector& poseKeys,
|
||||
void add(const MEASUREMENTS& measurements, const KeyVector& poseKeys,
|
||||
const FastVector<size_t>& cameraIds = FastVector<size_t>()) {
|
||||
if (poseKeys.size() != measurements.size() ||
|
||||
(poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) {
|
||||
|
|
|
@ -129,7 +129,7 @@ public:
|
|||
<< std::endl;
|
||||
if (throwCheirality_)
|
||||
throw e;
|
||||
return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * camera_.calibration().fx());
|
||||
return camera_.defaultErrorWhenTriangulatingBehindCamera();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -17,11 +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;
|
||||
|
@ -44,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;
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
@ -63,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
|
||||
|
@ -78,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
|
||||
|
@ -93,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
|
||||
|
@ -111,7 +113,8 @@ Camera cam1(level_pose, K);
|
|||
Camera cam2(pose_right, K);
|
||||
Camera cam3(pose_above, K);
|
||||
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
|
||||
}
|
||||
} // namespace bundler
|
||||
|
||||
/* *************************************************************************/
|
||||
// Cal3Bundler poses
|
||||
namespace bundlerPose {
|
||||
|
@ -119,35 +122,50 @@ 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
|
||||
namespace sphericalCamera {
|
||||
typedef SphericalCamera Camera;
|
||||
typedef CameraSet<Camera> Cameras;
|
||||
typedef SmartProjectionRigFactor<Camera> SmartFactorP;
|
||||
static EmptyCal::shared_ptr emptyK;
|
||||
Camera level_camera(level_pose);
|
||||
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) {
|
||||
Point2 cam1_uv1 = cam1.project(landmark);
|
||||
Point2 cam2_uv1 = cam2.project(landmark);
|
||||
Point2 cam3_uv1 = cam3.project(landmark);
|
||||
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);
|
||||
measurements_cam.push_back(cam1_uv1);
|
||||
measurements_cam.push_back(cam2_uv1);
|
||||
measurements_cam.push_back(cam3_uv1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
@ -55,8 +55,6 @@ Key cameraId3 = 2;
|
|||
static Point2 measurement1(323.0, 240.0);
|
||||
|
||||
LevenbergMarquardtParams lmParams;
|
||||
// Make more verbose like so (in tests):
|
||||
// params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// default Cal3_S2 poses with rolling shutter effect
|
||||
|
@ -1187,10 +1185,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.06 CPU
|
||||
//(10000 times, 0.061226 wall, 0.06 children, min: 0 max: 0)
|
||||
//| -SmartPoseFactor LINEARIZE: 0.06 CPU
|
||||
//(10000 times, 0.073037 wall, 0.06 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;
|
||||
|
@ -1249,6 +1246,355 @@ TEST(SmartProjectionRigFactor, timing) {
|
|||
}
|
||||
#endif
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
|
||||
using namespace sphericalCamera;
|
||||
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);
|
||||
|
||||
// create inputs
|
||||
KeyVector keys;
|
||||
keys.push_back(x1);
|
||||
keys.push_back(x2);
|
||||
keys.push_back(x3);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), emptyK));
|
||||
|
||||
SmartProjectionParams params(
|
||||
gtsam::HESSIAN,
|
||||
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
|
||||
params.setRankTolerance(0.1);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor1(
|
||||
new SmartFactorP(model, cameraRig, params));
|
||||
smartFactor1->add(measurements_lmk1, keys);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor2(
|
||||
new SmartFactorP(model, cameraRig, params));
|
||||
smartFactor2->add(measurements_lmk2, keys);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor3(
|
||||
new SmartFactorP(model, cameraRig, params));
|
||||
smartFactor3->add(measurements_lmk3, keys);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.addPrior(x1, level_pose, noisePrior);
|
||||
graph.addPrior(x2, pose_right, noisePrior);
|
||||
|
||||
Values groundTruth;
|
||||
groundTruth.insert(x1, level_pose);
|
||||
groundTruth.insert(x2, pose_right);
|
||||
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 / 100),
|
||||
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
|
||||
values.insert(x3, pose_above * noise_pose);
|
||||
|
||||
DOUBLES_EQUAL(0.94148963675515274, graph.error(values), 1e-9);
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
|
||||
}
|
||||
|
||||
#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)
|
||||
/* *************************************************************************/
|
||||
TEST(SmartProjectionFactorP, timing_sphericalCamera) {
|
||||
// create common data
|
||||
Rot3 R = Rot3::identity();
|
||||
Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
|
||||
Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
|
||||
Pose3 body_P_sensorId = Pose3::identity();
|
||||
Point3 landmark1(0, 0, 10);
|
||||
|
||||
// create spherical data
|
||||
EmptyCal::shared_ptr emptyK;
|
||||
SphericalCamera cam1_sphere(pose1, emptyK), cam2_sphere(pose2, emptyK);
|
||||
// Project 2 landmarks into 2 cameras
|
||||
std::vector<Unit3> measurements_lmk1_sphere;
|
||||
measurements_lmk1_sphere.push_back(cam1_sphere.project(landmark1));
|
||||
measurements_lmk1_sphere.push_back(cam2_sphere.project(landmark1));
|
||||
|
||||
// create Cal3_S2 data
|
||||
static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
|
||||
PinholePose<Cal3_S2> cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
|
||||
// Project 2 landmarks into 2 cameras
|
||||
std::vector<Point2> measurements_lmk1;
|
||||
measurements_lmk1.push_back(cam1.project(landmark1));
|
||||
measurements_lmk1.push_back(cam2.project(landmark1));
|
||||
|
||||
SmartProjectionParams params(
|
||||
gtsam::HESSIAN,
|
||||
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
|
||||
|
||||
size_t nrTests = 1000;
|
||||
|
||||
for (size_t i = 0; i < nrTests; i++) {
|
||||
boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
|
||||
new CameraSet<SphericalCamera>()); // single camera in the rig
|
||||
cameraRig->push_back(SphericalCamera(body_P_sensorId, emptyK));
|
||||
|
||||
SmartProjectionRigFactor<SphericalCamera>::shared_ptr smartFactorP(
|
||||
new SmartProjectionRigFactor<SphericalCamera>(model, cameraRig,
|
||||
params));
|
||||
smartFactorP->add(measurements_lmk1_sphere[0], x1);
|
||||
smartFactorP->add(measurements_lmk1_sphere[1], x1);
|
||||
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
gttic_(SmartFactorP_spherical_LINEARIZE);
|
||||
smartFactorP->linearize(values);
|
||||
gttoc_(SmartFactorP_spherical_LINEARIZE);
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < nrTests; i++) {
|
||||
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
|
||||
new CameraSet<PinholePose<Cal3_S2>>()); // 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));
|
||||
smartFactorP2->add(measurements_lmk1[0], x1);
|
||||
smartFactorP2->add(measurements_lmk1[1], x1);
|
||||
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
gttic_(SmartFactorP_pinhole_LINEARIZE);
|
||||
smartFactorP2->linearize(values);
|
||||
gttoc_(SmartFactorP_pinhole_LINEARIZE);
|
||||
}
|
||||
tictoc_print_();
|
||||
}
|
||||
#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
|
||||
|
||||
// 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
|
||||
|
||||
Camera cam1(poseA, sharedK);
|
||||
Camera cam2(poseB, sharedK);
|
||||
|
||||
SmartProjectionParams params(
|
||||
gtsam::HESSIAN,
|
||||
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
|
||||
params.setRankTolerance(1);
|
||||
|
||||
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
|
||||
new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
|
||||
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), sharedK));
|
||||
|
||||
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);
|
||||
|
||||
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));
|
||||
}
|
||||
// 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);
|
||||
|
||||
SmartProjectionParams params(
|
||||
gtsam::HESSIAN,
|
||||
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
|
||||
params.setRankTolerance(0.1);
|
||||
|
||||
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
|
||||
new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
|
||||
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
|
||||
|
||||
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);
|
||||
|
||||
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()); // 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
|
||||
|
||||
Camera cam1(poseA, canonicalK);
|
||||
Camera cam2(poseB, canonicalK);
|
||||
|
||||
SmartProjectionParams params(
|
||||
gtsam::HESSIAN,
|
||||
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
|
||||
params.setRankTolerance(0.01);
|
||||
|
||||
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
|
||||
new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
|
||||
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
|
||||
|
||||
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);
|
||||
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
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
|
||||
|
||||
Camera cam1(poseA);
|
||||
Camera cam2(poseB);
|
||||
|
||||
boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
|
||||
new CameraSet<SphericalCamera>()); // single camera in the rig
|
||||
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
|
||||
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);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
|
||||
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
|
||||
}
|
||||
// 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(
|
||||
gtsam::HESSIAN,
|
||||
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
|
||||
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);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
@ -47,6 +47,8 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
typedef SmartProjectionFactor<CAMERA> Base;
|
||||
typedef SmartProjectionPoseFactorRollingShutter<CAMERA> This;
|
||||
typedef typename CAMERA::CalibrationType CALIBRATION;
|
||||
typedef typename CAMERA::Measurement MEASUREMENT;
|
||||
typedef typename CAMERA::MeasurementVector MEASUREMENTS;
|
||||
|
||||
protected:
|
||||
/// The keys of the pose of the body (with respect to an external world
|
||||
|
@ -68,12 +70,6 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
typedef CAMERA Camera;
|
||||
typedef CameraSet<CAMERA> Cameras;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
static const int DimBlock =
|
||||
12; ///< size of the variable stacking 2 poses from which the observation
|
||||
///< pose is interpolated
|
||||
|
@ -84,6 +80,12 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
|
||||
FBlocks; // vector of F blocks
|
||||
|
||||
typedef CAMERA Camera;
|
||||
typedef CameraSet<CAMERA> Cameras;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// Default constructor, only for serialization
|
||||
SmartProjectionPoseFactorRollingShutter() {}
|
||||
|
||||
|
@ -125,7 +127,7 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
* interpolated pose is the same as world_P_body_key1
|
||||
* @param cameraId ID of the camera taking the measurement (default 0)
|
||||
*/
|
||||
void add(const Point2& measured, const Key& world_P_body_key1,
|
||||
void add(const MEASUREMENT& measured, const Key& world_P_body_key1,
|
||||
const Key& world_P_body_key2, const double& alpha,
|
||||
const size_t& cameraId = 0) {
|
||||
// store measurements in base class
|
||||
|
@ -164,7 +166,7 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
* @param cameraIds IDs of the cameras taking each measurement (same order as
|
||||
* the measurements)
|
||||
*/
|
||||
void add(const Point2Vector& measurements,
|
||||
void add(const MEASUREMENTS& measurements,
|
||||
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
|
||||
const std::vector<double>& alphas,
|
||||
const FastVector<size_t>& cameraIds = FastVector<size_t>()) {
|
||||
|
@ -330,12 +332,13 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
|
||||
auto body_P_cam = camera_i.pose();
|
||||
auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
|
||||
PinholeCamera<CALIBRATION> camera(w_P_cam, camera_i.calibration());
|
||||
typename Base::Camera camera(
|
||||
w_P_cam, make_shared<typename CAMERA::CalibrationType>(
|
||||
camera_i.calibration()));
|
||||
|
||||
// get jacobians and error vector for current measurement
|
||||
Point2 reprojectionError_i =
|
||||
Point2(camera.project(*this->result_, dProject_dPoseCam, Ei) -
|
||||
this->measured_.at(i));
|
||||
Point2 reprojectionError_i = camera.reprojectionError(
|
||||
*this->result_, this->measured_.at(i), dProject_dPoseCam, Ei);
|
||||
Eigen::Matrix<double, ZDim, DimBlock> J; // 2 x 12
|
||||
J.block(0, 0, ZDim, 6) =
|
||||
dProject_dPoseCam * dPoseCam_dInterpPose *
|
||||
|
@ -403,7 +406,7 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
for (size_t i = 0; i < Fs.size(); i++)
|
||||
Fs[i] = this->noiseModel_->Whiten(Fs[i]);
|
||||
|
||||
Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
|
||||
Matrix3 P = Cameras::PointCov(E, lambda, diagonalDamping);
|
||||
|
||||
// Collect all the key pairs: these are the keys that correspond to the
|
||||
// blocks in Fs (on which we apply the Schur Complement)
|
||||
|
|
|
@ -1317,10 +1317,10 @@ TEST(SmartProjectionPoseFactorRollingShutter,
|
|||
#ifndef DISABLE_TIMING
|
||||
#include <gtsam/base/timing.h>
|
||||
//-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0)
|
||||
//| -SF RS LINEARIZE: 0.09 CPU
|
||||
// (10000 times, 0.124106 wall, 0.09 children, min: 0 max: 0)
|
||||
//| -RS LINEARIZE: 0.09 CPU
|
||||
// (10000 times, 0.068719 wall, 0.09 children, min: 0 max: 0)
|
||||
//| -SF RS LINEARIZE: 0.14 CPU
|
||||
//(10000 times, 0.131202 wall, 0.14 children, min: 0 max: 0)
|
||||
//| -RS LINEARIZE: 0.06 CPU
|
||||
//(10000 times, 0.066951 wall, 0.06 children, min: 0 max: 0)
|
||||
/* *************************************************************************/
|
||||
TEST(SmartProjectionPoseFactorRollingShutter, timing) {
|
||||
using namespace vanillaPose;
|
||||
|
@ -1384,6 +1384,105 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
|
|||
}
|
||||
#endif
|
||||
|
||||
#include <gtsam/geometry/SphericalCamera.h>
|
||||
/* ************************************************************************* */
|
||||
// spherical Camera with rolling shutter effect
|
||||
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);
|
||||
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) {
|
||||
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);
|
||||
|
||||
// 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<double> interp_factors;
|
||||
interp_factors.push_back(interp_factor1);
|
||||
interp_factors.push_back(interp_factor2);
|
||||
interp_factors.push_back(interp_factor3);
|
||||
|
||||
SmartProjectionParams params(
|
||||
gtsam::HESSIAN,
|
||||
gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
|
||||
params.setRankTolerance(0.1);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), emptyK));
|
||||
|
||||
SmartFactorRS_spherical::shared_ptr smartFactor1(
|
||||
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));
|
||||
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
|
||||
|
||||
SmartFactorRS_spherical::shared_ptr smartFactor3(
|
||||
new SmartFactorRS_spherical(model, cameraRig, params));
|
||||
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.addPrior(x1, level_pose, noisePrior);
|
||||
graph.addPrior(x2, pose_right, noisePrior);
|
||||
|
||||
Values groundTruth;
|
||||
groundTruth.insert(x1, level_pose);
|
||||
groundTruth.insert(x2, pose_right);
|
||||
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 / 100, 0., -M_PI / 100),
|
||||
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
|
||||
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)));
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
Loading…
Reference in New Issue