done with tests
lcarlone
parent
ce7e357ea7
commit
1f55e06a58
|
|
@ -22,28 +22,6 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix26 SphericalCamera::Dpose(const Point2& pn, double d) {
|
||||
// // optimized version of derivatives, see CalibratedCamera.nb
|
||||
// const double u = pn.x(), v = pn.y();
|
||||
// double uv = u * v, uu = u * u, vv = v * v;
|
||||
// Matrix26 Dpn_pose;
|
||||
// Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
|
||||
// return Dpn_pose;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix23 SphericalCamera::Dpoint(const Point2& pn, double d, const Matrix3& Rt) {
|
||||
// // optimized version of derivatives, see CalibratedCamera.nb
|
||||
// const double u = pn.x(), v = pn.y();
|
||||
// Matrix23 Dpn_point;
|
||||
// Dpn_point << //
|
||||
// Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), //
|
||||
// /**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2);
|
||||
// Dpn_point *= d;
|
||||
// return Dpn_point;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool SphericalCamera::equals(const SphericalCamera &camera, double tol) const {
|
||||
return pose_.equals(camera.pose(), tol);
|
||||
|
|
@ -57,39 +35,34 @@ void SphericalCamera::print(const string& s) const {
|
|||
/* ************************************************************************* */
|
||||
pair<Unit3, bool> SphericalCamera::projectSafe(const Point3& pw) const {
|
||||
const Point3 pc = pose().transformTo(pw);
|
||||
Unit3::FromPoint3(pc);
|
||||
return make_pair(pn, pc.norm() > 1e-8);
|
||||
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 {
|
||||
|
||||
Matrix3 Dtf_pose, Dtf_point; // calculated by transformTo if needed
|
||||
const Point3 pc = pose().transformTo(pw, Dpoint ? &Dtf_pose : 0, Dpoint ? &Dtf_point : 0);
|
||||
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);
|
||||
|
||||
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
|
||||
if (pc.norm() <= 1e-8)
|
||||
throw CheiralityException();
|
||||
#endif
|
||||
Matrix Dunit; // calculated by FromPoint3 if needed
|
||||
Unit3 pn = Unit3::FromPoint3(Point3(pc), Dpoint ? &Dunit : 0);
|
||||
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 pn;
|
||||
return pu;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Unit3 SphericalCamera::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
|
||||
OptionalJacobian<2, 2> Dpoint) const {
|
||||
return project2(Point3(pw), Dpose, Dpoint);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Point3 SphericalCamera::BackprojectFromCamera(const Unit3& pu, const double depth) {
|
||||
return depth * pu;
|
||||
Point3 SphericalCamera::backproject(const Unit3& pu, const double depth) const {
|
||||
return pose().transformFrom(depth * pu);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -128,7 +128,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/// Project a point into the image and check depth
|
||||
std::pair<Point2, bool> projectSafe(const Point3& pw) const;
|
||||
std::pair<Unit3, bool> projectSafe(const Point3& pw) const;
|
||||
|
||||
/** Project point into the image
|
||||
* (note: there is no CheiralityException for a spherical camera)
|
||||
|
|
@ -138,19 +138,8 @@ public:
|
|||
Unit3 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
|
||||
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
|
||||
|
||||
/** Project point at infinity 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 Unit3& point,
|
||||
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
|
||||
static Point3 BackprojectFromCamera(const Point2& p, const double depth,
|
||||
OptionalJacobian<3, 2> Dpoint = boost::none,
|
||||
OptionalJacobian<3, 1> Ddepth = boost::none);
|
||||
Point3 backproject(const Unit3& p, const double depth) const;
|
||||
|
||||
/** Project point into the image
|
||||
* (note: there is no CheiralityException for a spherical camera)
|
||||
|
|
@ -161,6 +150,22 @@ public:
|
|||
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
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
|
|
@ -170,4 +175,6 @@ private:
|
|||
ar & BOOST_SERIALIZATION_NVP(pose_);
|
||||
}
|
||||
};
|
||||
// end of class PinholeBase
|
||||
// end of class SphericalCamera
|
||||
|
||||
}
|
||||
|
|
@ -10,15 +10,13 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testPinholeCamera.cpp
|
||||
* @author Frank Dellaert
|
||||
* @brief test PinholeCamera class
|
||||
* @file SphericalCamera.h
|
||||
* @brief Calibrated camera with spherical projection
|
||||
* @date Aug 26, 2021
|
||||
* @author Luca Carlone
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/SphericalCamera.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
|
|
@ -31,322 +29,100 @@ using namespace std::placeholders;
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef PinholeCamera<Cal3_S2> Camera;
|
||||
|
||||
static const Cal3_S2 K(625, 625, 0, 0, 0);
|
||||
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, K);
|
||||
|
||||
static const Camera camera(pose);
|
||||
//
|
||||
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
|
||||
static const Camera camera1(pose1, K);
|
||||
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);
|
||||
|
||||
static const Unit3 point1_inf(-0.16,-0.16, -1.0);
|
||||
static const Unit3 point2_inf(-0.16, 0.16, -1.0);
|
||||
static const Unit3 point3_inf( 0.16, 0.16, -1.0);
|
||||
static const Unit3 point4_inf( 0.16,-0.16, -1.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( PinholeCamera, constructor)
|
||||
TEST( SphericalCamera, constructor)
|
||||
{
|
||||
EXPECT(assert_equal( K, camera.calibration()));
|
||||
EXPECT(assert_equal( pose, camera.pose()));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(PinholeCamera, Create) {
|
||||
|
||||
Matrix actualH1, actualH2;
|
||||
EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2)));
|
||||
|
||||
// Check derivative
|
||||
std::function<Camera(Pose3, Cal3_S2)> f = //
|
||||
std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2,
|
||||
boost::none, boost::none);
|
||||
Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
|
||||
EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
|
||||
Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
|
||||
EXPECT(assert_equal(numericalH2, actualH2, 1e-8));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(PinholeCamera, Pose) {
|
||||
|
||||
Matrix actualH;
|
||||
EXPECT(assert_equal(pose, camera.getPose(actualH)));
|
||||
|
||||
// Check derivative
|
||||
std::function<Pose3(Camera)> f = //
|
||||
std::bind(&Camera::getPose, std::placeholders::_1, boost::none);
|
||||
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
|
||||
EXPECT(assert_equal(numericalH, actualH, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, level2)
|
||||
TEST( SphericalCamera, project)
|
||||
{
|
||||
// Create a level camera, looking in Y-direction
|
||||
Pose2 pose2(0.4,0.3,M_PI/2.0);
|
||||
Camera camera = Camera::Level(K, pose2, 0.1);
|
||||
|
||||
// expected
|
||||
Point3 x(1,0,0),y(0,0,-1),z(0,1,0);
|
||||
Rot3 wRc(x,y,z);
|
||||
Pose3 expected(wRc,Point3(0.4,0.3,0.1));
|
||||
EXPECT(assert_equal( camera.pose(), expected));
|
||||
// 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( PinholeCamera, lookat)
|
||||
TEST( SphericalCamera, backproject)
|
||||
{
|
||||
// Create a level camera, looking in Y-direction
|
||||
Point3 C(10,0,0);
|
||||
Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1));
|
||||
|
||||
// expected
|
||||
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
|
||||
Pose3 expected(Rot3(xc,yc,zc),C);
|
||||
EXPECT(assert_equal(camera.pose(), expected));
|
||||
|
||||
Point3 C2(30,0,10);
|
||||
Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1));
|
||||
|
||||
Matrix R = camera2.pose().rotation().matrix();
|
||||
Matrix I = trans(R)*R;
|
||||
EXPECT(assert_equal(I, I_3x3));
|
||||
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( PinholeCamera, project)
|
||||
{
|
||||
EXPECT(assert_equal( camera.project(point1), Point2(-100, 100) ));
|
||||
EXPECT(assert_equal( camera.project(point2), Point2(-100, -100) ));
|
||||
EXPECT(assert_equal( camera.project(point3), Point2( 100, -100) ));
|
||||
EXPECT(assert_equal( camera.project(point4), Point2( 100, 100) ));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, backproject)
|
||||
{
|
||||
EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1));
|
||||
EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2));
|
||||
EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3));
|
||||
EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, backprojectInfinity)
|
||||
{
|
||||
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf));
|
||||
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf));
|
||||
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf));
|
||||
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, backproject2)
|
||||
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), K);
|
||||
Camera camera(Pose3(rot, origin));
|
||||
|
||||
Point3 actual = camera.backproject(Point2(0,0), 1.);
|
||||
Point3 actual = camera.backproject(Unit3(0,0,1), 1.);
|
||||
Point3 expected(0., 1., 0.);
|
||||
pair<Point2, bool> x = camera.projectSafe(expected);
|
||||
pair<Unit3, bool> x = camera.projectSafe(expected);
|
||||
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
EXPECT(assert_equal(Point2(0,0), x.first));
|
||||
EXPECT(assert_equal(Unit3(0,0,1), x.first));
|
||||
EXPECT(x.second);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, backprojectInfinity2)
|
||||
{
|
||||
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), K);
|
||||
|
||||
Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0));
|
||||
Unit3 expected(0., 1., 0.);
|
||||
Point2 x = camera.project(expected);
|
||||
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
EXPECT(assert_equal(Point2(0,0), x));
|
||||
static Unit3 project3(const Pose3& pose, const Point3& point) {
|
||||
return Camera(pose).project(point);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, backprojectInfinity3)
|
||||
TEST( SphericalCamera, Dproject)
|
||||
{
|
||||
Point3 origin(0,0,0);
|
||||
Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity
|
||||
Camera camera(Pose3(rot, origin), K);
|
||||
|
||||
Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0));
|
||||
Unit3 expected(0., 0., 1.);
|
||||
Point2 x = camera.project(expected);
|
||||
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
EXPECT(assert_equal(Point2(0,0), x));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Point2 project3(const Pose3& pose, const Point3& point, const Cal3_S2& cal) {
|
||||
return Camera(pose,cal).project(point);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, Dproject)
|
||||
{
|
||||
Matrix Dpose, Dpoint, Dcal;
|
||||
Point2 result = camera.project(point1, Dpose, Dpoint, Dcal);
|
||||
Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K);
|
||||
Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K);
|
||||
Matrix numerical_cal = numericalDerivative33(project3, pose, point1, K);
|
||||
EXPECT(assert_equal(Point2(-100, 100), result));
|
||||
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(Hexpected2, Dpoint, 1e-7));
|
||||
EXPECT(assert_equal(numerical_cal, Dcal, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Point2 projectInfinity3(const Pose3& pose, const Unit3& point3D, const Cal3_S2& cal) {
|
||||
return Camera(pose,cal).project(point3D);
|
||||
}
|
||||
|
||||
TEST( PinholeCamera, Dproject_Infinity)
|
||||
{
|
||||
Matrix Dpose, Dpoint, Dcal;
|
||||
Unit3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1
|
||||
|
||||
// test Projection
|
||||
Point2 actual = camera.project(point3D, Dpose, Dpoint, Dcal);
|
||||
Point2 expected(-5.0, 5.0);
|
||||
EXPECT(assert_equal(actual, expected, 1e-7));
|
||||
|
||||
// test Jacobians
|
||||
Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose, point3D, K);
|
||||
Matrix Hexpected2 = numericalDerivative32(projectInfinity3, pose, point3D, K);
|
||||
Matrix numerical_point2x2 = Hexpected2.block(0,0,2,2); // only the direction to the point matters
|
||||
Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose, point3D, K);
|
||||
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
|
||||
EXPECT(assert_equal(numerical_point2x2, Dpoint, 1e-7));
|
||||
EXPECT(assert_equal(numerical_cal, Dcal, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Point2 project4(const Camera& camera, const Point3& point) {
|
||||
return camera.project2(point);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, Dproject2)
|
||||
{
|
||||
Matrix Dcamera, Dpoint;
|
||||
Point2 result = camera.project2(point1, Dcamera, Dpoint);
|
||||
Matrix Hexpected1 = numericalDerivative21(project4, camera, point1);
|
||||
Matrix Hexpected2 = numericalDerivative22(project4, camera, point1);
|
||||
EXPECT(assert_equal(result, Point2(-100, 100) ));
|
||||
EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7));
|
||||
EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
|
||||
EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Add a test with more arbitrary rotation
|
||||
TEST( PinholeCamera, Dproject3)
|
||||
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(project4, camera, point1);
|
||||
Matrix numerical_point = numericalDerivative22(project4, camera, point1);
|
||||
Matrix numerical_pose = numericalDerivative21(project3, pose1, point1);
|
||||
Matrix numerical_point = numericalDerivative22(project3, pose1, point1);
|
||||
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
|
||||
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static double range0(const Camera& camera, const Point3& point) {
|
||||
return camera.range(point);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, range0) {
|
||||
Matrix D1; Matrix D2;
|
||||
double result = camera.range(point1, D1, D2);
|
||||
Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
|
||||
Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
|
||||
EXPECT_DOUBLES_EQUAL(distance3(point1, camera.pose().translation()), result,
|
||||
1e-9);
|
||||
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
|
||||
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static double range1(const Camera& camera, const Pose3& pose) {
|
||||
return camera.range(pose);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, range1) {
|
||||
Matrix D1; Matrix D2;
|
||||
double result = camera.range(pose1, D1, D2);
|
||||
Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1);
|
||||
Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1);
|
||||
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
|
||||
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
|
||||
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
typedef PinholeCamera<Cal3Bundler> Camera2;
|
||||
static const Cal3Bundler K2(625, 1e-3, 1e-3);
|
||||
static const Camera2 camera2(pose1, K2);
|
||||
static double range2(const Camera& camera, const Camera2& camera2) {
|
||||
return camera.range<Cal3Bundler>(camera2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, range2) {
|
||||
Matrix D1; Matrix D2;
|
||||
double result = camera.range<Cal3Bundler>(camera2, D1, D2);
|
||||
Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2);
|
||||
Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2);
|
||||
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
|
||||
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
|
||||
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static const CalibratedCamera camera3(pose1);
|
||||
static double range3(const Camera& camera, const CalibratedCamera& camera3) {
|
||||
return camera.range(camera3);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, range3) {
|
||||
Matrix D1; Matrix D2;
|
||||
double result = camera.range(camera3, D1, D2);
|
||||
Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3);
|
||||
Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3);
|
||||
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
|
||||
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
|
||||
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, Cal3Bundler) {
|
||||
Cal3Bundler calibration;
|
||||
Pose3 wTc;
|
||||
PinholeCamera<Cal3Bundler> camera(wTc, calibration);
|
||||
Point2 p(50, 100);
|
||||
camera.backproject(p, 10);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue