diff --git a/.gitignore b/.gitignore index e0d1d4e65..030d51b09 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ /build* *.pyc +*.DS_Store \ No newline at end of file diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp new file mode 100644 index 000000000..cccedc5bb --- /dev/null +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -0,0 +1,152 @@ +/* + * @file EssentialMatrix.cpp + * @brief EssentialMatrix class + * @author Frank Dellaert + * @date December 5, 2014 + */ + +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, + boost::optional H) { + const Rot3& _1R2_ = _1P2_.rotation(); + const Point3& _1T2_ = _1P2_.translation(); + if (!H) { + // just make a direction out of translation and create E + Sphere2 direction(_1T2_); + return EssentialMatrix(_1R2_, direction); + } else { + // Calculate the 5*6 Jacobian H = D_E_1P2 + // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation + // First get 2*3 derivative from Sphere2::FromPoint3 + Matrix D_direction_1T2; + Sphere2 direction = Sphere2::FromPoint3(_1T2_, D_direction_1T2); + H->resize(5, 6); + H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left + H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left + H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right + H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right + return EssentialMatrix(_1R2_, direction); + } +} + +/* ************************************************************************* */ +void EssentialMatrix::print(const string& s) const { + cout << s; + aRb_.print("R:\n"); + aTb_.print("d: "); +} + +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { + assert(xi.size() == 5); + Vector3 omega(sub(xi, 0, 3)); + Vector2 z(sub(xi, 3, 5)); + Rot3 R = aRb_.retract(omega); + Sphere2 t = aTb_.retract(z); + return EssentialMatrix(R, t); +} + +/* ************************************************************************* */ +Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { + return Vector(5) << // + aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_); +} + +/* ************************************************************************* */ +Point3 EssentialMatrix::transform_to(const Point3& p, + boost::optional DE, boost::optional Dpoint) const { + Pose3 pose(aRb_, aTb_.point3()); + Point3 q = pose.transform_to(p, DE, Dpoint); + if (DE) { + // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 + // The last 3 columns are derivative with respect to change in translation + // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() + // Duy made an educated guess that this needs to be rotated to the local frame + Matrix H(3, 5); + H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); + *DE = H; + } + return q; +} + +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, + boost::optional HE, boost::optional HR) const { + + // The rotation must be conjugated to act in the camera frame + Rot3 c1Rc2 = aRb_.conjugate(cRb); + + if (!HE && !HR) { + // Rotate translation direction and return + Sphere2 c1Tc2 = cRb * aTb_; + return EssentialMatrix(c1Rc2, c1Tc2); + } else { + // Calculate derivatives + Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2 + Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); + if (HE) { + *HE = zeros(5, 5); + HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 + HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2) + } + if (HR) { + throw runtime_error( + "EssentialMatrix::rotate: derivative HR not implemented yet"); + /* + HR->resize(5, 3); + HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ? + HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ? + */ + } + return EssentialMatrix(c1Rc2, c1Tc2); + } +} + +/* ************************************************************************* */ +double EssentialMatrix::error(const Vector& vA, const Vector& vB, // + boost::optional H) const { + if (H) { + H->resize(1, 5); + // See math.lyx + Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB); + Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) + * aTb_.basis(); + *H << HR, HD; + } + return dot(vA, E_ * vB); +} + +/* ************************************************************************* */ +ostream& operator <<(ostream& os, const EssentialMatrix& E) { + Rot3 R = E.rotation(); + Sphere2 d = E.direction(); + os.precision(10); + os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; + return os; +} + +/* ************************************************************************* */ +istream& operator >>(istream& is, EssentialMatrix& E) { + double rx, ry, rz, dx, dy, dz; + is >> rx >> ry >> rz; // Read the rotation rxyz + is >> dx >> dy >> dz; // Read the translation dxyz + + // Create EssentialMatrix from rotation and translation + Rot3 rot = Rot3::RzRyRx(rx, ry, rz); + Sphere2 dt = Sphere2(dx, dy, dz); + E = EssentialMatrix(rot, dt); + + return is; +} + +/* ************************************************************************* */ + +} // gtsam + diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 54fd94bbc..a7270fff0 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -26,7 +26,7 @@ private: Rot3 aRb_; ///< Rotation between a and b Sphere2 aTb_; ///< translation direction from a to b - Matrix E_; ///< Essential matrix + Matrix3 E_; ///< Essential matrix public: @@ -48,6 +48,10 @@ public: aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) { } + /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) + static EssentialMatrix FromPose3(const Pose3& _1P2_, + boost::optional H = boost::none); + /// Random, using Rot3::Random and Sphere2::Random template static EssentialMatrix Random(Engine & rng) { @@ -60,11 +64,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const { - std::cout << s; - aRb_.print("R:\n"); - aTb_.print("d: "); - } + void print(const std::string& s = "") const; /// assert equality up to a tolerance bool equals(const EssentialMatrix& other, double tol = 1e-8) const { @@ -87,20 +87,10 @@ public: } /// Retract delta to manifold - virtual EssentialMatrix retract(const Vector& xi) const { - assert(xi.size() == 5); - Vector3 omega(sub(xi, 0, 3)); - Vector2 z(sub(xi, 3, 5)); - Rot3 R = aRb_.retract(omega); - Sphere2 t = aTb_.retract(z); - return EssentialMatrix(R, t); - } + virtual EssentialMatrix retract(const Vector& xi) const; /// Compute the coordinates in the tangent space - virtual Vector localCoordinates(const EssentialMatrix& other) const { - return Vector(5) << // - aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_); - } + virtual Vector localCoordinates(const EssentialMatrix& other) const; /// @} @@ -108,17 +98,17 @@ public: /// @{ /// Rotation - const Rot3& rotation() const { + inline const Rot3& rotation() const { return aRb_; } /// Direction - const Sphere2& direction() const { + inline const Sphere2& direction() const { return aTb_; } /// Return 3*3 matrix representation - const Matrix& matrix() const { + inline const Matrix3& matrix() const { return E_; } @@ -131,20 +121,7 @@ public: */ Point3 transform_to(const Point3& p, boost::optional DE = boost::none, - boost::optional Dpoint = boost::none) const { - Pose3 pose(aRb_, aTb_.point3()); - Point3 q = pose.transform_to(p, DE, Dpoint); - if (DE) { - // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 - // The last 3 columns are derivative with respect to change in translation - // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() - // Duy made an educated guess that this needs to be rotated to the local frame - Matrix H(3, 5); - H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); - *DE = H; - } - return q; - } + boost::optional Dpoint = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -152,36 +129,7 @@ public: * @param E essential matrix E in camera frame C */ EssentialMatrix rotate(const Rot3& cRb, boost::optional HE = - boost::none, boost::optional HR = boost::none) const { - - // The rotation must be conjugated to act in the camera frame - Rot3 c1Rc2 = aRb_.conjugate(cRb); - - if (!HE && !HR) { - // Rotate translation direction and return - Sphere2 c1Tc2 = cRb * aTb_; - return EssentialMatrix(c1Rc2, c1Tc2); - } else { - // Calculate derivatives - Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2 - Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); - if (HE) { - *HE = zeros(5, 5); - HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 - HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2) - } - if (HR) { - throw std::runtime_error( - "EssentialMatrix::rotate: derivative HR not implemented yet"); - /* - HR->resize(5, 3); - HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ? - HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ? - */ - } - return EssentialMatrix(c1Rc2, c1Tc2); - } - } + boost::none, boost::optional HR = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -194,17 +142,18 @@ public: /// epipolar error, algebraic double error(const Vector& vA, const Vector& vB, // - boost::optional H = boost::none) const { - if (H) { - H->resize(1, 5); - // See math.lyx - Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) - * aTb_.basis(); - *H << HR, HD; - } - return dot(vA, E_ * vB); - } + boost::optional H = boost::none) const; + + /// @} + + /// @name Streaming operators + /// @{ + + /// stream to stream + friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E); + + /// stream from stream + friend std::istream& operator >>(std::istream& is, EssentialMatrix& E); /// @} diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 4f5f43665..b6244630e 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -26,7 +26,8 @@ INSTANTIATE_LIE(Point3); /* ************************************************************************* */ bool Point3::equals(const Point3 & q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol); + return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol + && fabs(z_ - q.z()) < tol); } /* ************************************************************************* */ @@ -37,18 +38,18 @@ void Point3::print(const string& s) const { /* ************************************************************************* */ -bool Point3::operator== (const Point3& q) const { +bool Point3::operator==(const Point3& q) const { return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; } /* ************************************************************************* */ Point3 Point3::operator+(const Point3& q) const { - return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ ); + return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_); } /* ************************************************************************* */ -Point3 Point3::operator- (const Point3& q) const { - return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ ); +Point3 Point3::operator-(const Point3& q) const { + return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_); } /* ************************************************************************* */ @@ -62,36 +63,53 @@ Point3 Point3::operator/(double s) const { } /* ************************************************************************* */ -Point3 Point3::add(const Point3 &q, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = eye(3,3); - if (H2) *H2 = eye(3,3); +Point3 Point3::add(const Point3 &q, boost::optional H1, + boost::optional H2) const { + if (H1) + *H1 = eye(3, 3); + if (H2) + *H2 = eye(3, 3); return *this + q; } /* ************************************************************************* */ -Point3 Point3::sub(const Point3 &q, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = eye(3,3); - if (H2) *H2 = -eye(3,3); +Point3 Point3::sub(const Point3 &q, boost::optional H1, + boost::optional H2) const { + if (H1) + *H1 = eye(3, 3); + if (H2) + *H2 = -eye(3, 3); return *this - q; } /* ************************************************************************* */ Point3 Point3::cross(const Point3 &q) const { - return Point3( y_*q.z_ - z_*q.y_, - z_*q.x_ - x_*q.z_, - x_*q.y_ - y_*q.x_ ); + return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_, + x_ * q.y_ - y_ * q.x_); } /* ************************************************************************* */ double Point3::dot(const Point3 &q) const { - return ( x_*q.x_ + y_*q.y_ + z_*q.z_ ); + return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_); } /* ************************************************************************* */ double Point3::norm() const { - return sqrt( x_*x_ + y_*y_ + z_*z_ ); + return sqrt(x_ * x_ + y_ * y_ + z_ * z_); +} + +/* ************************************************************************* */ +Point3 Point3::normalize(boost::optional H) const { + Point3 normalized = *this / norm(); + if (H) { + // 3*3 Derivative + double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; + double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; + H->resize(3, 3); + *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; + *H /= pow(x2 + y2 + z2, 1.5); + } + return normalized; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 5e2e0ff42..66924ec07 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -176,6 +176,9 @@ namespace gtsam { /** Distance of the point from the origin */ double norm() const; + /** normalize, with optional Jacobian */ + Point3 normalize(boost::optional H = boost::none) const; + /** cross product @return this x q */ Point3 cross(const Point3 &q) const; diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index 3c7765bd2..ed2561e22 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -28,6 +28,21 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +Sphere2 Sphere2::FromPoint3(const Point3& point, + boost::optional H) { + Sphere2 direction(point); + if (H) { + // 3*3 Derivative of representation with respect to point is 3*3: + Matrix D_p_point; + point.normalize(D_p_point); // TODO, this calculates norm a second time :-( + // Calculate the 2*3 Jacobian + H->resize(2, 3); + *H << direction.basis().transpose() * D_p_point; + } + return direction; +} + /* ************************************************************************* */ Sphere2 Sphere2::Random(boost::random::mt19937 & rng) { // TODO allow any engine without including all of boost :-( diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index 28e6f33d0..ac8124139 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -70,6 +70,10 @@ public: p_ = p_ / p_.norm(); } + /// Named constructor from Point3 with optional Jacobian + static Sphere2 FromPoint3(const Point3& point, + boost::optional H = boost::none); + /// Random direction, using boost::uniform_on_sphere static Sphere2 Random(boost::random::mt19937 & rng); @@ -90,7 +94,11 @@ public: /// @name Other functionality /// @{ - /// Returns the local coordinate frame to tangent plane + /** + * Returns the local coordinate frame to tangent plane + * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions + * tangent to the sphere at the current direction. + */ Matrix basis() const; /// Return skew-symmetric associated with 3D point on unit sphere diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index da5240b15..865f27f81 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -10,6 +10,7 @@ #include #include #include +#include using namespace std; using namespace gtsam; @@ -95,6 +96,38 @@ TEST (EssentialMatrix, rotate) { // Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8)); } +//************************************************************************* +TEST (EssentialMatrix, FromPose3_a) { + Matrix actualH; + Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras + EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + +//************************************************************************* +TEST (EssentialMatrix, FromPose3_b) { + Matrix actualH; + Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); + Point3 c1Tc2(0.4, 0.5, 0.6); + EssentialMatrix trueE(c1Rc2, c1Tc2); + Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras + EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + +//************************************************************************* +TEST (EssentialMatrix, streaming) { + EssentialMatrix expected(c1Rc2, c1Tc2), actual; + stringstream ss; + ss << expected; + ss >> actual; + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 5b49c92b2..ab0d12b9e 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -12,75 +12,86 @@ /** * @file testPoint3.cpp * @brief Unit tests for Point3 class - */ + */ -#include -#include #include +#include +#include +#include using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point3) GTSAM_CONCEPT_LIE_INST(Point3) -static Point3 P(0.2,0.7,-2); +static Point3 P(0.2, 0.7, -2); /* ************************************************************************* */ TEST(Point3, Lie) { - Point3 p1(1,2,3); - Point3 p2(4,5,6); + Point3 p1(1, 2, 3); + Point3 p2(4, 5, 6); Matrix H1, H2; - EXPECT(assert_equal(Point3(5,7,9), p1.compose(p2, H1, H2))); + EXPECT(assert_equal(Point3(5, 7, 9), p1.compose(p2, H1, H2))); EXPECT(assert_equal(eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(3,3,3), p1.between(p2, H1, H2))); + EXPECT(assert_equal(Point3(3, 3, 3), p1.between(p2, H1, H2))); EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vector(3) << 4., 5., 6.)))); + EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.)))); EXPECT(assert_equal((Vector)(Vector(3) << 3.,3.,3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ -TEST( Point3, arithmetic) -{ - CHECK(P*3==3*P); - CHECK(assert_equal( Point3(-1,-5,-6), -Point3(1,5,6) )); - CHECK(assert_equal( Point3(2,5,6), Point3(1,4,5)+Point3(1,1,1))); - CHECK(assert_equal( Point3(0,3,4), Point3(1,4,5)-Point3(1,1,1))); - CHECK(assert_equal( Point3(2,8,6), Point3(1,4,3)*2)); - CHECK(assert_equal( Point3(2,2,6), 2*Point3(1,1,3))); - CHECK(assert_equal( Point3(1,2,3), Point3(2,4,6)/2)); +TEST( Point3, arithmetic) { + CHECK(P * 3 == 3 * P); + CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6))); + CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1))); + CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1))); + CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2)); + CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3))); + CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2)); } /* ************************************************************************* */ -TEST( Point3, equals) -{ +TEST( Point3, equals) { CHECK(P.equals(P)); Point3 Q; CHECK(!P.equals(Q)); } /* ************************************************************************* */ -TEST( Point3, dot) -{ - Point3 origin, ones(1,1,1); - CHECK(origin.dot(Point3(1,1,0)) == 0); - CHECK(ones.dot(Point3(1,1,0)) == 2); +TEST( Point3, dot) { + Point3 origin, ones(1, 1, 1); + CHECK(origin.dot(Point3(1, 1, 0)) == 0); + CHECK(ones.dot(Point3(1, 1, 0)) == 2); } /* ************************************************************************* */ -TEST( Point3, stream) -{ - Point3 p(1,2, -3); +TEST( Point3, stream) { + Point3 p(1, 2, -3); std::ostringstream os; os << p; EXPECT(os.str() == "[1, 2, -3]';"); } +//************************************************************************* +TEST (Point3, normalize) { + Matrix actualH; + Point3 point(1, -2, 3); // arbitrary point + Point3 expected(point / sqrt(14)); + EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(&Point3::normalize, _1, boost::none), point); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index da33478dc..306d84b94 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -27,6 +27,7 @@ #include #include #include +#include using namespace boost::assign; using namespace gtsam; @@ -324,6 +325,17 @@ TEST(Sphere2, Random) { EXPECT(assert_equal(expectedMean,actualMean,0.1)); } +//************************************************************************* +TEST (Sphere2, FromPoint3) { + Matrix actualH; + Point3 point(1, -2, 3); // arbitrary point + Sphere2 expected(point); + EXPECT(assert_equal(expected, Sphere2::FromPoint3(point, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(Sphere2::FromPoint3, _1, boost::none), point); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + /* ************************************************************************* */ int main() { srand(time(NULL)); diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index c959473a8..4493a0b16 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -74,6 +74,16 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const { return graph_.linearize(state_.values); } +/* ************************************************************************* */ +void LevenbergMarquardtOptimizer::increaseLambda(){ + state_.lambda *= params_.lambdaFactor; +} + +/* ************************************************************************* */ +void LevenbergMarquardtOptimizer::decreaseLambda(){ + state_.lambda /= params_.lambdaFactor; +} + /* ************************************************************************* */ void LevenbergMarquardtOptimizer::iterate() { @@ -146,7 +156,7 @@ void LevenbergMarquardtOptimizer::iterate() { if (error <= state_.error) { state_.values.swap(newValues); state_.error = error; - state_.lambda /= params_.lambdaFactor; + decreaseLambda(); break; } else { // Either we're not cautious, or the same lambda was worse than the current error. @@ -157,7 +167,10 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { - state_.lambda *= params_.lambdaFactor; + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "increasing lambda: old error (" << state_.error << ") new error (" << error << ")" << endl; + + increaseLambda(); } } } catch (IndeterminantLinearSystemException& e) { @@ -172,7 +185,7 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { - state_.lambda *= params_.lambdaFactor; + increaseLambda(); } } // Frank asks: why would we do that? diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index d8962da4a..15e14ab47 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -174,6 +174,12 @@ public: return state_.lambda; } + // Apply policy to increase lambda if the current update was successful + virtual void increaseLambda(); + + // Apply policy to decrease lambda if the current update was NOT successful + virtual void decreaseLambda(); + /// Access the current number of inner iterations int getInnerIterations() const { return state_.totalNumberInnerIterations; diff --git a/gtsam/slam/EssentialMatrixConstraint.cpp b/gtsam/slam/EssentialMatrixConstraint.cpp new file mode 100644 index 000000000..e27bbc8c5 --- /dev/null +++ b/gtsam/slam/EssentialMatrixConstraint.cpp @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2014, 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 EssentialMatrixConstraint.cpp + * @author Frank Dellaert + * @author Pablo Alcantarilla + * @date Jan 5, 2014 + **/ + +#include +//#include +//#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +void EssentialMatrixConstraint::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1()) + << "," << keyFormatter(this->key2()) << ")\n"; + measuredE_.print(" measured: "); + this->noiseModel_->print(" noise model: "); +} + +/* ************************************************************************* */ +bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected, + double tol) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && this->measuredE_.equals(e->measuredE_, tol); +} + +/* ************************************************************************* */ +Vector EssentialMatrixConstraint::evaluateError(const Pose3& p1, + const Pose3& p2, boost::optional Hp1, + boost::optional Hp2) const { + + // compute relative Pose3 between p1 and p2 + Pose3 _1P2_ = p1.between(p2, Hp1, Hp2); + + // convert to EssentialMatrix + Matrix D_hx_1P2; + EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_, + (Hp1 || Hp2) ? boost::optional(D_hx_1P2) : boost::none); + + // Calculate derivatives if needed + if (Hp1) { + // Hp1 will already contain the 6*6 derivative D_1P2_p1 + const Matrix& D_1P2_p1 = *Hp1; + // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2: + *Hp1 = D_hx_1P2 * D_1P2_p1; + } + if (Hp2) { + // Hp2 will already contain the 6*6 derivative D_1P2_p1 + const Matrix& D_1P2_p2 = *Hp2; + // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2: + *Hp2 = D_hx_1P2 * D_1P2_p2; + } + + // manifold equivalent of h(x)-z -> log(z,h(x)) + return measuredE_.localCoordinates(hx); // 5D error +} + +} /// namespace gtsam diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h new file mode 100644 index 000000000..f2eb76e2d --- /dev/null +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2014, 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 EssentialMatrixConstraint.h + * @author Frank Dellaert + * @author Pablo Alcantarilla + * @date Jan 5, 2014 + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement + * @addtogroup SLAM + */ +class EssentialMatrixConstraint: public NoiseModelFactor2 { + +private: + + typedef EssentialMatrixConstraint This; + typedef NoiseModelFactor2 Base; + + EssentialMatrix measuredE_; /** The measurement is an essential matrix */ + +public: + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + EssentialMatrixConstraint() { + } + + /** + * Constructor + * @param key1 key for first pose + * @param key2 key for second pose + * @param measuredE measured EssentialMatrix + * @param model noise model, 5D ! + */ + EssentialMatrixConstraint(Key key1, Key key2, + const EssentialMatrix& measuredE, const SharedNoiseModel& model) : + Base(model, key1, key2), measuredE_(measuredE) { + } + + virtual ~EssentialMatrixConstraint() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + virtual Vector evaluateError(const Pose3& p1, const Pose3& p2, + boost::optional Hp1 = boost::none, // + boost::optional Hp2 = boost::none) const; + + /** return the measured */ + const EssentialMatrix& measured() const { + return measuredE_; + } + + /** number of variables attached to this factor */ + std::size_t size() const { + return 2; + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measuredE_); + } +}; +// \class EssentialMatrixConstraint + +}/// namespace gtsam diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 43927757e..db91fbd45 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -229,7 +229,7 @@ pair load2D( } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } @@ -403,7 +403,7 @@ pair load2D_robust( } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } @@ -671,33 +671,36 @@ bool writeBAL(const string& filename, SfM_data &data) bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){ - // CHECKS + // Store poses or cameras in SfM_data Values valuesPoses = values.filter(); - if( valuesPoses.size() != data.number_cameras()){ - cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras() - <<") and values (#cameras " << valuesPoses.size() << ")!!" << endl; - return false; + if( valuesPoses.size() == data.number_cameras() ){ // we only estimated camera poses + for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + Key poseKey = symbol('x',i); + Pose3 pose = values.at(poseKey); + Cal3Bundler K = data.cameras[i].calibration(); + PinholeCamera camera(pose, K); + data.cameras[i] = camera; + } + } else { + Values valuesCameras = values.filter< PinholeCamera >(); + if ( valuesCameras.size() == data.number_cameras() ){ // we only estimated camera poses and calibration + for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + Key cameraKey = symbol('c',i); + PinholeCamera camera = values.at >(cameraKey); + data.cameras[i] = camera; + } + }else{ + cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras() + <<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl; + return false; + } } + + // Store 3D points in SfM_data Values valuesPoints = values.filter(); if( valuesPoints.size() != data.number_tracks()){ cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks() - <<") and values (#points " << valuesPoints.size() << ")!!" << endl; - } - if(valuesPoints.size() + valuesPoses.size() != values.size()){ - cout << "writeBALfromValues write only poses and points values!!" << endl; - return false; - } - if(valuesPoints.size()==0 || valuesPoses.size()==0){ - cout << "writeBALfromValues: No point or pose in values!!" << endl; - return false; - } - - for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera - Key poseKey = symbol('x',i); - Pose3 pose = values.at(poseKey); - Cal3Bundler K = data.cameras[i].calibration(); - PinholeCamera camera(pose, K); - data.cameras[i] = camera; + <<") and values (#points " << valuesPoints.size() << ")!!" << endl; } for (size_t j = 0; j < data.number_tracks(); j++){ // for each point @@ -713,8 +716,8 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){ } } + // Write SfM_data to file return writeBAL(filename, data); } - } // \namespace gtsam diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index bd5a28cdd..0ee5aad9f 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -141,7 +141,9 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data); * while camera poses and values are read from Values) * @param filename The name of the BAL file to write * @param data SfM structure where the data is stored - * @param values structure where the graph values are stored + * @param values structure where the graph values are stored (values can be either Pose3 or PinholeCamera for the + * cameras, and should be Point3 for the 3D points). Note that the current version + * assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1 * @return true if the parsing was successful, false otherwise */ GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values); diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp new file mode 100644 index 000000000..eb87100f6 --- /dev/null +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -0,0 +1,76 @@ +/* ---------------------------------------------------------------------------- + + * 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 testEssentialMatrixConstraint.cpp + * @brief Unit tests for EssentialMatrixConstraint Class + * @author Frank Dellaert + * @author Pablo Alcantarilla + * @date Jan 5, 2014 + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( EssentialMatrixConstraint, test ) { + // Create a factor + Key poseKey1(1); + Key poseKey2(2); + Rot3 trueRotation = Rot3::RzRyRx(0.15, 0.15, -0.20); + Point3 trueTranslation(+0.5, -1.0, +1.0); + Sphere2 trueDirection(trueTranslation); + EssentialMatrix measurement(trueRotation, trueDirection); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 0.25); + EssentialMatrixConstraint factor(poseKey1, poseKey2, measurement, model); + + // Create a linearization point at the zero-error point + Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0)); + Pose3 pose2( + Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840), + Point3(-3.37493895, 6.14660244, -8.93650986)); + + Vector expected = zero(5); + Vector actual = factor.evaluateError(pose1,pose2); + CHECK(assert_equal(expected, actual, 1e-8)); + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11( + boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, + boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11( + boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, + boost::none, boost::none), pose2); + + // Use the factor to calculate the derivative + Matrix actualH1; + Matrix actualH2; + factor.evaluateError(pose1, pose2, actualH1, actualH2); + + // Verify we get the expected error + CHECK(assert_equal(expectedH1, actualH1, 1e-9)); + CHECK(assert_equal(expectedH2, actualH2, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index a521e43fb..6d85685d5 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -154,8 +154,8 @@ TEST (EssentialMatrixFactor2, factor) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2); // Check evaluation - Point3 P1 = data.tracks[i].p; - const Point2 pi = camera2.project(P1); + Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); + const Point2 pi = SimpleCamera::project_to_camera(P2); Point2 reprojectionError(pi - pB(i)); Vector expected = reprojectionError.vector(); @@ -269,6 +269,21 @@ TEST (EssentialMatrixFactor3, minimization) { truth.insert(i, LieScalar(baseline / P1.z())); } EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Optimize + LevenbergMarquardtParams parameters; + // parameters.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer optimizer(graph, truth, parameters); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actual = result.at(100); + EXPECT(assert_equal(bodyE, actual, 1e-1)); + for (size_t i = 0; i < 5; i++) + EXPECT(assert_equal(truth.at(i), result.at(i), 1e-1)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); } } // namespace example1