diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 069f78b12..23d698d86 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -69,10 +69,9 @@ public: * @param inv inverse depth * @return Point3 */ - static gtsam::Point3 invDepthTo3D(const gtsam::LieVector& pw, const gtsam::LieScalar& inv) { - gtsam::Point3 ray_base(pw.vector().segment(0,3)); + static gtsam::Point3 invDepthTo3D(const Vector5& pw, double rho) { + gtsam::Point3 ray_base(pw.segment(0,3)); double theta = pw(3), phi = pw(4); - double rho = inv.value(); // inverse depth gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi)); return ray_base + m/rho; } @@ -82,15 +81,14 @@ public: * @param H1 is the jacobian w.r.t. [pose3 calibration] * @param H2 is the jacobian w.r.t. inv_depth_landmark */ - inline gtsam::Point2 project(const gtsam::LieVector& pw, - const gtsam::LieScalar& inv_depth, + inline gtsam::Point2 project(const Vector5& pw, + double rho, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - gtsam::Point3 ray_base(pw.vector().segment(0,3)); + gtsam::Point3 ray_base(pw.segment(0,3)); double theta = pw(3), phi = pw(4); - double rho = inv_depth.value(); // inverse depth gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi)); const gtsam::Point3 landmark = ray_base + m/rho; @@ -155,7 +153,7 @@ public: * useful for point initialization */ - inline std::pair backproject(const gtsam::Point2& pi, const double depth) const { + inline std::pair backproject(const gtsam::Point2& pi, const double depth) const { const gtsam::Point2 pn = k_->calibrate(pi); gtsam::Point3 pc(pn.x(), pn.y(), 1.0); pc = pc/pc.norm(); @@ -164,8 +162,8 @@ public: gtsam::Point3 ray = pw - pt; double theta = atan2(ray.y(), ray.x()); // longitude double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); - return std::make_pair(gtsam::LieVector((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)), - gtsam::LieScalar(1./depth)); + return std::make_pair(Vector5((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)), + double(1./depth)); } private: diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 29f9d4972..ae30a4a49 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -80,7 +80,7 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - gtsam::Vector evaluateError(const POSE& pose, const gtsam::LieVector& point, const INVDEPTH& invDepth, + gtsam::Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth, boost::optional H1=boost::none, boost::optional H2=boost::none, boost::optional H3=boost::none) const { diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 38bc24aa8..6bf0722a5 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -22,7 +22,7 @@ namespace gtsam { /** * Binary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant1: public NoiseModelFactor2 { +class InvDepthFactorVariant1: public NoiseModelFactor2 { protected: // Keep a copy of measurement and calibration for I/O @@ -32,7 +32,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 Base; /// shorthand for this class typedef InvDepthFactorVariant1 This; @@ -78,7 +78,7 @@ public: && this->K_->equals(*e->K_, tol); } - Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const { + Vector inverseDepthError(const Pose3& pose, const Vector6& landmark) const { try { // Calculate the 3D coordinates of the landmark in the world frame double x = landmark(0), y = landmark(1), z = landmark(2); @@ -99,7 +99,7 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - Vector evaluateError(const Pose3& pose, const LieVector& landmark, + Vector evaluateError(const Pose3& pose, const Vector6& landmark, boost::optional H1=boost::none, boost::optional H2=boost::none) const { diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index a8b224b06..ae26b4240 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -23,7 +23,7 @@ namespace gtsam { /** * Binary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant2: public NoiseModelFactor2 { +class InvDepthFactorVariant2: public NoiseModelFactor2 { protected: // Keep a copy of measurement and calibration for I/O @@ -34,7 +34,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 Base; /// shorthand for this class typedef InvDepthFactorVariant2 This; @@ -82,7 +82,7 @@ public: && this->referencePoint_.equals(e->referencePoint_, tol); } - Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const { + Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const { try { // Calculate the 3D coordinates of the landmark in the world frame double theta = landmark(0), phi = landmark(1), rho = landmark(2); @@ -102,7 +102,7 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - Vector evaluateError(const Pose3& pose, const LieVector& landmark, + Vector evaluateError(const Pose3& pose, const Vector3& landmark, boost::optional H1=boost::none, boost::optional H2=boost::none) const { diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 2252ccbfd..f805e26a4 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -22,7 +22,7 @@ namespace gtsam { /** * Binary factor representing the first visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant3a: public NoiseModelFactor2 { +class InvDepthFactorVariant3a: public NoiseModelFactor2 { protected: // Keep a copy of measurement and calibration for I/O @@ -32,7 +32,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 Base; /// shorthand for this class typedef InvDepthFactorVariant3a This; @@ -80,7 +80,7 @@ public: && this->K_->equals(*e->K_, tol); } - Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const { + Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const { try { // Calculate the 3D coordinates of the landmark in the Pose frame double theta = landmark(0), phi = landmark(1), rho = landmark(2); @@ -102,7 +102,7 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - Vector evaluateError(const Pose3& pose, const LieVector& landmark, + Vector evaluateError(const Pose3& pose, const Vector3& landmark, boost::optional H1=boost::none, boost::optional H2=boost::none) const { @@ -141,7 +141,7 @@ private: /** * Ternary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant3b: public NoiseModelFactor3 { +class InvDepthFactorVariant3b: public NoiseModelFactor3 { protected: // Keep a copy of measurement and calibration for I/O @@ -151,7 +151,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /// shorthand for this class typedef InvDepthFactorVariant3b This; @@ -199,7 +199,7 @@ public: && this->K_->equals(*e->K_, tol); } - Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const LieVector& landmark) const { + Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark) const { try { // Calculate the 3D coordinates of the landmark in the Pose1 frame double theta = landmark(0), phi = landmark(1), rho = landmark(2); @@ -221,20 +221,19 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const LieVector& landmark, + Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark, boost::optional H1=boost::none, boost::optional H2=boost::none, boost::optional H3=boost::none) const { - if(H1) { + if(H1) (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); - } - if(H2) { + + if(H2) (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); - } - if(H3) { + + if(H3) (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); - } return inverseDepthError(pose1, pose2, landmark); } diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index 5ea9fe29d..bf15c7322 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -25,7 +25,7 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); SimpleCamera level_camera(level_pose, *K); -typedef InvDepthFactor3 InverseDepthFactor; +typedef InvDepthFactor3 InverseDepthFactor; typedef NonlinearEquality PoseConstraint; /* ************************************************************************* */ @@ -38,10 +38,10 @@ TEST( InvDepthFactor, optimize) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark((Vector(5) << 0., 0., 1., 0., 0.)); + Vector5 inv_landmark((Vector(5) << 0., 0., 1., 0., 0.)); // initialize inverse depth with "incorrect" depth of 1/4 // in reality this is 1/5, but initial depth is guessed - LieScalar inv_depth(1./4); + double inv_depth(1./4); gtsam::NonlinearFactorGraph graph; Values initial; @@ -82,8 +82,8 @@ TEST( InvDepthFactor, optimize) { Values result2 = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); Point3 result2_lmk = InvDepthCamera3::invDepthTo3D( - result2.at(Symbol('l',1)), - result2.at(Symbol('d',1))); + result2.at(Symbol('l',1)), + result2.at(Symbol('d',1))); EXPECT(assert_equal(landmark, result2_lmk, 1e-9)); // TODO: need to add priors to make this work with diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 503a4b953..bb3b81481 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -45,7 +45,7 @@ TEST( InvDepthFactorVariant1, optimize) { double theta = atan2(ray.y(), ray.x()); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double rho = 1./ray.norm(); - LieVector expected((Vector(6) << x, y, z, theta, phi, rho)); + Vector6 expected((Vector(6) << x, y, z, theta, phi, rho)); @@ -68,12 +68,12 @@ TEST( InvDepthFactorVariant1, optimize) { Values values; values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); - values.insert(landmarkKey, expected.retract((Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05))); + values.insert(landmarkKey, Vector6(expected + (Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); - LieVector actual = result.at(landmarkKey); + Vector6 actual = result.at(landmarkKey); // values.at(poseKey1).print("Pose1 Before:\n"); @@ -84,22 +84,22 @@ TEST( InvDepthFactorVariant1, optimize) { // result.at(poseKey2).print("Pose2 After:\n"); // pose2.print("Pose2 Truth:\n"); // cout << endl << endl; -// values.at(landmarkKey).print("Landmark Before:\n"); -// result.at(landmarkKey).print("Landmark After:\n"); +// values.at(landmarkKey).print("Landmark Before:\n"); +// result.at(landmarkKey).print("Landmark After:\n"); // expected.print("Landmark Truth:\n"); // cout << endl << endl; // Calculate world coordinates of landmark versions Point3 world_landmarkBefore; { - LieVector landmarkBefore = values.at(landmarkKey); + Vector6 landmarkBefore = values.at(landmarkKey); double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2); double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5); world_landmarkBefore = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } Point3 world_landmarkAfter; { - LieVector landmarkAfter = result.at(landmarkKey); + Vector6 landmarkAfter = result.at(landmarkKey); double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2); double theta = landmarkAfter(3), phi = landmarkAfter(4), rho = landmarkAfter(5); world_landmarkAfter = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index 49e5fc2aa..23f642a13 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant2, optimize) { double theta = atan2(ray.y(), ray.x()); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double rho = 1./ray.norm(); - LieVector expected((Vector(3) << theta, phi, rho)); + Vector3 expected((Vector(3) << theta, phi, rho)); @@ -66,12 +66,12 @@ TEST( InvDepthFactorVariant2, optimize) { Values values; values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); - values.insert(landmarkKey, expected.retract((Vector(3) << +0.02, -0.04, +0.05))); + values.insert(landmarkKey, Vector3(expected + (Vector(3) << +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); - LieVector actual = result.at(landmarkKey); + Vector3 actual = result.at(landmarkKey); // values.at(poseKey1).print("Pose1 Before:\n"); // result.at(poseKey1).print("Pose1 After:\n"); @@ -81,21 +81,21 @@ TEST( InvDepthFactorVariant2, optimize) { // result.at(poseKey2).print("Pose2 After:\n"); // pose2.print("Pose2 Truth:\n"); // std::cout << std::endl << std::endl; -// values.at(landmarkKey).print("Landmark Before:\n"); -// result.at(landmarkKey).print("Landmark After:\n"); +// values.at(landmarkKey).print("Landmark Before:\n"); +// result.at(landmarkKey).print("Landmark After:\n"); // expected.print("Landmark Truth:\n"); // std::cout << std::endl << std::endl; // Calculate world coordinates of landmark versions Point3 world_landmarkBefore; { - LieVector landmarkBefore = values.at(landmarkKey); + Vector3 landmarkBefore = values.at(landmarkKey); double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2); world_landmarkBefore = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } Point3 world_landmarkAfter; { - LieVector landmarkAfter = result.at(landmarkKey); + Vector3 landmarkAfter = result.at(landmarkKey); double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2); world_landmarkAfter = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } @@ -106,7 +106,7 @@ TEST( InvDepthFactorVariant2, optimize) { // std::cout << std::endl << std::endl; // Test that the correct landmark parameters have been recovered - EXPECT(assert_equal(expected, actual, 1e-9)); + EXPECT(assert_equal((Vector)expected, actual, 1e-9)); } diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index 11a91f687..e0be9c79c 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant3, optimize) { double theta = atan2(landmark_p1.x(), landmark_p1.z()); double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z())); double rho = 1./landmark_p1.norm(); - LieVector expected((Vector(3) << theta, phi, rho)); + Vector3 expected((Vector(3) << theta, phi, rho)); @@ -66,17 +66,17 @@ TEST( InvDepthFactorVariant3, optimize) { Values values; values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); - values.insert(landmarkKey, expected.retract((Vector(3) << +0.02, -0.04, +0.05))); + values.insert(landmarkKey, Vector3(expected + (Vector(3) << +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); - LieVector actual = result.at(landmarkKey); + Vector3 actual = result.at(landmarkKey); // Test that the correct landmark parameters have been recovered - EXPECT(assert_equal(expected, actual, 1e-9)); + EXPECT(assert_equal((Vector)expected, actual, 1e-9)); }