From 1f55e06a58253edd04a470f880d683cddbf3bae2 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 26 Aug 2021 18:06:00 -0400 Subject: [PATCH] done with tests --- ...SpericalCamera.cpp => SphericalCamera.cpp} | 51 +-- .../{SpericalCamera.h => SphericalCamera.h} | 35 +- gtsam/geometry/tests/testSphericalCamera.cpp | 316 +++--------------- 3 files changed, 79 insertions(+), 323 deletions(-) rename gtsam/geometry/{SpericalCamera.cpp => SphericalCamera.cpp} (53%) rename gtsam/geometry/{SpericalCamera.h => SphericalCamera.h} (84%) diff --git a/gtsam/geometry/SpericalCamera.cpp b/gtsam/geometry/SphericalCamera.cpp similarity index 53% rename from gtsam/geometry/SpericalCamera.cpp rename to gtsam/geometry/SphericalCamera.cpp index 9d412310c..b106b32d3 100644 --- a/gtsam/geometry/SpericalCamera.cpp +++ b/gtsam/geometry/SphericalCamera.cpp @@ -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 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); } /* ************************************************************************* */ diff --git a/gtsam/geometry/SpericalCamera.h b/gtsam/geometry/SphericalCamera.h similarity index 84% rename from gtsam/geometry/SpericalCamera.h rename to gtsam/geometry/SphericalCamera.h index e3ae10c23..bbd9f7e8d 100644 --- a/gtsam/geometry/SpericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -128,7 +128,7 @@ public: /// @{ /// Project a point into the image and check depth - std::pair projectSafe(const Point3& pw) const; + std::pair 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 + +} diff --git a/gtsam/geometry/tests/testSphericalCamera.cpp b/gtsam/geometry/tests/testSphericalCamera.cpp index 0679a4609..cd1886412 100644 --- a/gtsam/geometry/tests/testSphericalCamera.cpp +++ b/gtsam/geometry/tests/testSphericalCamera.cpp @@ -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 -#include -#include -#include +#include #include #include @@ -31,322 +29,100 @@ using namespace std::placeholders; using namespace std; using namespace gtsam; -typedef PinholeCamera 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 f = // - std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2, - boost::none, boost::none); - Matrix numericalH1 = numericalDerivative21(f,pose,K); - EXPECT(assert_equal(numericalH1, actualH1, 1e-9)); - Matrix numericalH2 = numericalDerivative22(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 f = // - std::bind(&Camera::getPose, std::placeholders::_1, boost::none); - Matrix numericalH = numericalDerivative11(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 x = camera.projectSafe(expected); + pair 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 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(camera2); -} - -/* ************************************************************************* */ -TEST( PinholeCamera, range2) { - Matrix D1; Matrix D2; - double result = camera.range(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 camera(wTc, calibration); - Point2 p(50, 100); - camera.backproject(p, 10); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */