diff --git a/gtsam/base/LieMatrix.cpp b/gtsam/base/LieMatrix.cpp index 9e88607cc..69054fc1c 100644 --- a/gtsam/base/LieMatrix.cpp +++ b/gtsam/base/LieMatrix.cpp @@ -19,20 +19,6 @@ namespace gtsam { -/* ************************************************************************* */ -LieMatrix::LieMatrix(size_t m, size_t n, ...) -: Matrix(m,n) { - va_list ap; - va_start(ap, n); - for(size_t i = 0; i < m; ++i) { - for(size_t j = 0; j < n; ++j) { - double value = va_arg(ap, double); - (*this)(i,j) = value; - } - } - va_end(ap); -} - /* ************************************************************************* */ void LieMatrix::print(const std::string& name) const { gtsam::print(matrix(), name); diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 0a27eff69..8d43dd6ea 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -48,9 +48,6 @@ struct LieMatrix : public Matrix, public DerivedValue { LieMatrix(size_t m, size_t n, const double* const data) : Matrix(Eigen::Map(data, m, n)) {} - /** Specify arguments directly, as in Matrix_() - always force these to be doubles */ - GTSAM_EXPORT LieMatrix(size_t m, size_t n, ...); - /// @} /// @name Testable interface /// @{ diff --git a/gtsam/base/LieVector.cpp b/gtsam/base/LieVector.cpp index 17aeff73a..f6cae28e2 100644 --- a/gtsam/base/LieVector.cpp +++ b/gtsam/base/LieVector.cpp @@ -24,21 +24,10 @@ namespace gtsam { /* ************************************************************************* */ LieVector::LieVector(size_t m, const double* const data) -: Vector(Vector_(m,data)) -{ -} - -/* ************************************************************************* */ -LieVector::LieVector(size_t m, ...) : Vector(m) { - va_list ap; - va_start(ap, m); - for( size_t i = 0 ; i < m ; i++) { - double value = va_arg(ap, double); - (*this)(i) = value; - } - va_end(ap); + for(size_t i = 0; i < m; i++) + (*this)(i) = data[i]; } /* ************************************************************************* */ diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index f1b05b34b..6989bbfd2 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -44,9 +44,6 @@ struct LieVector : public Vector, public DerivedValue { /** constructor with size and initial data, row order ! */ GTSAM_EXPORT LieVector(size_t m, const double* const data); - /** Specify arguments directly, as in Vector_() - always force these to be doubles */ - GTSAM_EXPORT LieVector(size_t m, ...); - /** get the underlying vector */ Vector vector() const { return static_cast(*this); diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index adf98cf9a..3b16beb51 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -36,38 +36,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -Matrix Matrix_( size_t m, size_t n, const double* const data) { - MatrixRowMajor A(m,n); - copy(data, data+m*n, A.data()); - return Matrix(A); -} - -/* ************************************************************************* */ -Matrix Matrix_( size_t m, size_t n, const Vector& v) -{ - Matrix A(m,n); - // column-wise copy - for( size_t j = 0, k=0 ; j < n ; j++) - for( size_t i = 0; i < m ; i++,k++) - A(i,j) = v(k); - return A; -} - -/* ************************************************************************* */ -Matrix Matrix_(size_t m, size_t n, ...) { - Matrix A(m,n); - va_list ap; - va_start(ap, n); - for( size_t i = 0 ; i < m ; i++) - for( size_t j = 0 ; j < n ; j++) { - double value = va_arg(ap, double); - A(i,j) = value; - } - va_end(ap); - return A; -} - /* ************************************************************************* */ Matrix zeros( size_t m, size_t n ) { return Matrix::Zero(m,n); @@ -211,17 +179,6 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec x += alpha * A.transpose() * e; } -/* ************************************************************************* */ -Vector Vector_(const Matrix& A) -{ - size_t m = A.rows(), n = A.cols(); - Vector v(m*n); - for( size_t j = 0, k=0 ; j < n ; j++) - for( size_t i = 0; i < m ; i++,k++) - v(k) = A(i,j); - return v; -} - /* ************************************************************************* */ void print(const Matrix& A, const string &s, ostream& stream) { size_t m = A.rows(), n = A.cols(); diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 05f3bc7c9..caee66851 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -45,27 +45,6 @@ typedef Eigen::Matrix Matrix6; typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; -/** - * constructor with size and initial data, row order ! - */ -GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, const double* const data); - -/** - * constructor with size and vector data, column order !!! - */ -GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, const Vector& v); - -/** - * constructor from Vector yielding v.size()*1 vector - */ -inline Matrix Matrix_(const Vector& v) { return Matrix_(v.size(),1,v);} - -/** - * nice constructor, dangerous as number of arguments must be exactly right - * and you have to pass doubles !!! always use 0.0 never 0 -*/ -GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, ...); - // Matlab-like syntax /** @@ -196,11 +175,6 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) { return result; } -/** - * convert to column vector, column order !!! - */ -GTSAM_EXPORT Vector Vector_(const Matrix& A); - /** * print a matrix */ diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 2a0192930..48ada018f 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -79,33 +79,6 @@ void odprintf(const char *format, ...) { //#endif } -/* ************************************************************************* */ -Vector Vector_( size_t m, const double* const data) { - Vector A(m); - copy(data, data+m, A.data()); - return A; -} - -/* ************************************************************************* */ -Vector Vector_(size_t m, ...) { - Vector v(m); - va_list ap; - va_start(ap, m); - for( size_t i = 0 ; i < m ; i++) { - double value = va_arg(ap, double); - v(i) = value; - } - va_end(ap); - return v; -} - -/* ************************************************************************* */ -Vector Vector_(const std::vector& d) { - Vector v(d.size()); - copy(d.begin(), d.end(), v.data()); - return v; -} - /* ************************************************************************* */ bool zero(const Vector& v) { bool result = true; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 2c6066042..b35afccdb 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -46,22 +46,6 @@ typedef Eigen::VectorBlock ConstSubVector; */ GTSAM_EXPORT void odprintf(const char *format, ...); -/** - * constructor with size and initial data, row order ! - */ -GTSAM_EXPORT Vector Vector_( size_t m, const double* const data); - -/** - * nice constructor, dangerous as number of arguments must be exactly right - * and you have to pass doubles !!! always use 0.0 never 0 - */ -GTSAM_EXPORT Vector Vector_(size_t m, ...); - -/** - * Create a numeric vector from an STL vector of doubles - */ -GTSAM_EXPORT Vector Vector_(const std::vector& data); - /** * Create vector initialized to a constant value * @param n is the size of the vector diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 053a89da8..036f23f68 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -59,7 +59,7 @@ namespace gtsam { /** global functions for converting to a LieVector for use with numericalDerivative */ inline LieVector makeLieVector(const Vector& v) { return LieVector(v); } - inline LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); } + inline LieVector makeLieVectorD(double d) { return LieVector((Vector) (Vector(1) << d)); } /** * Numerically compute gradient of scalar function diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index b21756dc4..ee8fe14d9 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -39,20 +39,17 @@ TEST( LieMatrix, construction ) { TEST( LieMatrix, other_constructors ) { Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0); LieMatrix exp(init); - LieMatrix a(2,2,10.0,20.0,30.0,40.0); double data[] = {10,30,20,40}; LieMatrix b(2,2,data); - EXPECT(assert_equal(exp, a)); EXPECT(assert_equal(exp, b)); - EXPECT(assert_equal(b, a)); } /* ************************************************************************* */ TEST(LieMatrix, retract) { - LieMatrix init(2,2, 1.0,2.0,3.0,4.0); - Vector update = Vector_(4, 3.0, 4.0, 6.0, 7.0); + LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0)); + Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0); - LieMatrix expected(2,2, 4.0, 6.0, 9.0, 11.0); + LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0)); LieMatrix actual = init.retract(update); EXPECT(assert_equal(expected, actual)); @@ -63,7 +60,7 @@ TEST(LieMatrix, retract) { EXPECT(assert_equal(expectedUpdate, actualUpdate)); Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4); - Vector actualLogmap = LieMatrix::Logmap(LieMatrix(2,2, 1.0, 2.0, 3.0, 4.0)); + Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0))); EXPECT(assert_equal(expectedLogmap, actualLogmap)); } diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 11157a701..68655cc71 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -42,7 +42,7 @@ TEST( testLieScalar, construction ) { TEST( testLieScalar, localCoordinates ) { LieScalar lie1(1.), lie2(3.); - EXPECT(assert_equal(Vector_(1, 2.), lie1.localCoordinates(lie2))); + EXPECT(assert_equal((Vector)(Vector(1) << 2), lie1.localCoordinates(lie2))); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index 67eed6656..f66678c25 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -39,14 +39,9 @@ TEST( testLieVector, construction ) { TEST( testLieVector, other_constructors ) { Vector init = (Vector(2) << 10.0, 20.0); LieVector exp(init); - LieVector a(2,10.0,20.0); double data[] = {10,20}; LieVector b(2,data); - LieVector c(2.3), c_exp(LieVector(1, 2.3)); - EXPECT(assert_equal(exp, a)); EXPECT(assert_equal(exp, b)); - EXPECT(assert_equal(b, a)); - EXPECT(assert_equal(c_exp, c)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 34502ba99..828208361 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -43,18 +43,6 @@ TEST( matrix, constructor_data ) EQUALITY(A,B); } -/* ************************************************************************* */ -/* -TEST( matrix, constructor_vector ) -{ - double data[] = { -5, 3, 0, -5 }; - Matrix A = Matrix_(2, 2, -5, 3, 0, -5); - Vector v(4); - copy(data, data + 4, v.data()); - Matrix B = Matrix_(2, 2, v); // this one is column order ! - EQUALITY(A,trans(B)); -} -*/ /* ************************************************************************* */ TEST( matrix, Matrix_ ) { diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index bd3c2c217..f7e4d3baa 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -48,7 +48,8 @@ double f2(const LieVector& x) { /* ************************************************************************* */ TEST(testNumericalDerivative, numericalHessian2) { - LieVector center(2, 0.5, 1.0); + Vector v_center = (Vector(2) << 0.5, 1.0); + LieVector center(v_center); Matrix expected = (Matrix(2,2) << -cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)), @@ -67,7 +68,9 @@ double f3(const LieVector& x1, const LieVector& x2) { /* ************************************************************************* */ TEST(testNumericalDerivative, numericalHessian211) { - LieVector center1(1, 1.0), center2(1, 5.0); + Vector v_center1 = (Vector(1) << 1.0); + Vector v_center2 = (Vector(1) << 5.0); + LieVector center1(v_center1), center2(v_center2); Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0))); Matrix actual11 = numericalHessian211(f3, center1, center2); @@ -90,7 +93,11 @@ double f4(const LieVector& x, const LieVector& y, const LieVector& z) { /* ************************************************************************* */ TEST(testNumericalDerivative, numericalHessian311) { - LieVector center1(1, 1.0), center2(1, 2.0), center3(1, 3.0); + Vector v_center1 = (Vector(1) << 1.0); + Vector v_center2 = (Vector(1) << 2.0); + Vector v_center3 = (Vector(1) << 3.0); + LieVector center1(v_center1), center2(v_center2), center3(v_center3); + double x = center1(0), y = center2(0), z = center3(0); Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); Matrix actual11 = numericalHessian311(f4, center1, center2, center3); diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index fc2b7a266..ee0d94366 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -23,15 +23,6 @@ using namespace std; using namespace gtsam; -/* ************************************************************************* */ -TEST( TestVector, Vector_variants ) -{ - Vector a = (Vector(2) << 10.0,20.0); - double data[] = {10,20}; - Vector b = Vector_(2, data); - EXPECT(assert_equal(a, b)); -} - namespace { /* ************************************************************************* */ template 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/Point2.h b/gtsam/geometry/Point2.h index 0838650ea..dc9a1dac8 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -172,7 +172,7 @@ public: static inline Point2 Expmap(const Vector& v) { return Point2(v); } /// Log map around identity - just return the Point2 as a vector - static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } + static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()); } /// @} /// @name Vector Space 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/Pose3.cpp b/gtsam/geometry/Pose3.cpp index adbe763b3..bfd2fcb9a 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -98,7 +98,7 @@ Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, /* ************************************************************************* */ Matrix6 Pose3::dExpInv_exp(const Vector& xi) { // Bernoulli numbers, from Wikipedia - static const Vector B = Vector_(9, 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, + static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, 0.0, 1.0 / 42.0, 0.0, -1.0 / 30); static const int N = 5; // order of approximation Matrix res = I6; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 45012770f..d121beb12 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -170,7 +170,7 @@ namespace gtsam { ///Log map at identity - return the canonical coordinates of this rotation static inline Vector Logmap(const Rot2& r) { - return Vector_(1, r.theta()); + return (Vector(1) << r.theta()); } /// @} diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index d302a3502..847d31d65 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -194,7 +194,7 @@ namespace gtsam { * @return incremental rotation matrix */ static Rot3 rodriguez(double wx, double wy, double wz) - { return rodriguez(Vector_(3,wx,wy,wz));} + { return rodriguez((Vector(3) << wx, wy, wz));} /// @} /// @name Testable diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index ce386ac17..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 :-( @@ -170,7 +185,7 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const { double alpha = p.transpose() * q; assert(alpha != 0.0); Matrix coeffs = (B.transpose() * q) / alpha; - Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0)); + Vector result = (Vector(2) << coeffs(0, 0), coeffs(1, 0)); return result; } else { assert (false); 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/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index c73ae1182..53a21a9fb 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -36,7 +36,7 @@ TEST( Cal3DS2, uncalibrate) double g = 1+k[0]*r+k[1]*r*r ; double tx = 2*k[2]*p.x()*p.y() + k[3]*(r+2*p.x()*p.x()) ; double ty = k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ; - Vector v_hat = Vector_(3, g*p.x() + tx, g*p.y() + ty, 1.0) ; + Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0) ; Vector v_i = K.K() * v_hat ; Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ; Point2 q = K.uncalibrate(p); 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 ef4a3894c..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(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2))); + 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/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 3de661224..c67031a13 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -96,7 +96,7 @@ TEST(Rot2, logmap) { Rot2 rot0(Rot2::fromAngle(M_PI/2.0)); Rot2 rot(Rot2::fromAngle(M_PI)); - Vector expected = Vector_(1, M_PI/2.0); + Vector expected = (Vector(1) << M_PI/2.0); Vector actual = rot0.localCoordinates(rot); CHECK(assert_equal(expected, actual)); } diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index 61dadb8eb..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; @@ -212,9 +213,9 @@ inline static Vector randomVector(const Vector& minLimits, TEST(Sphere2, localCoordinates_retract) { size_t numIterations = 10000; - Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit = - Vector_(3, 1.0, 1.0, 1.0); - Vector minXiLimit = Vector_(2, -1.0, -1.0), maxXiLimit = Vector_(2, 1.0, 1.0); + Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit = + (Vector(3) << 1.0, 1.0, 1.0); + Vector minXiLimit = (Vector(2) << -1.0, -1.0), maxXiLimit = (Vector(2) << 1.0, 1.0); for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). @@ -242,9 +243,9 @@ TEST(Sphere2, localCoordinates_retract) { TEST(Sphere2, localCoordinates_retract_expmap) { size_t numIterations = 10000; - Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit = - Vector_(3, 1.0, 1.0, 1.0); - Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, M_PI, M_PI); + Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit = + (Vector(3) << 1.0, 1.0, 1.0); + Vector minXiLimit = (Vector(2) << -M_PI, -M_PI), maxXiLimit = (Vector(2) << M_PI, M_PI); for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). @@ -287,7 +288,7 @@ TEST(Sphere2, localCoordinates_retract_expmap) { // EXPECT(assert_equal(expected,actual1)); // EXPECT(assert_equal(expected,actual2)); // -// Matrix expectedH1 = Matrix_(3,3, +// Matrix expectedH1 = (Matrix(3,3) << // 0.0,-1.0,-2.0, // 1.0, 0.0,-2.0, // 0.0, 0.0,-1.0 @@ -298,7 +299,7 @@ TEST(Sphere2, localCoordinates_retract_expmap) { // // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx // EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1)); // -// Matrix expectedH2 = Matrix_(3,3, +// Matrix expectedH2 = (Matrix(3,3) << // 1.0, 0.0, 0.0, // 0.0, 1.0, 0.0, // 0.0, 0.0, 1.0 @@ -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/geometry/tests/timePose2.cpp b/gtsam/geometry/tests/timePose2.cpp index a8f2f7137..28044068c 100644 --- a/gtsam/geometry/tests/timePose2.cpp +++ b/gtsam/geometry/tests/timePose2.cpp @@ -138,7 +138,7 @@ int main() // create a random pose: double x = 4.0, y = 2.0, r = 0.3; - Vector v = Vector_(3,x,y,r); + Vector v = (Vector(3) << x, y, r); Pose2 X = Pose2(3,2,0.4), X2 = X.retract(v), X3(5,6,0.3); TEST(Expmap, Pose2::Expmap(v)); diff --git a/gtsam/geometry/tests/timeRot2.cpp b/gtsam/geometry/tests/timeRot2.cpp index 6d828ef9c..86dda2b8c 100644 --- a/gtsam/geometry/tests/timeRot2.cpp +++ b/gtsam/geometry/tests/timeRot2.cpp @@ -95,7 +95,7 @@ int main() // create a random direction: double norm=sqrt(16.0+4.0); double x=4.0/norm, y=2.0/norm; - Vector v = Vector_(2,x,y); + Vector v = (Vector(2) << x, y); Rot2 R = Rot2(0.4), R2 = R.retract(v), R3(0.6); TEST(Rot2_Expmap, Rot2::Expmap(v)); diff --git a/gtsam/geometry/tests/timeRot3.cpp b/gtsam/geometry/tests/timeRot3.cpp index 64feb1909..dd98465ed 100644 --- a/gtsam/geometry/tests/timeRot3.cpp +++ b/gtsam/geometry/tests/timeRot3.cpp @@ -39,7 +39,7 @@ int main() // create a random direction: double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; - Vector v = Vector_(3,x,y,z); + Vector v = (Vector(3) << x, y, z); Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2), R2 = R.retract(v); TEST("Rodriguez formula given axis angle", Rot3::rodriguez(v,0.001)) diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index a2f078ee2..502d9314b 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -49,15 +49,15 @@ namespace gtsam { * Example: * \code VectorValues values; - values.insert(3, Vector_(3, 1.0, 2.0, 3.0)); - values.insert(4, Vector_(2, 4.0, 5.0)); - values.insert(0, Vector_(4, 6.0, 7.0, 8.0, 9.0)); + values.insert(3, (Vector(3) << 1.0, 2.0, 3.0)); + values.insert(4, (Vector(2) << 4.0, 5.0)); + values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0)); // Prints [ 3.0 4.0 ] gtsam::print(values[1]); // Prints [ 8.0 9.0 ] - values[1] = Vector_(2, 8.0, 9.0); + values[1] = (Vector(2) << 8.0, 9.0); gtsam::print(values[1]); \endcode * diff --git a/gtsam/linear/tests/testSampler.cpp b/gtsam/linear/tests/testSampler.cpp index 1d041d017..9db8b7edc 100644 --- a/gtsam/linear/tests/testSampler.cpp +++ b/gtsam/linear/tests/testSampler.cpp @@ -24,7 +24,7 @@ const double tol = 1e-5; /* ************************************************************************* */ TEST(testSampler, basic) { - Vector sigmas = Vector_(3, 1.0, 0.1, 0.0); + Vector sigmas = (Vector(3) << 1.0, 0.1, 0.0); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas); char seed = 'A'; Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1); diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 0a7fb270c..f7e3cfec8 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -236,7 +236,7 @@ namespace gtsam { // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(15,15); F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, - H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, + H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; @@ -274,6 +274,7 @@ namespace gtsam { // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ + // deltaPij += deltaVij * deltaT; deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; deltaVij += deltaRij.matrix() * correctedAcc * deltaT; deltaRij = deltaRij * Rincr; @@ -341,8 +342,11 @@ namespace gtsam { public: /** Shorthand for a smart pointer to a factor */ - typedef boost::shared_ptr shared_ptr; - +#ifndef _MSC_VER + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif /** Default constructor - only use for serialization */ CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index b535f5179..32911e589 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -113,7 +113,7 @@ namespace imuBias { // // return measurement - biasGyro_ - w_earth_rate_I; // -//// Vector bias_gyro_temp(Vector_(3, -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2))); +//// Vector bias_gyro_temp((Vector(3) << -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2))); //// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G; // } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index c3753e5e7..bd8a0f80b 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -304,7 +304,11 @@ namespace gtsam { public: /** Shorthand for a smart pointer to a factor */ +#ifndef _MSC_VER + typedef typename boost::shared_ptr shared_ptr; +#else typedef boost::shared_ptr shared_ptr; +#endif /** Default constructor - only use for serialization */ ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 25a2765eb..2d766dc6a 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -168,9 +168,9 @@ TEST( CombinedImuFactor, ErrorWithBiases ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 2db7c7c68..cc88505bd 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -167,9 +167,9 @@ TEST( ImuFactor, Error ) // Linearization point imuBias::ConstantBias bias; // Bias Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -238,16 +238,16 @@ TEST( ImuFactor, ErrorWithBiases ) // Linearization point // Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1(3, 0.5, 0.0, 0.0); +// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// LieVector v2(3, 0.5, 0.0, 0.0); +// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -445,9 +445,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) //{ // // Linearization point // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1(3, 0.5, 0.0, 0.0); +// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// LieVector v2(3, 0.5, 0.0, 0.0); +// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); // // // Pre-integrator @@ -501,9 +501,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index d00be006b..469c78f33 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -122,8 +122,8 @@ struct GTSAM_EXPORT ISAM2Params { * entries would be added with: * \code FastMap thresholds; - thresholds['x'] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold - thresholds['l'] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold + thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold + thresholds['l'] = (Vector(3) << 1.0, 1.0, 1.0); // 1.0 m landmark position threshold params.relinearizeThreshold = thresholds; \endcode */ diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index bf75f134b..fd70519dc 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -19,7 +19,7 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)); +const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0)); const double tol = 1e-5; gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index ec386950d..041ea0387 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -65,7 +65,8 @@ public: TEST( Values, equals1 ) { Values expected; - LieVector v(3, 5.0, 6.0, 7.0); + LieVector v((Vector(3) << 5.0, 6.0, 7.0)); + expected.insert(key1,v); Values actual; actual.insert(key1,v); @@ -76,8 +77,9 @@ TEST( Values, equals1 ) TEST( Values, equals2 ) { Values cfg1, cfg2; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 5.0, 6.0, 8.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 5.0, 6.0, 8.0)); + cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -88,8 +90,9 @@ TEST( Values, equals2 ) TEST( Values, equals_nan ) { Values cfg1, cfg2; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, inf, inf, inf); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << inf, inf, inf)); + cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -100,10 +103,11 @@ TEST( Values, equals_nan ) TEST( Values, insert_good ) { Values cfg1, cfg2, expected; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 8.0, 9.0, 1.0); - LieVector v3(3, 2.0, 4.0, 3.0); - LieVector v4(3, 8.0, 3.0, 7.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); + LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); + LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); + cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key3, v4); @@ -121,10 +125,11 @@ TEST( Values, insert_good ) TEST( Values, insert_bad ) { Values cfg1, cfg2; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 8.0, 9.0, 1.0); - LieVector v3(3, 2.0, 4.0, 3.0); - LieVector v4(3, 8.0, 3.0, 7.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); + LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); + LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); + cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key2, v3); @@ -137,8 +142,8 @@ TEST( Values, insert_bad ) TEST( Values, update_element ) { Values cfg; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 8.0, 9.0, 1.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); cfg.insert(key1, v1); CHECK(cfg.size() == 1); @@ -181,8 +186,8 @@ TEST(Values, basic_functions) //TEST(Values, dim_zero) //{ // Values config0; -// config0.insert(key1, LieVector(2, 2.0, 3.0)); -// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); +// config0.insert(key1, LieVector((Vector(2) << 2.0, 3.0)); +// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)); // LONGS_EQUAL(5, config0.dim()); // // VectorValues expected; @@ -195,16 +200,16 @@ TEST(Values, basic_functions) TEST(Values, expmap_a) { Values config0; - config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); VectorValues increment = pair_list_of (key1, (Vector(3) << 1.0, 1.1, 1.2)) (key2, (Vector(3) << 1.3, 1.4, 1.5)); Values expected; - expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); - expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); + expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); + expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); CHECK(assert_equal(expected, config0.retract(increment))); } @@ -213,15 +218,15 @@ TEST(Values, expmap_a) TEST(Values, expmap_b) { Values config0; - config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); VectorValues increment = pair_list_of (key2, (Vector(3) << 1.3, 1.4, 1.5)); Values expected; - expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); + expected.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); CHECK(assert_equal(expected, config0.retract(increment))); } @@ -230,16 +235,16 @@ TEST(Values, expmap_b) //TEST(Values, expmap_c) //{ // Values config0; -// config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); -// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); +// config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); +// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); // // Vector increment = LieVector(6, // 1.0, 1.1, 1.2, // 1.3, 1.4, 1.5); // // Values expected; -// expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); -// expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); +// expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); +// expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); // // CHECK(assert_equal(expected, config0.retract(increment))); //} @@ -248,8 +253,8 @@ TEST(Values, expmap_b) TEST(Values, expmap_d) { Values config0; - config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); //config0.print("config0"); CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); @@ -266,8 +271,8 @@ TEST(Values, expmap_d) TEST(Values, localCoordinates) { Values valuesA; - valuesA.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + valuesA.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + valuesA.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); VectorValues expDelta = pair_list_of (key1, (Vector(3) << 0.1, 0.2, 0.3)) @@ -314,17 +319,17 @@ TEST(Values, exists_) TEST(Values, update) { Values config0; - config0.insert(key1, LieVector(1, 1.)); - config0.insert(key2, LieVector(1, 2.)); + config0.insert(key1, LieVector((Vector(1) << 1.))); + config0.insert(key2, LieVector((Vector(1) << 2.))); Values superset; - superset.insert(key1, LieVector(1, -1.)); - superset.insert(key2, LieVector(1, -2.)); + superset.insert(key1, LieVector((Vector(1) << -1.))); + superset.insert(key2, LieVector((Vector(1) << -2.))); config0.update(superset); Values expected; - expected.insert(key1, LieVector(1, -1.)); - expected.insert(key2, LieVector(1, -2.)); + expected.insert(key1, LieVector((Vector(1) << -1.))); + expected.insert(key2, LieVector((Vector(1) << -2.))); CHECK(assert_equal(expected,config0)); } diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 6f00c81a6..cf5760695 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -96,7 +96,7 @@ namespace gtsam { Vector e1 = Rot::Logmap(measuredBearing_.between(y1)); double y2 = pose.range(point, H21_, H22_); - Vector e2 = Vector_(1, y2 - measuredRange_); + Vector e2 = (Vector(1) << y2 - measuredRange_); if (H1) *H1 = gtsam::stack(2, &H11, &H21); if (H2) *H2 = gtsam::stack(2, &H12, &H22); 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/RangeFactor.h b/gtsam/slam/RangeFactor.h index 7b807d88f..fe140a298 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -73,7 +73,7 @@ namespace gtsam { } else { hx = pose.range(point, H1, H2); } - return Vector_(1, hx - measured_); + return (Vector(1) << hx - measured_); } /** return the measured */ 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 diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index b37b36546..a7c91de3f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) { Pose3 x1(R,t1); values.insert(X(1), GeneralCamera(x1)); Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values))); + EXPECT(assert_equal(((Vector) (Vector(2) << -3.0, 0.0)), factor->unwhitenedError(values))); } static const double baseline = 5.0 ; diff --git a/gtsam_unstable/base/FixedVector.h b/gtsam_unstable/base/FixedVector.h index 53a9190ce..dd3668087 100644 --- a/gtsam_unstable/base/FixedVector.h +++ b/gtsam_unstable/base/FixedVector.h @@ -45,23 +45,6 @@ public: std::copy(values, values+N, this->data()); } - /** - * nice constructor, dangerous as number of arguments must be exactly right - * and you have to pass doubles !!! always use 0.0 never 0 - * - * NOTE: this will throw warnings/explode if there is no argument - * before the variadic section, so there is a meaningless size argument. - */ - FixedVector(size_t n, ...) { - va_list ap; - va_start(ap, n); - for(size_t i = 0 ; i < N ; i++) { - double value = va_arg(ap, double); - (*this)(i) = value; - } - va_end(ap); - } - /** * Create vector initialized to a constant value * @param value constant value diff --git a/gtsam_unstable/base/tests/testFixedVector.cpp b/gtsam_unstable/base/tests/testFixedVector.cpp index 1fddf80ca..e5e297b7c 100644 --- a/gtsam_unstable/base/tests/testFixedVector.cpp +++ b/gtsam_unstable/base/tests/testFixedVector.cpp @@ -28,7 +28,7 @@ static const double tol = 1e-9; /* ************************************************************************* */ TEST( testFixedVector, conversions ) { double data1[] = {1.0, 2.0, 3.0}; - Vector v1 = Vector_(3, data1); + Vector v1 = (Vector(3) << data1[0], data1[1], data1[2]); TestVector3 fv1(v1), fv2(data1); Vector actFv2(fv2); @@ -37,7 +37,7 @@ TEST( testFixedVector, conversions ) { /* ************************************************************************* */ TEST( testFixedVector, variable_constructor ) { - TestVector3 act(3, 1.0, 2.0, 3.0); + TestVector3 act((Vector(3) << 1.0, 2.0, 3.0)); EXPECT_DOUBLES_EQUAL(1.0, act(0), tol); EXPECT_DOUBLES_EQUAL(2.0, act(1), tol); EXPECT_DOUBLES_EQUAL(3.0, act(2), tol); @@ -45,8 +45,9 @@ TEST( testFixedVector, variable_constructor ) { /* ************************************************************************* */ TEST( testFixedVector, equals ) { - TestVector3 vec1(3, 1.0, 2.0, 3.0), vec2(3, 1.0, 2.0, 3.0), vec3(3, 2.0, 3.0, 4.0); - TestVector5 vec4(5, 1.0, 2.0, 3.0, 4.0, 5.0); + TestVector3 vec1((Vector(3) << 1.0, 2.0, 3.0)), vec2((Vector(3) << 1.0, 2.0, 3.0)), + vec3((Vector(3) << 2.0, 3.0, 4.0)); + TestVector5 vec4((Vector(5) << 1.0, 2.0, 3.0, 4.0, 5.0)); EXPECT(assert_equal(vec1, vec1, tol)); EXPECT(assert_equal(vec1, vec2, tol)); @@ -60,23 +61,23 @@ TEST( testFixedVector, equals ) { /* ************************************************************************* */ TEST( testFixedVector, static_constructors ) { TestVector3 actZero = TestVector3::zero(); - TestVector3 expZero(3, 0.0, 0.0, 0.0); + TestVector3 expZero((Vector(3) << 0.0, 0.0, 0.0)); EXPECT(assert_equal(expZero, actZero, tol)); TestVector3 actOnes = TestVector3::ones(); - TestVector3 expOnes(3, 1.0, 1.0, 1.0); + TestVector3 expOnes((Vector(3) << 1.0, 1.0, 1.0)); EXPECT(assert_equal(expOnes, actOnes, tol)); TestVector3 actRepeat = TestVector3::repeat(2.3); - TestVector3 expRepeat(3, 2.3, 2.3, 2.3); + TestVector3 expRepeat((Vector(3) << 2.3, 2.3, 2.3)); EXPECT(assert_equal(expRepeat, actRepeat, tol)); TestVector3 actBasis = TestVector3::basis(1); - TestVector3 expBasis(3, 0.0, 1.0, 0.0); + TestVector3 expBasis((Vector(3) << 0.0, 1.0, 0.0)); EXPECT(assert_equal(expBasis, actBasis, tol)); TestVector3 actDelta = TestVector3::delta(1, 2.3); - TestVector3 expDelta(3, 0.0, 2.3, 0.0); + TestVector3 expDelta((Vector(3) << 0.0, 2.3, 0.0)); EXPECT(assert_equal(expDelta, actDelta, tol)); } diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp index df9e14a36..3e1af8eb3 100644 --- a/gtsam_unstable/geometry/BearingS2.cpp +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -69,7 +69,7 @@ BearingS2 BearingS2::retract(const Vector& v) const { /* ************************************************************************* */ Vector BearingS2::localCoordinates(const BearingS2& x) const { - return Vector_(2, azimuth_.localCoordinates(x.azimuth_)(0), + return (Vector(2) << azimuth_.localCoordinates(x.azimuth_)(0), elevation_.localCoordinates(x.elevation_)(0)); } diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index c33505c54..4eb7992a2 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -166,7 +166,7 @@ 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(5, pt.x(),pt.y(),pt.z(), theta, phi), + return std::make_pair(gtsam::LieVector((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)), gtsam::LieScalar(1./depth)); } diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index 85b592b98..15fd47236 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -145,7 +145,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( // extend line by random dist and angle to get BC double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len); double tABC = randomAngle().theta(); - Pose2 xB = xA.retract(Vector_(3, dAB, 0.0, tABC)); + Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC)); // extend from B to find C double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len); diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index 78347a077..894312556 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -128,7 +128,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, // calculate angle to change by Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta()); - Pose2 test_pose = cur_pose.retract(Vector_(3, step_size, 0.0, Rot2::Logmap(dtheta)(0))); + Pose2 test_pose = cur_pose.retract((Vector(3) << step_size, 0.0, Rot2::Logmap(dtheta)(0))); // create a segment to use for intersection checking // find the closest intersection diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index e899da0c9..b477d3e44 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark(5, 1., 0., 1., 0., 0.); + LieVector inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); LieScalar inv_depth(1./4); Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); EXPECT(assert_equal(expected_uv, actual_uv)); @@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))); + LieVector diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)))); LieScalar inv_depth(1/sqrt(3.0)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))); + LieVector diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)))); LieScalar inv_depth( 1./sqrt(1.0+1+4)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))); + LieVector diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)))); LieScalar inv_depth(1./sqrt(1.+16.+4.)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -88,7 +88,7 @@ Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& i TEST( InvDepthFactor, Dproject_pose) { - LieVector landmark(6,0.1,0.2,0.3, 0.1,0.2); + LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_landmark) { - LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); + LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_inv_depth) { - LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); + LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -124,7 +124,7 @@ TEST( InvDepthFactor, Dproject_inv_depth) /* ************************************************************************* */ TEST(InvDepthFactor, backproject) { - LieVector expected(5,0.,0.,1., 0.1,0.2); + LieVector expected((Vector(5) << 0.,0.,1., 0.1,0.2)); LieScalar inv_depth(1./4); InvDepthCamera3 inv_camera(level_pose,K); Point2 z = inv_camera.project(expected, inv_depth); @@ -140,7 +140,7 @@ TEST(InvDepthFactor, backproject) TEST(InvDepthFactor, backproject2) { // backwards facing camera - LieVector expected(5,-5.,-5.,2., 3., -0.1); + LieVector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); LieScalar inv_depth(1./10); InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); Point2 z = inv_camera.project(expected, inv_depth); diff --git a/gtsam_unstable/geometry/tests/testPose3Upright.cpp b/gtsam_unstable/geometry/tests/testPose3Upright.cpp index 806f7dc16..dacf28992 100644 --- a/gtsam_unstable/geometry/tests/testPose3Upright.cpp +++ b/gtsam_unstable/geometry/tests/testPose3Upright.cpp @@ -72,7 +72,7 @@ TEST( testPose3Upright, manifold ) { EXPECT(assert_equal(x1, x1.retract(zero(4)), tol)); EXPECT(assert_equal(x2, x2.retract(zero(4)), tol)); - Vector delta12 = Vector_(4, 3.0, 0.0, 4.0, 0.0), delta21 = -delta12; + Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0), delta21 = -delta12; EXPECT(assert_equal(x2, x1.retract(delta12), tol)); EXPECT(assert_equal(x1, x2.retract(delta21), tol)); diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 8eff62acb..234ad25bc 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -266,7 +266,7 @@ namespace gtsam { } } - return Vector_(2, p_inlier, p_outlier); + return (Vector(2) << p_inlier, p_outlier); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/CombinedImuFactor.h b/gtsam_unstable/slam/CombinedImuFactor.h deleted file mode 100644 index bf86b6933..000000000 --- a/gtsam_unstable/slam/CombinedImuFactor.h +++ /dev/null @@ -1,673 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 CombinedImuFactor.h - * @author Luca Carlone, Stephen Williams - **/ - -#pragma once - -/* GTSAM includes */ -#include -#include -#include -#include -#include -#include - -/* External or standard includes */ -#include - - -namespace gtsam { - - /** - * - * @addtogroup SLAM - * - * REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. - */ - - class CombinedImuFactor: public NoiseModelFactor6 { - - public: - - /** Struct to store results of preintegrating IMU measurements. Can be build - * incrementally so as to avoid costly integration at time of factor construction. */ - - /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */ - static Matrix3 rightJacobianExpMapSO3(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jr; - if (normx < 10e-8){ - Jr = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + - ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian - } - return Jr; - } - - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jrinv; - - if (normx < 10e-8){ - Jrinv = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = Matrix3::Identity() + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; - } - return Jrinv; - } - - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ - class CombinedPreintegratedMeasurements { - public: - imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration - Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector - ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) - - Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i) - double deltaTij; ///< Time interval from i to j - - Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - - Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) - ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases - ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] - /** Default constructor, initialize with no IMU measurements */ - CombinedPreintegratedMeasurements( - const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) - const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) - const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements - ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements) - ) : biasHat(bias), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), - delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), - delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) - { - // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) - measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3); - } - - CombinedPreintegratedMeasurements() : - biasHat(imuBias::ConstantBias()), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), - delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), - delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) - { - } - - /** print */ - void print(const std::string& s = "Preintegrated Measurements:") const { - std::cout << s << std::endl; - biasHat.print(" biasHat"); - std::cout << " deltaTij " << deltaTij << std::endl; - std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl; - deltaRij.print(" deltaRij "); - std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl; - std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl; - } - - /** equals */ - bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const { - return biasHat.equals(expected.biasHat, tol) - && equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol) - && equal_with_abs_tol(deltaPij, expected.deltaPij, tol) - && equal_with_abs_tol(deltaVij, expected.deltaVij, tol) - && deltaRij.equals(expected.deltaRij, tol) - && std::fabs(deltaTij - expected.deltaTij) < tol - && equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol) - && equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol) - && equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol) - && equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol) - && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); - } - - /** Add a single IMU measurement to the preintegration. */ - void integrateMeasurement( - const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) - const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) - double deltaT, ///< Time step - boost::optional body_P_sensor = boost::none ///< Sensor frame - ) { - // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. - // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty - Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr - - // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - delPdelBiasAcc += delVdelBiasAcc * deltaT; - delPdelBiasOmega += delVdelBiasOmega * deltaT; - delVdelBiasAcc += -deltaRij.matrix() * deltaT; - delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; - delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; - - // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that - // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we - // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - Matrix3 Z_3x3 = Matrix3::Zero(); - Matrix3 I_3x3 = Matrix3::Identity(); - const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3) - const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i); - - Rot3 Rot_j = deltaRij * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j); - - // Single Jacobians to propagate covariance - Matrix3 H_pos_pos = I_3x3; - Matrix3 H_pos_vel = I_3x3 * deltaT; - Matrix3 H_pos_angles = Z_3x3; - - Matrix3 H_vel_pos = Z_3x3; - Matrix3 H_vel_vel = I_3x3; - Matrix3 H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - Matrix3 H_vel_biasacc = - deltaRij.matrix() * deltaT; - - Matrix3 H_angles_pos = Z_3x3; - Matrix3 H_angles_vel = Z_3x3; - Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(15,15); - F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, - H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, - H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; - - - // first order uncertainty propagation - // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - - Matrix G_measCov_Gt = Matrix::Zero(15,15); - // BLOCK DIAGONAL TERMS - G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3); - -// G_measCov_Gt.block(3,3,3,3) = (H_vel_biasacc) * (1/deltaT) * measurementCovariance.block(3,3,3,3) * (H_vel_biasacc.transpose()) + -// (H_vel_biasacc) * (1/deltaT) * -// ( measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) * -// (H_vel_biasacc.transpose()); - - G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) * - (measurementCovariance.block(3,3,3,3) + measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) * - (H_vel_biasacc.transpose()); - - G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) * - (measurementCovariance.block(6,6,3,3) + measurementCovariance.block(12,12,3,3) + measurementCovariance.block(18,18,3,3) ) * - (H_angles_biasomega.transpose()); - - G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance.block(9,9,3,3); - - G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance.block(12,12,3,3); - - // OFF BLOCK DIAGONAL TERMS - Matrix3 block24 = H_vel_biasacc * measurementCovariance.block(9,9,3,3); - G_measCov_Gt.block(3,9,3,3) = block24; - G_measCov_Gt.block(9,3,3,3) = block24.transpose(); - - Matrix3 block35 = H_angles_biasomega * measurementCovariance.block(12,12,3,3); - G_measCov_Gt.block(6,12,3,3) = block35; - G_measCov_Gt.block(12,6,3,3) = block35.transpose(); - - /* - // overall Jacobian wrt raw measurements (df/du) - Matrix3 H_vel_initbiasacc = H_vel_biasacc; - Matrix3 H_angles_initbiasomega = H_angles_biasomega; - - // COMBINED IMU FACTOR, preserves correlation with bias evolution and considers initial uncertainty on biases - Matrix G(15,21); - G << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, - H_vel_biasacc, Z_3x3, H_vel_biasacc, Z_3x3, H_vel_initbiasacc, Z_3x3, - Z_3x3, Z_3x3, - H_angles_biasomega, Z_3x3, H_angles_biasomega, Z_3x3, H_angles_initbiasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3; - - Matrix ErrorMatrix = (1/deltaT) * G * measurementCovariance * G.transpose() - G_measCov_Gt; - std::cout << "---- matrix multiplication error = [" << ErrorMatrix << "];"<< std::endl; - double max_err=0; - for(int i=0;i<15;i++) - { - for(int j=0;j<15;j++) - { - if(fabs(ErrorMatrix(i,j))>max_err) - max_err = fabs(ErrorMatrix(i,j)); - } - } - std::cout << "---- max matrix multiplication error = [" << max_err << "];"<< std::endl; - - if(max_err>10e-15) - std::cout << "---- max matrix multiplication error *large* = [" << max_err << "];"<< std::endl; - - PreintMeasCov = F * PreintMeasCov * F.transpose() + (1/deltaT) * G * measurementCovariance * G.transpose(); - */ - - PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt; - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - deltaPij += deltaVij * deltaT; - deltaVij += deltaRij.matrix() * correctedAcc * deltaT; - deltaRij = deltaRij * Rincr; - deltaTij += deltaT; - } - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance); - ar & BOOST_SERIALIZATION_NVP(deltaPij); - ar & BOOST_SERIALIZATION_NVP(deltaVij); - ar & BOOST_SERIALIZATION_NVP(deltaRij); - ar & BOOST_SERIALIZATION_NVP(deltaTij); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega); - } - }; - - private: - - typedef CombinedImuFactor This; - typedef NoiseModelFactor6 Base; - - CombinedPreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - - public: - - /** Shorthand for a smart pointer to a factor */ -#ifndef _MSC_VER - typedef typename boost::shared_ptr shared_ptr; -#else - typedef boost::shared_ptr shared_ptr; -#endif - /** Default constructor - only use for serialization */ - CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} - - /** Constructor */ - CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - const SharedNoiseModel& model) : - Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis) { - } - - virtual ~CombinedImuFactor() {} - - /// @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 { - std::cout << s << "CombinedImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "," - << keyFormatter(this->key6()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; - this->noiseModel_->print(" noise model: "); - } - - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol); - } - - /** Access the preintegrated measurements. */ - const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } - - /** implement functions needed to derive from Factor */ - - /** vector of errors */ - Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, - const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const - { - - const double& deltaTij = preintegratedMeasurements_.deltaTij; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(15,6); - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - - Rot_i.matrix(), - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - Matrix3::Zero(), - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(), - //dBiasAcc/dPi - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPi - Matrix3::Zero(), Matrix3::Zero(); - } - - if(H2) { - H2->resize(15,3); - (*H2) << - // dfP/dVi - - Matrix3::Identity() * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(), - //dBiasAcc/dVi - Matrix3::Zero(), - //dBiasOmega/dVi - Matrix3::Zero(); - - } - - if(H3) { - - H3->resize(15,6); - (*H3) << - // dfP/dPosej - Matrix3::Zero(), Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(), - //dBiasAcc/dPosej - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPosej - Matrix3::Zero(), Matrix3::Zero(); - } - - if(H4) { - H4->resize(15,3); - (*H4) << - // dfP/dVj - Matrix3::Zero(), - // dfV/dVj - Matrix3::Identity(), - // dfR/dVj - Matrix3::Zero(), - //dBiasAcc/dVj - Matrix3::Zero(), - //dBiasOmega/dVj - Matrix3::Zero(); - } - - if(H5) { - const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; - - H5->resize(15,6); - (*H5) << - // dfP/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega, - // dfV/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega, - // dfR/dBias_i - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), - //dBiasAcc/dBias_i - -Matrix3::Identity(), Matrix3::Zero(), - //dBiasOmega/dBias_i - Matrix3::Zero(), -Matrix3::Identity(); - } - - if(H6) { - - H6->resize(15,6); - (*H6) << - // dfP/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - // dfV/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - // dfR/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - //dBiasAcc/dBias_j - Matrix3::Identity(), Matrix3::Zero(), - //dBiasOmega/dBias_j - Matrix3::Zero(), Matrix3::Identity(); - } - - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); - - const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); - - Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15 - - return r; - } - - - /** predicted states from IMU */ - static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, - const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j, - const CombinedPreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis) - { - - const double& deltaTij = preintegratedMeasurements.deltaTij; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - pose_j = Pose3( Rot_j, Point3(pos_j) ); - - bias_j = bias_i; - } - - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor6", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - } - }; // \class CombinedImuFactor - -} /// namespace gtsam diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 5f09ec216..dec1b2378 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -537,7 +537,7 @@ public: Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); - Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5)); omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; // Calculating g @@ -551,7 +551,7 @@ public: double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); double g_calc( g0/( pow(1 + height/Ro, 2) ) ); - g_ENU = Vector_(3, 0.0, 0.0, -g_calc); + g_ENU = (Vector(3) << 0.0, 0.0, -g_calc); // Calculate rho @@ -560,7 +560,7 @@ public: double rho_E = -Vn/(Rm + height); double rho_N = Ve/(Rp + height); double rho_U = Ve*tan(lat_new)/(Rp + height); - rho_ENU = Vector_(3, rho_E, rho_N, rho_U); + rho_ENU = (Vector(3) << rho_E, rho_N, rho_U); } static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ diff --git a/gtsam_unstable/slam/ImuFactor.h b/gtsam_unstable/slam/ImuFactor.h deleted file mode 100644 index 0c08ed7be..000000000 --- a/gtsam_unstable/slam/ImuFactor.h +++ /dev/null @@ -1,565 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts - **/ - -#pragma once - -/* GTSAM includes */ -#include -#include -#include -#include -#include -#include - -/* External or standard includes */ -#include - - -namespace gtsam { - - /** - * - * @addtogroup SLAM - * * REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. - */ - - class ImuFactor: public NoiseModelFactor5 { - - public: - - /** Struct to store results of preintegrating IMU measurements. Can be build - * incrementally so as to avoid costly integration at time of factor construction. */ - - /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */ - static Matrix3 rightJacobianExpMapSO3(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jr; - if (normx < 10e-8){ - Jr = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + - ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian - } - return Jr; - } - - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jrinv; - - if (normx < 10e-8){ - Jrinv = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = Matrix3::Identity() + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; - } - return Jrinv; - } - - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ - class PreintegratedMeasurements { - public: - imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration - Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - - Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, , in [2]) (in frame i) - Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i) - double deltaTij; ///< Time interval from i to j - - Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - - Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) - - Vector3 initialRotationRate; ///< initial rotation rate reading from the IMU (at time i) - Vector3 finalRotationRate; ///< final rotation rate reading from the IMU (at time j) - - /** Default constructor, initialize with no IMU measurements */ - PreintegratedMeasurements( - const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc - const Vector3& initialRotationRate = Vector3::Zero() ///< initial rotation rate reading from the IMU (at time i) - ) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), - delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), - delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), - initialRotationRate(initialRotationRate), finalRotationRate(initialRotationRate) - { - measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; - PreintMeasCov = Matrix::Zero(9,9); - } - - PreintegratedMeasurements() : - biasHat(imuBias::ConstantBias()), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), - delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), - delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), - initialRotationRate(Vector3::Zero()), finalRotationRate(Vector3::Zero()) - { - measurementCovariance = Matrix::Zero(9,9); - PreintMeasCov = Matrix::Zero(9,9); - } - - /** print */ - void print(const std::string& s = "Preintegrated Measurements:") const { - std::cout << s << std::endl; - biasHat.print(" biasHat"); - std::cout << " deltaTij " << deltaTij << std::endl; - std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl; - deltaRij.print(" deltaRij "); - std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl; - std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl; - } - - /** equals */ - bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const { - return biasHat.equals(expected.biasHat, tol) - && equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol) - && equal_with_abs_tol(deltaPij, expected.deltaPij, tol) - && equal_with_abs_tol(deltaVij, expected.deltaVij, tol) - && deltaRij.equals(expected.deltaRij, tol) - && std::fabs(deltaTij - expected.deltaTij) < tol - && equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol) - && equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol) - && equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol) - && equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol) - && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); - } - - /** Add a single IMU measurement to the preintegration. */ - void integrateMeasurement( - const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) - const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) - double deltaT, ///< Time step - boost::optional body_P_sensor = boost::none ///< Sensor frame - ) { - - // NOTE: order is important here because each update uses old values. - // First we compensate the measurements for the bias - Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega); - - finalRotationRate = correctedOmega; - - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - - const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr - - // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - delPdelBiasAcc += delVdelBiasAcc * deltaT; - delPdelBiasOmega += delVdelBiasOmega * deltaT; - delVdelBiasAcc += -deltaRij.matrix() * deltaT; - delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; - delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; - - // Update preintegrated mesurements covariance - /* ----------------------------------------------------------------------------------------------------------------------- */ - Matrix3 Z_3x3 = Matrix3::Zero(); - Matrix3 I_3x3 = Matrix3::Identity(); - const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3) - const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i); - - Rot3 Rot_j = deltaRij * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j); - - // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that - // can be seen as a prediction phase in an EKF framework - Matrix H_pos_pos = I_3x3; - Matrix H_pos_vel = I_3x3 * deltaT; - Matrix H_pos_angles = Z_3x3; - - Matrix H_vel_pos = Z_3x3; - Matrix H_vel_vel = I_3x3; - Matrix H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - - Matrix H_angles_pos = Z_3x3; - Matrix H_angles_vel = Z_3x3; - Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(9,9); - F << H_pos_pos, H_pos_vel, H_pos_angles, - H_vel_pos, H_vel_vel, H_vel_angles, - H_angles_pos, H_angles_vel, H_angles_angles; - - - // first order uncertainty propagation - // the deltaT allows to pass from continuous time noise to discrete time noise - PreintMeasCov = F * PreintMeasCov * F.transpose() + measurementCovariance * deltaT ; - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - deltaPij += deltaVij * deltaT; - deltaVij += deltaRij.matrix() * correctedAcc * deltaT; - deltaRij = deltaRij * Rincr; - deltaTij += deltaT; - } - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance); - ar & BOOST_SERIALIZATION_NVP(deltaPij); - ar & BOOST_SERIALIZATION_NVP(deltaVij); - ar & BOOST_SERIALIZATION_NVP(deltaRij); - ar & BOOST_SERIALIZATION_NVP(deltaTij); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega); - } - }; - - private: - - typedef ImuFactor This; - typedef NoiseModelFactor5 Base; - - PreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - public: - - /** Shorthand for a smart pointer to a factor */ -#ifndef _MSC_VER - typedef typename boost::shared_ptr shared_ptr; -#else - typedef boost::shared_ptr shared_ptr; -#endif - /** Default constructor - only use for serialization */ - ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} - - /** Constructor */ - ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : - Base(model, pose_i, vel_i, pose_j, vel_j, bias), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor) { - } - - virtual ~ImuFactor() {} - - /// @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 { - std::cout << s << "ImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; - this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - } - - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } - - /** Access the preintegrated measurements. */ - const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } - - /** implement functions needed to derive from Factor */ - - /** vector of errors */ - Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, - const imuBias::ConstantBias& bias, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const - { - - const double& deltaTij = preintegratedMeasurements_.deltaTij; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(9,6); - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - - Rot_i.matrix(), - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - Matrix3::Zero(), - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(); - } - if(H2) { - H2->resize(9,3); - (*H2) << - // dfP/dVi - - Matrix3::Identity() * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(); - - } - if(H3) { - - H3->resize(9,6); - (*H3) << - // dfP/dPosej - Matrix3::Zero(), Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(); - } - if(H4) { - H4->resize(9,3); - (*H4) << - // dfP/dVj - Matrix3::Zero(), - // dfV/dVj - Matrix3::Identity(), - // dfR/dVj - Matrix3::Zero(); - } - if(H5) { - - const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; - - H5->resize(9,6); - (*H5) << - // dfP/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega, - // dfV/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega, - // dfR/dBias - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - Vector r(9); r << fp, fv, fR; - return r; - } - - - /** predicted states from IMU */ - static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, - const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none) - { - - const double& deltaTij = preintegratedMeasurements.deltaTij; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - pose_j = Pose3( Rot_j, Point3(pos_j) ); - } - - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor5", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); - } - }; // \class ImuFactor - -} /// namespace gtsam diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 3cd87b8d2..29f9d4972 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -96,7 +96,7 @@ public: " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (gtsam::Vector(1) << 0.0); } /** return the measurement */ diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 5e67a39d3..d61787358 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -96,7 +96,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (gtsam::Vector(1) << 0.0); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 2da440425..9d4113431 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -99,7 +99,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (gtsam::Vector(1) << 0.0); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 711a44bf9..747ceafe7 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -35,7 +35,7 @@ public: } Vector b_g(double g_e) { - Vector n_g = Vector_(3, 0.0, 0.0, g_e); + Vector n_g = (Vector(3) << 0.0, 0.0, g_e); return (bRn_ * n_g).vector(); } diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index bf0b2b6e3..1925a3fe4 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -70,7 +70,7 @@ namespace gtsam { /** Single Element Constructor: acts on a single parameter specified by idx */ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : - Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { + Base(model, key), prior_((Vector(1) << prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { assert(model->dim() == 1); this->fillH(); } diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index 11d8ed3a3..292e3f68f 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -32,7 +32,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p *H2 = zeros(1, 3); (*H2)(0, 2) = -1.0; } - return Vector_(1, hx - measured_); + return (Vector(1) << hx - measured_); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index 5ff8f66e7..0411765e8 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -276,7 +276,7 @@ namespace gtsam { } } - return Vector_(2, p_inlier, p_outlier); + return (Vector(2) << p_inlier, p_outlier); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp index cc3d693ad..a3c963a58 100644 --- a/gtsam_unstable/slam/tests/testAHRS.cpp +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -75,7 +75,7 @@ TEST (AHRS, Mechanization_integrate) { Mechanization_bRn2 mech; KalmanFilter::State state; // boost::tie(mech,state) = ahrs.initialize(g_e); -// Vector u = Vector_(3,0.05,0.0,0.0); +// Vector u = (Vector(3) << 0.05,0.0,0.0); // double dt = 2; // Rot3 expected; // Mechanization_bRn2 mech2 = mech.integrate(u,dt); diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 53d94a4bc..7756e79d3 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -119,10 +119,10 @@ TEST( InertialNavFactor_GlobalVelocity, Predict) InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - LieVector Vel1(3, 0.50, -0.50, 0.40); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector expectedVel2(3, 0.51, -0.48, 0.43); + LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); Pose3 actualPose2; LieVector actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); @@ -157,8 +157,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector Vel1(3, 0.50, -0.50, 0.40); - LieVector Vel2(3, 0.51, -0.48, 0.43); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); + LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -192,8 +192,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot) Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0)); - LieVector Vel1(3,0.0,0.0,0.0); - LieVector Vel2(3,0.0,0.0,0.0); + LieVector Vel1((Vector(3) << 0.0, 0.0, 0.0)); + LieVector Vel2((Vector(3) << 0.0, 0.0, 0.0)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -230,7 +230,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); @@ -302,13 +302,13 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) { -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Pose3 Pose2(R2, t2); - LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); + LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); imuBias::ConstantBias Bias1; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; @@ -447,10 +447,10 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - LieVector Vel1(3, 0.50, -0.50, 0.40); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector expectedVel2(3, 0.51, -0.48, 0.43); + LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); Pose3 actualPose2; LieVector actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); @@ -488,8 +488,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector Vel1(3, 0.50, -0.50, 0.40); - LieVector Vel2(3, 0.51, -0.48, 0.43); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); + LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -527,8 +527,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); Pose3 Pose2(Rot3::Expmap(body_P_sensor.rotation().matrix()*measurement_gyro*measurement_dt), Point3(2.0, 1.0, 3.0)); - LieVector Vel1(3,0.0,0.0,0.0); - LieVector Vel2(3,0.0,0.0,0.0); + LieVector Vel1((Vector(3) << 0.0,0.0,0.0)); + LieVector Vel2((Vector(3) << 0.0,0.0,0.0)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -569,7 +569,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); @@ -618,13 +618,13 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Pose3 Pose2(R2, t2); - LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); + LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); imuBias::ConstantBias Bias1; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index 65cc1adbc..5ea9fe29d 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -38,7 +38,7 @@ TEST( InvDepthFactor, optimize) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark(5, 0., 0., 1., 0., 0.); + LieVector 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); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 7bbee65ee..24535854d 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(6, x, y, z, theta, phi, rho); + LieVector expected((Vector(6) << x, y, z, theta, phi, rho)); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index 1512d0fc2..e99c9bcdf 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(3, theta, phi, rho); + LieVector expected((Vector(3) << theta, phi, rho)); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index 799ebdf36..e65b7cacb 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(3, theta, phi, rho); + LieVector expected((Vector(3) << theta, phi, rho)); diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index dd16e6a9e..7b5f31660 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -187,7 +187,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { EXPECT(!rangeBound.isGreaterThan()); EXPECT(rangeBound.dim() == 1); - EXPECT(assert_equal(Vector_(1, 2.0), rangeBound.evaluateError(pt1, pt1))); + EXPECT(assert_equal(((Vector)Vector(1) << 2.0), rangeBound.evaluateError(pt1, pt1))); EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2))); EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3))); EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4))); diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index ea34afa79..0b84f137d 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -199,7 +199,7 @@ TEST(GaussianJunctionTreeB, simpleMarginal) { // Create a simple graph NonlinearFactorGraph fg; fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); - fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0)))); + fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas((Vector(3) << 10.0, 1.0, 1.0)))); Values init; init.insert(X(0), Pose2()); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 247c6a6b0..390257f02 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -266,10 +266,10 @@ public: TEST(NonlinearFactor, NoiseModelFactor4) { TestFactor4 tf; Values tv; - tv.insert(X(1), LieVector(1, 1.0)); - tv.insert(X(2), LieVector(1, 2.0)); - tv.insert(X(3), LieVector(1, 3.0)); - tv.insert(X(4), LieVector(1, 4.0)); + tv.insert(X(1), LieVector((Vector(1) << 1.0))); + tv.insert(X(2), LieVector((Vector(1) << 2.0))); + tv.insert(X(3), LieVector((Vector(1) << 3.0))); + tv.insert(X(4), LieVector((Vector(1) << 4.0))); EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); @@ -312,11 +312,11 @@ public: TEST(NonlinearFactor, NoiseModelFactor5) { TestFactor5 tf; Values tv; - tv.insert(X(1), LieVector(1, 1.0)); - tv.insert(X(2), LieVector(1, 2.0)); - tv.insert(X(3), LieVector(1, 3.0)); - tv.insert(X(4), LieVector(1, 4.0)); - tv.insert(X(5), LieVector(1, 5.0)); + tv.insert(X(1), LieVector((Vector(1) << 1.0))); + tv.insert(X(2), LieVector((Vector(1) << 2.0))); + tv.insert(X(3), LieVector((Vector(1) << 3.0))); + tv.insert(X(4), LieVector((Vector(1) << 4.0))); + tv.insert(X(5), LieVector((Vector(1) << 5.0))); EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); @@ -364,12 +364,12 @@ public: TEST(NonlinearFactor, NoiseModelFactor6) { TestFactor6 tf; Values tv; - tv.insert(X(1), LieVector(1, 1.0)); - tv.insert(X(2), LieVector(1, 2.0)); - tv.insert(X(3), LieVector(1, 3.0)); - tv.insert(X(4), LieVector(1, 4.0)); - tv.insert(X(5), LieVector(1, 5.0)); - tv.insert(X(6), LieVector(1, 6.0)); + tv.insert(X(1), LieVector((Vector(1) << 1.0))); + tv.insert(X(2), LieVector((Vector(1) << 2.0))); + tv.insert(X(3), LieVector((Vector(1) << 3.0))); + tv.insert(X(4), LieVector((Vector(1) << 4.0))); + tv.insert(X(5), LieVector((Vector(1) << 5.0))); + tv.insert(X(6), LieVector((Vector(1) << 6.0))); EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index ecf82d1bd..c9f434da2 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -287,8 +287,8 @@ TEST (testSerializationSLAM, smallExample_nonlinear) { /* ************************************************************************* */ TEST (testSerializationSLAM, factors) { - LieVector lieVector(4, 1.0, 2.0, 3.0, 4.0); - LieMatrix lieMatrix(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0); + LieVector lieVector((Vector(4) << 1.0, 2.0, 3.0, 4.0)); + LieMatrix lieMatrix((Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0)); Point2 point2(1.0, 2.0); StereoPoint2 stereoPoint2(1.0, 2.0, 3.0); Point3 point3(1.0, 2.0, 3.0); diff --git a/tests/testSimulated2DOriented.cpp b/tests/testSimulated2DOriented.cpp index cf505c485..dcc31e0ec 100644 --- a/tests/testSimulated2DOriented.cpp +++ b/tests/testSimulated2DOriented.cpp @@ -58,7 +58,7 @@ TEST( simulated2DOriented, Dprior ) TEST( simulated2DOriented, constructor ) { Pose2 measurement(0.2, 0.3, 0.1); - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1., 1., 1.)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1., 1., 1.)); simulated2DOriented::Odometry factor(measurement, model, X(1), X(2)); simulated2DOriented::Values config;