From e5677e3805b1fed5ed2d94de10146fede3522ed7 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 26 Aug 2021 20:57:01 -0400 Subject: [PATCH] testing mode: step 1: need to figure out the manifold stuff --- gtsam/geometry/PinholePose.h | 7 + gtsam/geometry/SphericalCamera.cpp | 14 ++ gtsam/geometry/SphericalCamera.h | 115 ++++++++++------ gtsam/geometry/triangulation.h | 4 +- gtsam/slam/SmartProjectionFactorP.h | 6 +- gtsam/slam/tests/smartFactorScenarios.h | 20 ++- .../slam/tests/testSmartProjectionFactorP.cpp | 127 ++++++++++++++++++ 7 files changed, 248 insertions(+), 45 deletions(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index cc6435a58..8ef538aa3 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -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, diff --git a/gtsam/geometry/SphericalCamera.cpp b/gtsam/geometry/SphericalCamera.cpp index b106b32d3..3124d10ff 100644 --- a/gtsam/geometry/SphericalCamera.cpp +++ b/gtsam/geometry/SphericalCamera.cpp @@ -65,11 +65,25 @@ 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 { + // onmanifold version of: camera.project(point) - zi + std::cout << "SphericalCam:reprojectionError fix jacobians " << std::endl; + return measured.localCoordinates( project2(point, Dpose, Dpoint) ); +} + /* ************************************************************************* */ } diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index bbd9f7e8d..df433e807 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -29,6 +29,19 @@ namespace gtsam { +class GTSAM_EXPORT EmptyCal { + protected: + double d_ = 0; + public: + ///< shared pointer to calibration object + EmptyCal() + : d_(0) { + } + /// Default destructor + virtual ~EmptyCal() = default; + using shared_ptr = boost::shared_ptr; +}; + /** * A spherical camera class that has a Pose3 and measures bearing vectors * @addtogroup geometry @@ -36,61 +49,69 @@ namespace gtsam { */ class GTSAM_EXPORT SphericalCamera { -public: + public: enum { dimension = 6 }; - typedef Point2 Measurement; - typedef Point2Vector MeasurementVector; + typedef Unit3 Measurement; + typedef std::vector MeasurementVector; + typedef EmptyCal CalibrationType; -private: + private: - Pose3 pose_; ///< 3D pose of camera + Pose3 pose_; ///< 3D pose of camera -protected: + protected: - /// @name Derivatives - /// @{ + EmptyCal::shared_ptr emptyCal_; -// /** -// * Calculate Jacobian with respect to pose -// * @param pn projection in normalized coordinates -// * @param d disparity (inverse depth) -// */ -// static Matrix26 Dpose(const Point2& pn, double d); -// -// /** -// * Calculate Jacobian with respect to point -// * @param pn projection in normalized coordinates -// * @param d disparity (inverse depth) -// * @param Rt transposed rotation matrix -// */ -// static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt); -// -// /// @} - -public: + public: /// @} /// @name Standard Constructors /// @{ /// Default constructor - SphericalCamera() {} + SphericalCamera() + : pose_(Pose3::identity()), + emptyCal_(boost::make_shared()) { + } /// Constructor with pose - explicit SphericalCamera(const Pose3& pose) : pose_(pose) {} + explicit SphericalCamera(const Pose3& pose) + : pose_(pose), + emptyCal_(boost::make_shared()) { + } + + /// Constructor with empty intrinsics (needed for smart factors) + explicit SphericalCamera(const Pose3& pose, + const boost::shared_ptr& cal) + : pose_(pose), + emptyCal_(boost::make_shared()) { + } /// @} /// @name Advanced Constructors /// @{ - explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {} + explicit SphericalCamera(const Vector& v) + : pose_(Pose3::Expmap(v)) { + } /// Default destructor virtual ~SphericalCamera() = default; + /// return shared pointer to calibration + const boost::shared_ptr& sharedCalibration() const { + return emptyCal_; + } + + /// return calibration + const EmptyCal& calibration() const { + return *emptyCal_; + } + /// @} /// @name Testable /// @{ @@ -120,8 +141,8 @@ public: return pose_.translation(); } -// /// return pose, with derivative -// const Pose3& getPose(OptionalJacobian<6, 6> H) const; + // /// return pose, with derivative + // const Pose3& getPose(OptionalJacobian<6, 6> H) const; /// @} /// @name Transformations and measurement functions @@ -135,19 +156,30 @@ public: * @param point 3D point in world coordinates * @return the intrinsic coordinates of the projected point */ - Unit3 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = - boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> 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; + 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 @@ -162,11 +194,10 @@ public: /// for Canonical static SphericalCamera identity() { - return SphericalCamera(Pose3::identity()); // assumes that the default constructor is valid + return SphericalCamera(Pose3::identity()); // assumes that the default constructor is valid } - -private: + private: /** Serialization function */ friend class boost::serialization::access; @@ -177,4 +208,12 @@ private: }; // end of class SphericalCamera +template<> +struct traits : public internal::LieGroup { +}; + +template<> +struct traits : public internal::LieGroup { +}; + } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 6f6ade6f7..54f442acc 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -474,8 +474,8 @@ TriangulationResult triangulateSafe(const CameraSet& 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; diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index 929ec41f7..d1c4fd8ec 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -53,6 +53,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor { typedef SmartProjectionFactor Base; typedef SmartProjectionFactorP 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 (Point2) @@ -108,7 +110,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { * @param K (fixed) camera intrinsic calibration * @param body_P_sensor (fixed) camera extrinsic calibration */ - void add(const Point2& measured, const Key& poseKey, + void add(const MEASUREMENT& measured, const Key& poseKey, const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { // store measurement and key @@ -133,7 +135,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { * @param Ks vector of (fixed) intrinsic calibration objects * @param body_P_sensors vector of (fixed) extrinsic calibration objects */ - void add(const Point2Vector& measurements, const std::vector& poseKeys, + void add(const MEASUREMENTS& measurements, const std::vector& poseKeys, const std::vector>& Ks, const std::vector body_P_sensors = std::vector()) { assert(poseKeys.size() == measurements.size()); diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 7421f73af..8a3bc81f9 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -23,6 +23,7 @@ #include #include #include +#include using namespace std; using namespace gtsam; @@ -123,6 +124,19 @@ Camera cam1(level_pose, sharedBundlerK); Camera cam2(pose_right, sharedBundlerK); Camera cam3(pose_above, sharedBundlerK); } + +/* ************************************************************************* */ +// sphericalCamera +namespace sphericalCamera { +typedef SphericalCamera Camera; +typedef SmartProjectionFactorP 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); +} /* *************************************************************************/ template @@ -137,9 +151,9 @@ CAMERA perturbCameraPose(const CAMERA& camera) { template 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); + 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); diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index 4591dc08e..98b40e8c7 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -1086,6 +1086,133 @@ TEST( SmartProjectionFactorP, 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(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector keys; + keys.push_back(x1); + keys.push_back(x2); + keys.push_back(x3); + + std::vector emptyKs; + emptyKs.push_back(emptyK); + emptyKs.push_back(emptyK); + emptyKs.push_back(emptyK); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); + smartFactor1->add(measurements_lmk1, keys, emptyKs); + +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); +// smartFactor2->add(measurements_lmk2, keys, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); +// smartFactor3->add(measurements_lmk3, keys, sharedKs); +// +// 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(x3))); +// +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); +} + +//#ifndef DISABLE_TIMING +//#include +//// this factor is actually slightly faster (but comparable) to original SmartProjectionPoseFactor +////-Total: 0 CPU (0 times, 0 wall, 0.01 children, min: 0 max: 0) +////| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0) +////| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0) +///* *************************************************************************/ +//TEST( SmartProjectionFactorP, timing ) { +// +// using namespace vanillaPose; +// +// // Default cameras for simple derivatives +// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); +// +// Rot3 R = Rot3::identity(); +// Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); +// Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); +// Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); +// Pose3 body_P_sensorId = Pose3::identity(); +// +// // one landmarks 1m in front of camera +// Point3 landmark1(0, 0, 10); +// +// Point2Vector measurements_lmk1; +// +// // Project 2 landmarks into 2 cameras +// measurements_lmk1.push_back(cam1.project(landmark1)); +// measurements_lmk1.push_back(cam2.project(landmark1)); +// +// size_t nrTests = 1000; +// +// for(size_t i = 0; iadd(measurements_lmk1[0], x1, sharedKSimple, body_P_sensorId); +// smartFactorP->add(measurements_lmk1[1], x1, sharedKSimple, body_P_sensorId); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// gttic_(SmartFactorP_LINEARIZE); +// smartFactorP->linearize(values); +// gttoc_(SmartFactorP_LINEARIZE); +// } +// +// for(size_t i = 0; iadd(measurements_lmk1[0], x1); +// smartFactor->add(measurements_lmk1[1], x2); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// gttic_(SmartPoseFactor_LINEARIZE); +// smartFactor->linearize(values); +// gttoc_(SmartPoseFactor_LINEARIZE); +// } +// tictoc_print_(); +//} +//#endif + /* ************************************************************************* */ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");