diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index d8eca0fcf..3fb5cf185 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -3,17 +3,24 @@ install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLA install(FILES UFconfig/UFconfig.h DESTINATION include/gtsam/3rdparty/UFconfig) if(NOT GTSAM_USE_SYSTEM_EIGEN) + # Find plain .h files + file(GLOB_RECURSE eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/Eigen/*.h") + + # Find header files without extension + file(GLOB eigen_dir_headers_all "Eigen/Eigen/*") + foreach(eigen_dir ${eigen_dir_headers_all}) + get_filename_component(filename ${eigen_dir} NAME) + if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) + list(APPEND eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/Eigen/${filename}") + install(FILES Eigen/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/Eigen) + endif() + endforeach(eigen_dir) + + # Add to project source + set(eigen_headers ${eigen_headers} PARENT_SCOPE) + # install Eigen - only the headers in our 3rdparty directory install(DIRECTORY Eigen/Eigen DESTINATION include/gtsam/3rdparty/Eigen FILES_MATCHING PATTERN "*.h") - file(GLOB eigen_dir_headers_all "Eigen/Eigen/*") - - # ensure that Eigen folders without extensions get added - foreach(eigen_dir ${eigen_dir_headers_all}) - get_filename_component(filename ${eigen_dir} NAME) - if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) - install(FILES Eigen/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/Eigen) - endif() - endforeach(eigen_dir) endif() diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h index 64daa445c..b9bcb5240 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h @@ -33,7 +33,7 @@ #ifndef EIGEN_LLT_MKL_H #define EIGEN_LLT_MKL_H -#include "Eigen/src/Core/util/MKL_support.h" +#include "../Core/util/MKL_support.h" #include namespace Eigen { @@ -60,7 +60,7 @@ template<> struct mkl_llt \ lda = m.outerStride(); \ \ info = LAPACKE_##MKLPREFIX##potrf( matrix_order, uplo, size, (MKLTYPE*)a, lda ); \ - info = (info==0) ? Success : NumericalIssue; \ + info = (info==0) ? -1 : 1; \ return info; \ } \ }; \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h index a96867af4..57ffb6b9d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h @@ -25,10 +25,14 @@ namespace Eigen { * \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished() */ template -struct CommaInitializer +struct CommaInitializer : + public internal::dense_xpr_base >::type { - typedef typename XprType::Scalar Scalar; - typedef typename XprType::Index Index; + typedef typename internal::dense_xpr_base >::type Base; + EIGEN_DENSE_PUBLIC_INTERFACE(CommaInitializer) + typedef typename internal::conditional::ret, + XprType, const XprType&>::type ExpressionTypeNested; + typedef typename XprType::InnerIterator InnerIterator; inline CommaInitializer(XprType& xpr, const Scalar& s) : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1) @@ -104,11 +108,81 @@ struct CommaInitializer */ inline XprType& finished() { return m_xpr; } + // The following implement the DenseBase interface + + inline Index rows() const { return m_xpr.rows(); } + inline Index cols() const { return m_xpr.cols(); } + inline Index outerStride() const { return m_xpr.outerStride(); } + inline Index innerStride() const { return m_xpr.innerStride(); } + + inline CoeffReturnType coeff(Index row, Index col) const + { + return m_xpr.coeff(row, col); + } + + inline CoeffReturnType coeff(Index index) const + { + return m_xpr.coeff(index); + } + + inline const Scalar& coeffRef(Index row, Index col) const + { + return m_xpr.const_cast_derived().coeffRef(row, col); + } + + inline const Scalar& coeffRef(Index index) const + { + return m_xpr.const_cast_derived().coeffRef(index); + } + + inline Scalar& coeffRef(Index row, Index col) + { + return m_xpr.const_cast_derived().coeffRef(row, col); + } + + inline Scalar& coeffRef(Index index) + { + return m_xpr.const_cast_derived().coeffRef(index); + } + + template + inline const PacketScalar packet(Index row, Index col) const + { + return m_xpr.template packet(row, col); + } + + template + inline void writePacket(Index row, Index col, const PacketScalar& x) + { + m_xpr.const_cast_derived().template writePacket(row, col, x); + } + + template + inline const PacketScalar packet(Index index) const + { + return m_xpr.template packet(index); + } + + template + inline void writePacket(Index index, const PacketScalar& x) + { + m_xpr.const_cast_derived().template writePacket(index, x); + } + + const XprType& _expression() const { return m_xpr; } + XprType& m_xpr; // target expression Index m_row; // current row id Index m_col; // current col id Index m_currentBlockRows; // current block height }; + +namespace internal { + template + struct traits > : traits + { + }; +} /** \anchor MatrixBaseCommaInitRef * Convenient operator to set the coefficients of a matrix. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur_MKL.h index 91496ae5b..4af8be30f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur_MKL.h @@ -33,7 +33,7 @@ #ifndef EIGEN_COMPLEX_SCHUR_MKL_H #define EIGEN_COMPLEX_SCHUR_MKL_H -#include "Eigen/src/Core/util/MKL_support.h" +#include "../Core/util/MKL_support.h" namespace Eigen { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur_MKL.h index ad9736460..26f72775c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur_MKL.h @@ -33,7 +33,7 @@ #ifndef EIGEN_REAL_SCHUR_MKL_H #define EIGEN_REAL_SCHUR_MKL_H -#include "Eigen/src/Core/util/MKL_support.h" +#include "../Core/util/MKL_support.h" namespace Eigen { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_MKL.h index 17c0dadd2..041c8b7c2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_MKL.h @@ -33,7 +33,7 @@ #ifndef EIGEN_SAEIGENSOLVER_MKL_H #define EIGEN_SAEIGENSOLVER_MKL_H -#include "Eigen/src/Core/util/MKL_support.h" +#include "../Core/util/MKL_support.h" namespace Eigen { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU_MKL.h index 9035953c8..d391ff780 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU_MKL.h @@ -33,7 +33,7 @@ #ifndef EIGEN_PARTIALLU_LAPACK_H #define EIGEN_PARTIALLU_LAPACK_H -#include "Eigen/src/Core/util/MKL_support.h" +#include "../Core/util/MKL_support.h" namespace Eigen { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h index b5b198326..e66196b1d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h @@ -34,7 +34,7 @@ #ifndef EIGEN_COLPIVOTINGHOUSEHOLDERQR_MKL_H #define EIGEN_COLPIVOTINGHOUSEHOLDERQR_MKL_H -#include "Eigen/src/Core/util/MKL_support.h" +#include "../Core/util/MKL_support.h" namespace Eigen { @@ -63,12 +63,12 @@ ColPivHouseholderQR -void householder_qr_inplace_blocked(MatrixQR& mat, HCoeffs& hCoeffs, - typename MatrixQR::Index maxBlockSize=32, - typename MatrixQR::Scalar* tempData = 0) +template +struct householder_qr_inplace_blocked { - typedef typename MatrixQR::Index Index; - typedef typename MatrixQR::Scalar Scalar; - typedef Block BlockType; - - Index rows = mat.rows(); - Index cols = mat.cols(); - Index size = (std::min)(rows, cols); - - typedef Matrix TempType; - TempType tempVector; - if(tempData==0) + // This is specialized for MKL-supported Scalar types in HouseholderQR_MKL.h + static void run(MatrixQR& mat, HCoeffs& hCoeffs, + typename MatrixQR::Index maxBlockSize=32, + typename MatrixQR::Scalar* tempData = 0) { - tempVector.resize(cols); - tempData = tempVector.data(); - } + typedef typename MatrixQR::Index Index; + typedef typename MatrixQR::Scalar Scalar; + typedef Block BlockType; - Index blockSize = (std::min)(maxBlockSize,size); + Index rows = mat.rows(); + Index cols = mat.cols(); + Index size = (std::min)(rows, cols); - Index k = 0; - for (k = 0; k < size; k += blockSize) - { - Index bs = (std::min)(size-k,blockSize); // actual size of the block - Index tcols = cols - k - bs; // trailing columns - Index brows = rows-k; // rows of the block - - // partition the matrix: - // A00 | A01 | A02 - // mat = A10 | A11 | A12 - // A20 | A21 | A22 - // and performs the qr dec of [A11^T A12^T]^T - // and update [A21^T A22^T]^T using level 3 operations. - // Finally, the algorithm continue on A22 - - BlockType A11_21 = mat.block(k,k,brows,bs); - Block hCoeffsSegment = hCoeffs.segment(k,bs); - - householder_qr_inplace_unblocked(A11_21, hCoeffsSegment, tempData); - - if(tcols) + typedef Matrix TempType; + TempType tempVector; + if(tempData==0) { - BlockType A21_22 = mat.block(k,k+bs,brows,tcols); - apply_block_householder_on_the_left(A21_22,A11_21,hCoeffsSegment.adjoint()); + tempVector.resize(cols); + tempData = tempVector.data(); + } + + Index blockSize = (std::min)(maxBlockSize,size); + + Index k = 0; + for (k = 0; k < size; k += blockSize) + { + Index bs = (std::min)(size-k,blockSize); // actual size of the block + Index tcols = cols - k - bs; // trailing columns + Index brows = rows-k; // rows of the block + + // partition the matrix: + // A00 | A01 | A02 + // mat = A10 | A11 | A12 + // A20 | A21 | A22 + // and performs the qr dec of [A11^T A12^T]^T + // and update [A21^T A22^T]^T using level 3 operations. + // Finally, the algorithm continue on A22 + + BlockType A11_21 = mat.block(k,k,brows,bs); + Block hCoeffsSegment = hCoeffs.segment(k,bs); + + householder_qr_inplace_unblocked(A11_21, hCoeffsSegment, tempData); + + if(tcols) + { + BlockType A21_22 = mat.block(k,k+bs,brows,tcols); + apply_block_householder_on_the_left(A21_22,A11_21,hCoeffsSegment.adjoint()); + } } } -} +}; template struct solve_retval, Rhs> @@ -352,7 +358,7 @@ HouseholderQR& HouseholderQR::compute(const MatrixType& m_temp.resize(cols); - internal::householder_qr_inplace_blocked(m_qr, m_hCoeffs, 48, m_temp.data()); + internal::householder_qr_inplace_blocked::run(m_qr, m_hCoeffs, 48, m_temp.data()); m_isInitialized = true; return *this; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR_MKL.h index 5313de604..b80f1b48d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR_MKL.h @@ -34,28 +34,30 @@ #ifndef EIGEN_QR_MKL_H #define EIGEN_QR_MKL_H -#include "Eigen/src/Core/util/MKL_support.h" +#include "../Core/util/MKL_support.h" namespace Eigen { -namespace internal { + namespace internal { -/** \internal Specialization for the data types supported by MKL */ + /** \internal Specialization for the data types supported by MKL */ #define EIGEN_MKL_QR_NOPIV(EIGTYPE, MKLTYPE, MKLPREFIX) \ template \ -void householder_qr_inplace_blocked(MatrixQR& mat, HCoeffs& hCoeffs, \ - typename MatrixQR::Index maxBlockSize=32, \ - EIGTYPE* tempData = 0) \ +struct householder_qr_inplace_blocked \ { \ - lapack_int m = mat.rows(); \ - lapack_int n = mat.cols(); \ - lapack_int lda = mat.outerStride(); \ - lapack_int matrix_order = (MatrixQR::IsRowMajor) ? LAPACK_ROW_MAJOR : LAPACK_COL_MAJOR; \ - LAPACKE_##MKLPREFIX##geqrf( matrix_order, m, n, (MKLTYPE*)mat.data(), lda, (MKLTYPE*)hCoeffs.data()); \ - hCoeffs.adjointInPlace(); \ -\ -} + static void run(MatrixQR& mat, HCoeffs& hCoeffs, \ + typename MatrixQR::Index = 32, \ + typename MatrixQR::Scalar* = 0) \ + { \ + lapack_int m = (lapack_int) mat.rows(); \ + lapack_int n = (lapack_int) mat.cols(); \ + lapack_int lda = (lapack_int) mat.outerStride(); \ + lapack_int matrix_order = (MatrixQR::IsRowMajor) ? LAPACK_ROW_MAJOR : LAPACK_COL_MAJOR; \ + LAPACKE_##MKLPREFIX##geqrf( matrix_order, m, n, (MKLTYPE*)mat.data(), lda, (MKLTYPE*)hCoeffs.data()); \ + hCoeffs.adjointInPlace(); \ + } \ +}; EIGEN_MKL_QR_NOPIV(double, double, d) EIGEN_MKL_QR_NOPIV(float, float, s) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h index decda7540..f76a7082a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h @@ -33,7 +33,7 @@ #ifndef EIGEN_JACOBISVD_MKL_H #define EIGEN_JACOBISVD_MKL_H -#include "Eigen/src/Core/util/MKL_support.h" +#include "../Core/util/MKL_support.h" namespace Eigen { diff --git a/gtsam/3rdparty/gtsam_eigen_includes.h.in b/gtsam/3rdparty/gtsam_eigen_includes.h.in index bd41228b5..ecc38d5c4 100644 --- a/gtsam/3rdparty/gtsam_eigen_includes.h.in +++ b/gtsam/3rdparty/gtsam_eigen_includes.h.in @@ -17,6 +17,7 @@ #pragma once +#cmakedefine EIGEN_USE_MKL_ALL // This is also defined in config.h #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Dense> #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/QR> #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/LU> diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 187280bbf..f0b82cf9c 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -583,15 +583,16 @@ Matrix collect(size_t nrMatrices, ...) /* ************************************************************************* */ // row scaling, in-place void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) { - const size_t m = A.rows(); + const int m = A.rows(); if (inf_mask) { - for (size_t i=0; i::run(A, hCoeffs, 48, temp.data()); zeroBelowDiagonal(A); } diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 7a71eb125..9bd0926ae 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -165,7 +165,7 @@ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) { if (vec1.size()!=vec2.size()) return false; size_t m = vec1.size(); for(size_t i=0; i tol) return false; @@ -178,7 +178,7 @@ bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol if (vec1.size()!=vec2.size()) return false; size_t m = vec1.size(); for(size_t i=0; i tol) return false; diff --git a/gtsam/geometry/CMakeLists.txt b/gtsam/geometry/CMakeLists.txt index e5acb3c21..b659d8314 100644 --- a/gtsam/geometry/CMakeLists.txt +++ b/gtsam/geometry/CMakeLists.txt @@ -2,12 +2,6 @@ file(GLOB geometry_headers "*.h") install(FILES ${geometry_headers} DESTINATION include/gtsam/geometry) -# Components to link tests in this subfolder against -set(geometry_local_libs - base - geometry -) - # Files to exclude from compilation of tests and timing scripts set(geometry_excluded_files # "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test @@ -16,11 +10,11 @@ set(geometry_excluded_files # Build tests if (GTSAM_BUILD_TESTS) - gtsam_add_subdir_tests(geometry "${geometry_local_libs}" "${gtsam-default}" "${geometry_excluded_files}") + gtsam_add_subdir_tests(geometry "${gtsam-default}" "${gtsam-default}" "${geometry_excluded_files}") endif(GTSAM_BUILD_TESTS) # Build timing scripts if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(geometry "${geometry_local_libs}" "${gtsam-default}" "${geometry_excluded_files}") + gtsam_add_subdir_timing(geometry "${gtsam-default}" "${gtsam-default}" "${geometry_excluded_files}") endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 26e7e3260..8b7771410 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -24,131 +24,135 @@ namespace gtsam { /* ************************************************************************* */ -Cal3Bundler::Cal3Bundler() : f_(1), k1_(0), k2_(0) {} - -/* ************************************************************************* */ -Cal3Bundler::Cal3Bundler(const Vector &v) : f_(v(0)), k1_(v(1)), k2_(v(2)) {} - -/* ************************************************************************* */ -Cal3Bundler::Cal3Bundler(const double f, const double k1, const double k2) : f_(f), k1_(k1), k2_(k2) {} - -/* ************************************************************************* */ -Matrix Cal3Bundler::K() const { return Matrix_(3,3, f_, 0.0, 0.0, 0.0, f_, 0.0, 0.0, 0.0, 1.0); } - -/* ************************************************************************* */ -Vector Cal3Bundler::k() const { return Vector_(4, k1_, k2_, 0, 0) ; } - -/* ************************************************************************* */ -Vector Cal3Bundler::vector() const { return Vector_(3, f_, k1_, k2_) ; } - -/* ************************************************************************* */ -void Cal3Bundler::print(const std::string& s) const { gtsam::print(vector(), s + ".K") ; } - -/* ************************************************************************* */ -bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { - if (fabs(f_ - K.f_) > tol || fabs(k1_ - K.k1_) > tol || fabs(k2_ - K.k2_) > tol) - return false; - return true ; +Cal3Bundler::Cal3Bundler() : + f_(1), k1_(0), k2_(0), u0_(0), v0_(0) { } /* ************************************************************************* */ -Point2 Cal3Bundler::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { -// r = x^2 + y^2 ; -// g = (1 + k(1)*r + k(2)*r^2) ; -// pi(:,i) = g * pn(:,i) - const double x = p.x(), y = p.y() ; - const double r = x*x + y*y ; - const double r2 = r*r ; - const double g = 1 + k1_ * r + k2_*r2 ; // save one multiply - const double fg = f_*g ; +Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) : + f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) { +} - // semantic meaningful version - //if (H1) *H1 = D2d_calibration(p) ; - //if (H2) *H2 = D2d_intrinsic(p) ; +/* ************************************************************************* */ +Matrix Cal3Bundler::K() const { + Matrix3 K; + K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1; + return K; +} - // unrolled version, much faster - if ( H1 || H2 ) { +/* ************************************************************************* */ +Vector Cal3Bundler::k() const { + return (Vector(4) << k1_, k2_, 0, 0); +} - const double fx = f_*x, fy = f_*y ; - if ( H1 ) { - *H1 = Matrix_(2, 3, - g*x, fx*r , fx*r2, - g*y, fy*r , fy*r2) ; - } +/* ************************************************************************* */ +Vector Cal3Bundler::vector() const { + return (Vector(3) << f_, k1_, k2_); +} - if ( H2 ) { - const double dr_dx = 2*x ; - const double dr_dy = 2*y ; - const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ; - const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ; - *H2 = Matrix_(2, 2, - fg + fx*dg_dx, fx*dg_dy, - fy*dg_dx , fg + fy*dg_dy) ; - } +/* ************************************************************************* */ +void Cal3Bundler::print(const std::string& s) const { + gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_), s + ".K"); +} + +/* ************************************************************************* */ +bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { + if (fabs(f_ - K.f_) > tol || fabs(k1_ - K.k1_) > tol + || fabs(k2_ - K.k2_) > tol || fabs(u0_ - K.u0_) > tol + || fabs(v0_ - K.v0_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +Point2 Cal3Bundler::uncalibrate(const Point2& p, // + boost::optional Dcal, boost::optional Dp) const { + // r = x^2 + y^2; + // g = (1 + k(1)*r + k(2)*r^2); + // pi(:,i) = g * pn(:,i) + const double x = p.x(), y = p.y(); + const double r = x * x + y * y; + const double g = 1. + (k1_ + k2_ * r) * r; + const double u = g * x, v = g * y; + + // Derivatives make use of intermediate variables above + if (Dcal) { + double rx = r * x, ry = r * y; + Eigen::Matrix D; + D << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry; + *Dcal = D; } - return Point2(fg * x, fg * y) ; + if (Dp) { + const double a = 2. * (k1_ + 2. * k2_ * r); + const double axx = a * x * x, axy = a * x * y, ayy = a * y * y; + Eigen::Matrix D; + D << g + axx, axy, axy, g + ayy; + *Dp = f_ * D; + } + + return Point2(u0_ + f_ * u, v0_ + f_ * v); +} + +/* ************************************************************************* */ +Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { + // Copied from Cal3DS2 :-( + // but specialized with k1,k2 non-zero only and fx=fy and s=0 + const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_); + + // initialize by ignoring the distortion at all, might be problematic for pixels around boundary + Point2 pn = invKPi; + + // iterate until the uncalibrate is close to the actual pixel coordinate + const int maxIterations = 10; + int iteration; + for (iteration = 0; iteration < maxIterations; ++iteration) { + if (uncalibrate(pn).distance(pi) <= tol) + break; + const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y; + const double rr = xx + yy; + const double g = (1 + k1_ * rr + k2_ * rr * rr); + pn = invKPi / g; + } + + if (iteration >= maxIterations) + throw std::runtime_error( + "Cal3DS2::calibrate fails to converge. need a better initialization"); + + return pn; } /* ************************************************************************* */ Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const { - const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; - const double r = xx + yy ; - const double dr_dx = 2*x ; - const double dr_dy = 2*y ; - const double g = 1 + (k1_+k2_*r) * r ; // save one multiply - //const double g = 1 + k1_*r + k2_*r*r ; - const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ; - const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ; - - Matrix DK = Matrix_(2, 2, f_, 0.0, 0.0, f_); - Matrix DR = Matrix_(2, 2, - g + x*dg_dx, x*dg_dy, - y*dg_dx , g + y*dg_dy) ; - return DK * DR; + Matrix Dp; + uncalibrate(p, boost::none, Dp); + return Dp; } /* ************************************************************************* */ Matrix Cal3Bundler::D2d_calibration(const Point2& p) const { - - const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ; - const double r = xx + yy ; - const double r2 = r*r ; - const double f = f_ ; - const double g = 1 + (k1_+k2_*r) * r ; // save one multiply - //const double g = (1+k1_*r+k2_*r2) ; - - return Matrix_(2, 3, - g*x, f*x*r , f*x*r2, - g*y, f*y*r , f*y*r2); + Matrix Dcal; + uncalibrate(p, Dcal, boost::none); + return Dcal; } /* ************************************************************************* */ Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { - - const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; - const double r = xx + yy ; - const double r2 = r*r; - const double dr_dx = 2*x ; - const double dr_dy = 2*y ; - const double g = 1 + (k1_*r) + (k2_*r2) ; - const double f = f_ ; - const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ; - const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ; - - Matrix H = Matrix_(2,5, - f*(g + x*dg_dx), f*(x*dg_dy), g*x, f*x*r , f*x*r2, - f*(y*dg_dx) , f*(g + y*dg_dy), g*y, f*y*r , f*y*r2); - - return H ; + Matrix Dcal, Dp; + uncalibrate(p, Dcal, Dp); + Matrix H(2, 5); + H << Dp, Dcal; + return H; } /* ************************************************************************* */ -Cal3Bundler Cal3Bundler::retract(const Vector& d) const { return Cal3Bundler(vector() + d) ; } +Cal3Bundler Cal3Bundler::retract(const Vector& d) const { + return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_); +} /* ************************************************************************* */ -Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { return vector() - T2.vector(); } +Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { + return vector() - T2.vector(); +} } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index a39d7a4e5..fff9a6e6d 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -13,7 +13,7 @@ * @file Cal3Bundler.h * @brief Calibration used by Bundler * @date Sep 25, 2010 - * @author ydjian + * @author Yong Dian Jian */ #pragma once @@ -28,33 +28,30 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Bundler : public DerivedValue { +class GTSAM_EXPORT Cal3Bundler: public DerivedValue { private: - double f_, k1_, k2_ ; + double f_; ///< focal length + double k1_, k2_; ///< radial distortion + double u0_, v0_; ///< image center, not a parameter to be optimized but a constant public: - Matrix K() const ; - Vector k() const ; - - Vector vector() const; - /// @name Standard Constructors /// @{ - ///TODO: comment - Cal3Bundler() ; + /// Default constructor + Cal3Bundler(); - ///TODO: comment - Cal3Bundler(const double f, const double k1, const double k2) ; - - /// @} - /// @name Advanced Constructors - /// @{ - - ///TODO: comment - Cal3Bundler(const Vector &v) ; + /** + * Constructor + * @param f focal length + * @param k1 first radial distortion coefficient (quadratic) + * @param k2 second radial distortion coefficient (quartic) + * @param u0 optional image center (default 0), considered a constant + * @param v0 optional image center (default 0), considered a constant + */ + Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0); /// @} /// @name Testable @@ -70,35 +67,83 @@ public: /// @name Standard Interface /// @{ - ///TODO: comment - Point2 uncalibrate(const Point2& p, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const ; + Matrix K() const; ///< Standard 3*3 calibration matrix + Vector k() const; ///< Radial distortion parameters (4 of them, 2 0) - ///TODO: comment - Matrix D2d_intrinsic(const Point2& p) const ; + Vector vector() const; - ///TODO: comment - Matrix D2d_calibration(const Point2& p) const ; + /// focal length x + inline double fx() const { + return f_; + } - ///TODO: comment - Matrix D2d_intrinsic_calibration(const Point2& p) const ; + /// focal length y + inline double fy() const { + return f_; + } + + /// distorsion parameter k1 + inline double k1() const { + return k1_; + } + + /// distorsion parameter k2 + inline double k2() const { + return k2_; + } + + /// get parameter u0 + inline double u0() const { + return u0_; + } + + /// get parameter v0 + inline double v0() const { + return v0_; + } + + + /** + * convert intrinsic coordinates xy to image coordinates uv + * @param p point in intrinsic coordinates + * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p, boost::optional Dcal = + boost::none, boost::optional Dp = boost::none) const; + + /// Conver a pixel coordinate to ideal coordinate + Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; + + /// @deprecated might be removed in next release, use uncalibrate + Matrix D2d_intrinsic(const Point2& p) const; + + /// @deprecated might be removed in next release, use uncalibrate + Matrix D2d_calibration(const Point2& p) const; + + /// @deprecated might be removed in next release, use uncalibrate + Matrix D2d_intrinsic_calibration(const Point2& p) const; /// @} /// @name Manifold /// @{ - ///TODO: comment - Cal3Bundler retract(const Vector& d) const ; + /// Update calibration with tangent space delta + Cal3Bundler retract(const Vector& d) const; - ///TODO: comment - Vector localCoordinates(const Cal3Bundler& T2) const ; + /// Calculate local coordinates to another calibration + Vector localCoordinates(const Cal3Bundler& T2) const; - ///TODO: comment - virtual size_t dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + /// dimensionality + virtual size_t dim() const { + return 3; + } - ///TODO: comment - static size_t Dim() { return 3; } //TODO: make a final dimension variable + /// dimensionality + static size_t Dim() { + return 3; + } private: @@ -109,18 +154,19 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) - { - ar & boost::serialization::make_nvp("Cal3Bundler", - boost::serialization::base_object(*this)); + void serialize(Archive & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("Cal3Bundler", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(f_); ar & BOOST_SERIALIZATION_NVP(k1_); ar & BOOST_SERIALIZATION_NVP(k2_); + ar & BOOST_SERIALIZATION_NVP(u0_); + ar & BOOST_SERIALIZATION_NVP(v0_); } - /// @} -}; + }; -} // namespace gtsam + } // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 1d5ad695a..0d263b625 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -28,44 +28,73 @@ Cal3DS2::Cal3DS2(const Vector &v): fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){} /* ************************************************************************* */ -Matrix Cal3DS2::K() const { return Matrix_(3,3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); } +Matrix Cal3DS2::K() const { return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); } /* ************************************************************************* */ -Vector Cal3DS2::vector() const { return Vector_(9, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_) ; } +Vector Cal3DS2::vector() const { return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); } /* ************************************************************************* */ -void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K") ; gtsam::print(Vector(k()), s + ".k") ; } +void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K"); gtsam::print(Vector(k()), s + ".k"); } /* ************************************************************************* */ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol) - return false ; - return true ; + return false; + return true; } /* ************************************************************************* */ Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, boost::optional H2) const { -// rr = x^2 + y^2 ; -// g = (1 + k(1)*rr + k(2)*rr^2) ; -// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2) ; 2*k(4)*x*y + k(3)*(rr + 2*y^2)] ; -// pi(:,i) = g * pn(:,i) + dp ; - const double x = p.x(), y = p.y(), xy = x*y, xx = x*x, yy = y*y ; - const double rr = xx + yy ; - const double g = (1+k1_*rr+k2_*rr*rr) ; - const double dx = 2*k3_*xy + k4_*(rr+2*xx) ; - const double dy = 2*k4_*xy + k3_*(rr+2*yy) ; - const double x2 = g*x + dx ; - const double y2 = g*y + dy ; + // parameters + const double fx = fx_, fy = fy_, s = s_; + const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; - if (H1) *H1 = D2d_calibration(p) ; - if (H2) *H2 = D2d_intrinsic(p) ; + // rr = x^2 + y^2; + // g = (1 + k(1)*rr + k(2)*rr^2); + // dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)]; + // pi(:,i) = g * pn(:,i) + dp; + const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y; + const double rr = xx + yy; + const double r4 = rr * rr; + const double g = 1. + k1 * rr + k2 * r4; + const double dx = 2. * k3 * xy + k4 * (rr + 2. * xx); + const double dy = 2. * k4 * xy + k3 * (rr + 2. * yy); - return Point2(fx_* x2 + s_ * y2 + u0_, fy_ * y2 + v0_) ; + const double pnx = g*x + dx; + const double pny = g*y + dy; + + // Inlined derivative for calibration + if (H1) { + *H1 = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr, + fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy), + fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0, + fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy)); + } + // Inlined derivative for points + if (H2) { + const double dr_dx = 2. * x; + const double dr_dy = 2. * y; + const double dg_dx = k1 * dr_dx + k2 * 2. * rr * dr_dx; + const double dg_dy = k1 * dr_dy + k2 * 2. * rr * dr_dy; + + const double dDx_dx = 2. * k3 * y + k4 * (dr_dx + 4. * x); + const double dDx_dy = 2. * k3 * x + k4 * dr_dy; + const double dDy_dx = 2. * k4 * y + k3 * dr_dx; + const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y); + + Matrix DK = (Matrix(2, 2) << fx, s_, 0.0, fy); + Matrix DR = (Matrix(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy, + y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy); + + *H2 = DK * DR; + } + + return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_); } /* ************************************************************************* */ @@ -82,14 +111,14 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { // iterate until the uncalibrate is close to the actual pixel coordinate const int maxIterations = 10; int iteration; - for ( iteration = 0 ; iteration < maxIterations ; ++iteration ) { + for ( iteration = 0; iteration < maxIterations; ++iteration ) { if ( uncalibrate(pn).distance(pi) <= tol ) break; - const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y ; - const double rr = xx + yy ; - const double g = (1+k1_*rr+k2_*rr*rr) ; - const double dx = 2*k3_*xy + k4_*(rr+2*xx) ; - const double dy = 2*k4_*xy + k3_*(rr+2*yy) ; - pn = (invKPi - Point2(dx,dy))/g ; + const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y; + const double rr = xx + yy; + const double g = (1+k1_*rr+k2_*rr*rr); + const double dx = 2*k3_*xy + k4_*(rr+2*xx); + const double dy = 2*k4_*xy + k3_*(rr+2*yy); + pn = (invKPi - Point2(dx,dy))/g; } if ( iteration >= maxIterations ) @@ -100,51 +129,51 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { /* ************************************************************************* */ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { - //const double fx = fx_, fy = fy_, s = s_ ; + //const double fx = fx_, fy = fy_, s = s_; const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; - //const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y ; - const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ; - const double rr = xx + yy ; - const double dr_dx = 2*x ; - const double dr_dy = 2*y ; - const double r4 = rr*rr ; - const double g = 1 + k1*rr + k2*r4 ; - const double dg_dx = k1*dr_dx + k2*2*rr*dr_dx ; - const double dg_dy = k1*dr_dy + k2*2*rr*dr_dy ; + //const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y; + const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; + const double rr = xx + yy; + const double dr_dx = 2*x; + const double dr_dy = 2*y; + const double r4 = rr*rr; + const double g = 1 + k1*rr + k2*r4; + const double dg_dx = k1*dr_dx + k2*2*rr*dr_dx; + const double dg_dy = k1*dr_dy + k2*2*rr*dr_dy; - // Dx = 2*k3*xy + k4*(rr+2*xx) ; - // Dy = 2*k4*xy + k3*(rr+2*yy) ; - const double dDx_dx = 2*k3*y + k4*(dr_dx + 4*x) ; - const double dDx_dy = 2*k3*x + k4*dr_dy ; - const double dDy_dx = 2*k4*y + k3*dr_dx ; - const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y) ; + // Dx = 2*k3*xy + k4*(rr+2*xx); + // Dy = 2*k4*xy + k3*(rr+2*yy); + const double dDx_dx = 2*k3*y + k4*(dr_dx + 4*x); + const double dDx_dy = 2*k3*x + k4*dr_dy; + const double dDy_dx = 2*k4*y + k3*dr_dx; + const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y); - Matrix DK = Matrix_(2, 2, fx_, s_, 0.0, fy_); - Matrix DR = Matrix_(2, 2, g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy, - y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy) ; + Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_); + Matrix DR = (Matrix(2, 2) << g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy, + y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy); return DK * DR; } /* ************************************************************************* */ Matrix Cal3DS2::D2d_calibration(const Point2& p) const { - const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y ; - const double rr = xx + yy ; - const double r4 = rr*rr ; - const double fx = fx_, fy = fy_, s = s_ ; - const double g = (1+k1_*rr+k2_*r4) ; - const double dx = 2*k3_*xy + k4_*(rr+2*xx) ; - const double dy = 2*k4_*xy + k3_*(rr+2*yy) ; + const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y; + const double rr = xx + yy; + const double r4 = rr*rr; + const double fx = fx_, fy = fy_, s = s_; + const double g = (1+k1_*rr+k2_*r4); + const double dx = 2*k3_*xy + k4_*(rr+2*xx); + const double dy = 2*k4_*xy + k3_*(rr+2*yy); const double pnx = g*x + dx; const double pny = g*y + dy; - return Matrix_(2, 9, + return (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx*x*rr + s*y*rr, fx*x*r4 + s*y*r4, fx*2*xy + s*(rr+2*yy), fx*(rr+2*xx) + s*(2*xy), 0.0, pny, 0.0, 0.0, 1.0, fy*y*rr , fy*y*r4 , fy*(rr+2*yy) , fy*(2*xy) ); } /* ************************************************************************* */ -Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d) ; } +Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d); } /* ************************************************************************* */ Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { return vector() - T2.vector(); } diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 32a4654cb..5453e1481 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -92,10 +92,16 @@ public: /// image center in y inline double py() const { return v0_;} - /// Convert ideal point p (in intrinsic coordinates) into pixel coordinates + /** + * convert intrinsic coordinates xy to image coordinates uv + * @param p point in intrinsic coordinates + * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ Point2 uncalibrate(const Point2& p, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const ; + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const ; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& p, const double tol=1e-5) const; diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 7e3703a4b..1fb7f4069 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -22,58 +22,72 @@ #include namespace gtsam { - using namespace std; +using namespace std; - /* ************************************************************************* */ - Cal3_S2::Cal3_S2(double fov, int w, int h) : +/* ************************************************************************* */ +Cal3_S2::Cal3_S2(double fov, int w, int h) : s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) { - double a = fov * M_PI / 360.0; // fov/2 in radians - fx_ = (double)w / (2.0*tan(a)); // old formula: fx_ = (double) w * tan(a); - fy_ = fx_; + double a = fov * M_PI / 360.0; // fov/2 in radians + fx_ = (double) w / (2.0 * tan(a)); // old formula: fx_ = (double) w * tan(a); + fy_ = fx_; +} + +/* ************************************************************************* */ +Cal3_S2::Cal3_S2(const std::string &path) : + fx_(320), fy_(320), s_(0), u0_(320), v0_(140) { + + char buffer[200]; + buffer[0] = 0; + sprintf(buffer, "%s/calibration_info.txt", path.c_str()); + std::ifstream infile(buffer, std::ios::in); + + if (infile) + infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_; + else { + printf("Unable to load the calibration\n"); + exit(0); } - /* ************************************************************************* */ - Cal3_S2::Cal3_S2(const std::string &path) { + infile.close(); +} - char buffer[200]; - buffer[0] = 0; - sprintf(buffer, "%s/calibration_info.txt", path.c_str()); - std::ifstream infile(buffer, std::ios::in); +/* ************************************************************************* */ +void Cal3_S2::print(const std::string& s) const { + gtsam::print(matrix(), s); +} - if (infile) - infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_; - else { - printf("Unable to load the calibration\n"); - exit(0); - } +/* ************************************************************************* */ +bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { + if (fabs(fx_ - K.fx_) > tol) + return false; + if (fabs(fy_ - K.fy_) > tol) + return false; + if (fabs(s_ - K.s_) > tol) + return false; + if (fabs(u0_ - K.u0_) > tol) + return false; + if (fabs(v0_ - K.v0_) > tol) + return false; + return true; +} - infile.close(); - } +/* ************************************************************************* */ +Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, + boost::optional Dp) const { + const double x = p.x(), y = p.y(); + if (Dcal) + *Dcal = (Matrix(2, 5) << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0); + if (Dp) + *Dp = (Matrix(2, 2) << fx_, s_, 0.000, fy_); + return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); +} - /* ************************************************************************* */ - void Cal3_S2::print(const std::string& s) const { - gtsam::print(matrix(), s); - } - - /* ************************************************************************* */ - bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { - if (fabs(fx_ - K.fx_) > tol) return false; - if (fabs(fy_ - K.fy_) > tol) return false; - if (fabs(s_ - K.s_) > tol) return false; - if (fabs(u0_ - K.u0_) > tol) return false; - if (fabs(v0_ - K.v0_) > tol) return false; - return true; - } - - /* ************************************************************************* */ - Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional H1, - boost::optional H2) const { - if (H1) *H1 = Matrix_(2, 5, p.x(), 000.0, p.y(), 1.0, 0.0, 0.000, p.y(), - 0.000, 0.0, 1.0); - if (H2) *H2 = Matrix_(2, 2, fx_, s_, 0.000, fy_); - const double x = p.x(), y = p.y(); - return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); - } +/* ************************************************************************* */ +Point2 Cal3_S2::calibrate(const Point2& p) const { + const double u = p.x(), v = p.y(); + return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)), + (1 / fy_) * (v - v0_)); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index e3ae8859d..bb3327afc 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -26,166 +26,184 @@ namespace gtsam { - /** - * @brief The most common 5DOF 3D->2D calibration - * @addtogroup geometry - * \nosubgrouping - */ - class GTSAM_EXPORT Cal3_S2 : public DerivedValue { - private: - double fx_, fy_, s_, u0_, v0_; +/** + * @brief The most common 5DOF 3D->2D calibration + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Cal3_S2: public DerivedValue { +private: + double fx_, fy_, s_, u0_, v0_; - public: +public: - typedef boost::shared_ptr shared_ptr; ///< shared pointer to calibration object + typedef boost::shared_ptr shared_ptr; ///< shared pointer to calibration object - /// @name Standard Constructors - /// @{ + /// @name Standard Constructors + /// @{ - /// Create a default calibration that leaves coordinates unchanged - Cal3_S2() : + /// Create a default calibration that leaves coordinates unchanged + Cal3_S2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0) { - } + } - /// constructor from doubles - Cal3_S2(double fx, double fy, double s, double u0, double v0) : + /// constructor from doubles + Cal3_S2(double fx, double fy, double s, double u0, double v0) : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) { - } + } - /// constructor from vector - Cal3_S2(const Vector &d): fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)){} + /// constructor from vector + Cal3_S2(const Vector &d) : + fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) { + } + /** + * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect + * @param fov field of view in degrees + * @param w image width + * @param h image height + */ + Cal3_S2(double fov, int w, int h); - /** - * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect - * @param fov field of view in degrees - * @param w image width - * @param h image height - */ - Cal3_S2(double fov, int w, int h); + /// @} + /// @name Advanced Constructors + /// @{ - /// @} - /// @name Advanced Constructors - /// @{ + /// load calibration from location (default name is calibration_info.txt) + Cal3_S2(const std::string &path); - /// load calibration from location (default name is calibration_info.txt) - Cal3_S2(const std::string &path); + /// @} + /// @name Testable + /// @{ - /// @} - /// @name Testable - /// @{ + /// print with optional string + void print(const std::string& s = "Cal3_S2") const; - /// print with optional string - void print(const std::string& s = "Cal3_S2") const; + /// Check if equal up to specified tolerance + bool equals(const Cal3_S2& K, double tol = 10e-9) const; - /// Check if equal up to specified tolerance - bool equals(const Cal3_S2& K, double tol = 10e-9) const; + /// @} + /// @name Standard Interface + /// @{ - /// @} - /// @name Standard Interface - /// @{ + /// focal length x + inline double fx() const { + return fx_; + } - /// focal length x - inline double fx() const { return fx_;} + /// focal length y + inline double fy() const { + return fy_; + } - /// focal length y - inline double fy() const { return fy_;} + /// skew + inline double skew() const { + return s_; + } - /// skew - inline double skew() const { return s_;} + /// image center in x + inline double px() const { + return u0_; + } - /// image center in x - inline double px() const { return u0_;} + /// image center in y + inline double py() const { + return v0_; + } - /// image center in y - inline double py() const { return v0_;} + /// return the principal point + Point2 principalPoint() const { + return Point2(u0_, v0_); + } - /// return the principal point - Point2 principalPoint() const { - return Point2(u0_, v0_); - } + /// vectorized form (column-wise) + Vector vector() const { + double r[] = { fx_, fy_, s_, u0_, v0_ }; + Vector v(5); + std::copy(r, r + 5, v.data()); + return v; + } - /// vectorized form (column-wise) - Vector vector() const { - double r[] = { fx_, fy_, s_, u0_, v0_ }; - Vector v(5); - std::copy(r, r + 5, v.data()); - return v; - } + /// return calibration matrix K + Matrix K() const { + return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); + } - /// return calibration matrix K - Matrix matrix() const { - return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); - } + /** @deprecated The following function has been deprecated, use K above */ + Matrix matrix() const { + return K(); + } - /// return inverted calibration matrix inv(K) - Matrix matrix_inverse() const { - const double fxy = fx_*fy_, sv0 = s_*v0_, fyu0 = fy_*u0_; - return Matrix_(3, 3, - 1.0/fx_, -s_/fxy, (sv0-fyu0)/fxy, - 0.0, 1.0/fy_, -v0_/fy_, - 0.0, 0.0, 1.0); - } + /// return inverted calibration matrix inv(K) + Matrix matrix_inverse() const { + const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_; + return (Matrix(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, + 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0); + } - /** - * convert intrinsic coordinates xy to image coordinates uv - * with optional derivatives - */ - Point2 uncalibrate(const Point2& p, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const; + /** + * convert intrinsic coordinates xy to image coordinates uv + * @param p point in intrinsic coordinates + * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p, boost::optional Dcal = + boost::none, boost::optional Dp = boost::none) const; - /// convert image coordinates uv to intrinsic coordinates xy - Point2 calibrate(const Point2& p) const { - const double u = p.x(), v = p.y(); - return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)), - (1 / fy_) * (v - v0_)); - } + /** + * convert image coordinates uv to intrinsic coordinates xy + * @param p point in image coordinates + * @return point in intrinsic coordinates + */ + Point2 calibrate(const Point2& p) const; - /// @} - /// @name Manifold - /// @{ + /// @} + /// @name Manifold + /// @{ - /// return DOF, dimensionality of tangent space - inline size_t dim() const { - return 5; - } + /// return DOF, dimensionality of tangent space + inline size_t dim() const { + return 5; + } - /// return DOF, dimensionality of tangent space - static size_t Dim() { - return 5; - } + /// return DOF, dimensionality of tangent space + static size_t Dim() { + return 5; + } - /// Given 5-dim tangent vector, create new calibration - inline Cal3_S2 retract(const Vector& d) const { - return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4)); - } + /// Given 5-dim tangent vector, create new calibration + inline Cal3_S2 retract(const Vector& d) const { + return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4)); + } - /// Unretraction for the calibration - Vector localCoordinates(const Cal3_S2& T2) const { - return vector() - T2.vector(); - } + /// Unretraction for the calibration + Vector localCoordinates(const Cal3_S2& T2) const { + return vector() - T2.vector(); + } - /// @} - /// @name Advanced Interface - /// @{ + /// @} + /// @name Advanced Interface + /// @{ - private: +private: - /// Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3_S2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(fx_); - ar & BOOST_SERIALIZATION_NVP(fy_); - ar & BOOST_SERIALIZATION_NVP(s_); - ar & BOOST_SERIALIZATION_NVP(u0_); - ar & BOOST_SERIALIZATION_NVP(v0_); - } + /// Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("Cal3_S2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(fx_); + ar & BOOST_SERIALIZATION_NVP(fy_); + ar & BOOST_SERIALIZATION_NVP(s_); + ar & BOOST_SERIALIZATION_NVP(u0_); + ar & BOOST_SERIALIZATION_NVP(v0_); + } - /// @} + /// @} - }; +}; } // \ namespace gtsam diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 66412b3fc..35079827b 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -23,26 +23,27 @@ namespace gtsam { /* ************************************************************************* */ CalibratedCamera::CalibratedCamera(const Pose3& pose) : - pose_(pose) { + pose_(pose) { } /* ************************************************************************* */ -CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {} +CalibratedCamera::CalibratedCamera(const Vector &v) : + pose_(Pose3::Expmap(v)) { +} /* ************************************************************************* */ Point2 CalibratedCamera::project_to_camera(const Point3& P, boost::optional H1) { if (H1) { double d = 1.0 / P.z(), d2 = d * d; - *H1 = Matrix_(2, 3, - d, 0.0, -P.x() * d2, - 0.0, d, -P.y() * d2); + *H1 = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); } return Point2(P.x() / P.z(), P.y() / P.z()); } /* ************************************************************************* */ -Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) { +Point3 CalibratedCamera::backproject_from_camera(const Point2& p, + const double scale) { return Point3(p.x() * scale, p.y() * scale, scale); } @@ -58,41 +59,39 @@ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { /* ************************************************************************* */ Point2 CalibratedCamera::project(const Point3& point, - boost::optional H1, - boost::optional H2) const { + boost::optional Dpose, boost::optional Dpoint) const { #ifdef CALIBRATEDCAMERA_CHAIN_RULE - Point3 q = pose_.transform_to(point, H1, H2); + Point3 q = pose_.transform_to(point, Dpose, Dpoint); #else Point3 q = pose_.transform_to(point); #endif Point2 intrinsic = project_to_camera(q); // Check if point is in front of camera - if(q.z() <= 0) + if (q.z() <= 0) throw CheiralityException(); - if (H1 || H2) { + if (Dpose || Dpoint) { #ifdef CALIBRATEDCAMERA_CHAIN_RULE // just implement chain rule Matrix H; project_to_camera(q,H); - if (H1) *H1 = H * (*H1); - if (H2) *H2 = H * (*H2); + if (Dpose) *Dpose = H * (*Dpose); + if (Dpoint) *Dpoint = H * (*Dpoint); #else // optimized version, see CalibratedCamera.nb - const double z = q.z(), d = 1.0/z; - const double u = intrinsic.x(), v = intrinsic.y(), uv = u*v; - if (H1) *H1 = Matrix_(2,6, - uv,-(1.+u*u), v, -d , 0., d*u, - (1.+v*v), -uv,-u, 0.,-d , d*v - ); - if (H2) { + const double z = q.z(), d = 1.0 / z; + const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v; + if (Dpose) + *Dpose = (Matrix(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), + -uv, -u, 0., -d, d * v); + if (Dpoint) { const Matrix R(pose_.rotation().matrix()); - *H2 = d * Matrix_(2,3, - R(0,0) - u*R(0,2), R(1,0) - u*R(1,2), R(2,0) - u*R(2,2), - R(0,1) - v*R(0,2), R(1,1) - v*R(1,2), R(2,1) - v*R(2,2) - ); + *Dpoint = d + * (Matrix(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), + R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2), + R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2)); } #endif } @@ -101,12 +100,12 @@ Point2 CalibratedCamera::project(const Point3& point, /* ************************************************************************* */ CalibratedCamera CalibratedCamera::retract(const Vector& d) const { - return CalibratedCamera(pose().retract(d)) ; + return CalibratedCamera(pose().retract(d)); } /* ************************************************************************* */ Vector CalibratedCamera::localCoordinates(const CalibratedCamera& T2) const { - return pose().localCoordinates(T2.pose()) ; + return pose().localCoordinates(T2.pose()); } /* ************************************************************************* */ diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index d67c69cbc..5e0376e92 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -24,192 +24,200 @@ namespace gtsam { - class GTSAM_EXPORT CheiralityException: public std::runtime_error { - public: - CheiralityException() : std::runtime_error("Cheirality Exception") {} - }; +class GTSAM_EXPORT CheiralityException: public std::runtime_error { +public: + CheiralityException() : + std::runtime_error("Cheirality Exception") { + } +}; + +/** + * A Calibrated camera class [R|-R't], calibration K=I. + * If calibration is known, it is more computationally efficient + * to calibrate the measurements rather than try to predict in pixels. + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT CalibratedCamera: public DerivedValue { +private: + Pose3 pose_; // 6DOF pose + +public: + + /// @name Standard Constructors + /// @{ + + /// default constructor + CalibratedCamera() { + } + + /// construct with pose + explicit CalibratedCamera(const Pose3& pose); + + /// @} + /// @name Advanced Constructors + /// @{ + + /// construct from vector + explicit CalibratedCamera(const Vector &v); + + /// @} + /// @name Testable + /// @{ + + virtual void print(const std::string& s = "") const { + pose_.print(s); + } + + /// check equality to another camera + bool equals(const CalibratedCamera &camera, double tol = 1e-9) const { + return pose_.equals(camera.pose(), tol); + } + + /// @} + /// @name Standard Interface + /// @{ + + /// destructor + virtual ~CalibratedCamera() { + } + + /// return pose + inline const Pose3& pose() const { + return pose_; + } + + /// compose the two camera poses: TODO Frank says this might not make sense + inline const CalibratedCamera compose(const CalibratedCamera &c, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + return CalibratedCamera(pose_.compose(c.pose(), H1, H2)); + } + + /// between the two camera poses: TODO Frank says this might not make sense + inline const CalibratedCamera between(const CalibratedCamera& c, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + return CalibratedCamera(pose_.between(c.pose(), H1, H2)); + } + + /// invert the camera pose: TODO Frank says this might not make sense + inline const CalibratedCamera inverse(boost::optional H1 = + boost::none) const { + return CalibratedCamera(pose_.inverse(H1)); + } /** - * A Calibrated camera class [R|-R't], calibration K=I. - * If calibration is known, it is more computationally efficient - * to calibrate the measurements rather than try to predict in pixels. - * @addtogroup geometry - * \nosubgrouping + * Create a level camera at the given 2D pose and height + * @param pose2 specifies the location and viewing direction + * @param height specifies the height of the camera (along the positive Z-axis) + * (theta 0 = looking in direction of positive X axis) */ - class GTSAM_EXPORT CalibratedCamera : public DerivedValue { - private: - Pose3 pose_; // 6DOF pose + static CalibratedCamera Level(const Pose2& pose2, double height); - public: + /// @} + /// @name Manifold + /// @{ - /// @name Standard Constructors - /// @{ + /// move a cameras pose according to d + CalibratedCamera retract(const Vector& d) const; - /// default constructor - CalibratedCamera() {} + /// Return canonical coordinate + Vector localCoordinates(const CalibratedCamera& T2) const; - /// construct with pose - explicit CalibratedCamera(const Pose3& pose); + /// Lie group dimensionality + inline size_t dim() const { + return 6; + } - /// @} - /// @name Advanced Constructors - /// @{ + /// Lie group dimensionality + inline static size_t Dim() { + return 6; + } - /// construct from vector - explicit CalibratedCamera(const Vector &v); + /* ************************************************************************* */ + // measurement functions and derivatives + /* ************************************************************************* */ - /// @} - /// @name Testable - /// @{ + /// @} + /// @name Transformations and mesaurement functions + /// @{ + /** + * This function receives the camera pose and the landmark location and + * returns the location the point is supposed to appear in the image + * @param point a 3D point to be projected + * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dpoint the optionally computed Jacobian with respect to the 3D point + * @return the intrinsic coordinates of the projected point + */ + Point2 project(const Point3& point, boost::optional Dpose = + boost::none, boost::optional Dpoint = boost::none) const; - virtual void print(const std::string& s = "") const { - pose_.print(s); - } + /** + * projects a 3-dimensional point in camera coordinates into the + * camera and returns a 2-dimensional point, no calibration applied + * With optional 2by3 derivative + */ + static Point2 project_to_camera(const Point3& cameraPoint, + boost::optional H1 = boost::none); - /// check equality to another camera - bool equals (const CalibratedCamera &camera, double tol = 1e-9) const { - return pose_.equals(camera.pose(), tol) ; - } + /** + * backproject a 2-dimensional point to a 3-dimension point + */ + static Point3 backproject_from_camera(const Point2& p, const double scale); - /// @} - /// @name Standard Interface - /// @{ + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @param H1 optionally computed Jacobian with respect to pose + * @param H2 optionally computed Jacobian with respect to the 3D point + * @return range (double) + */ + double range(const Point3& point, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + return pose_.range(point, H1, H2); + } - /// destructor - virtual ~CalibratedCamera() {} + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @param H1 optionally computed Jacobian with respect to pose + * @param H2 optionally computed Jacobian with respect to the 3D point + * @return range (double) + */ + double range(const Pose3& pose, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + return pose_.range(pose, H1, H2); + } - /// return pose - inline const Pose3& pose() const { return pose_; } - - /// compose the two camera poses: TODO Frank says this might not make sense - inline const CalibratedCamera compose(const CalibratedCamera &c, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - return CalibratedCamera( pose_.compose(c.pose(), H1, H2) ); - } - - /// between the two camera poses: TODO Frank says this might not make sense - inline const CalibratedCamera between(const CalibratedCamera& c, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - return CalibratedCamera( pose_.between(c.pose(), H1, H2) ); - } - - /// invert the camera pose: TODO Frank says this might not make sense - inline const CalibratedCamera inverse(boost::optional H1=boost::none) const { - return CalibratedCamera( pose_.inverse(H1) ); - } - - /** - * Create a level camera at the given 2D pose and height - * @param pose2 specifies the location and viewing direction - * @param height specifies the height of the camera (along the positive Z-axis) - * (theta 0 = looking in direction of positive X axis) - */ - static CalibratedCamera Level(const Pose2& pose2, double height); - - /// @} - /// @name Manifold - /// @{ - - /// move a cameras pose according to d - CalibratedCamera retract(const Vector& d) const; - - /// Return canonical coordinate - Vector localCoordinates(const CalibratedCamera& T2) const; - - /// Lie group dimensionality - inline size_t dim() const { return 6 ; } - - /// Lie group dimensionality - inline static size_t Dim() { return 6 ; } - - - /* ************************************************************************* */ - // measurement functions and derivatives - /* ************************************************************************* */ - - /// @} - /// @name Transformations and mesaurement functions - /// @{ - - /** - * This function receives the camera pose and the landmark location and - * returns the location the point is supposed to appear in the image - * @param point a 3D point to be projected - * @param D_intrinsic_pose the optionally computed Jacobian with respect to pose - * @param D_intrinsic_point the optionally computed Jacobian with respect to the 3D point - * @return the intrinsic coordinates of the projected point - */ - Point2 project(const Point3& point, - boost::optional D_intrinsic_pose = boost::none, - boost::optional D_intrinsic_point = boost::none) const; - - /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point, no calibration applied - * With optional 2by3 derivative - */ - static Point2 project_to_camera(const Point3& cameraPoint, - boost::optional H1 = boost::none); - - /** - * backproject a 2-dimensional point to a 3-dimension point - */ - static Point3 backproject_from_camera(const Point2& p, const double scale); - - /** - * Calculate range to a landmark - * @param point 3D location of landmark - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point - * @return range (double) - */ - double range(const Point3& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - return pose_.range(point, H1, H2); } - - /** - * Calculate range to another pose - * @param pose Other SO(3) pose - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point - * @return range (double) - */ - double range(const Pose3& pose, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - return pose_.range(pose, H1, H2); } - - /** - * Calculate range to another camera - * @param camera Other camera - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point - * @return range (double) - */ - double range(const CalibratedCamera& camera, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - return pose_.range(camera.pose_, H1, H2); } + /** + * Calculate range to another camera + * @param camera Other camera + * @param H1 optionally computed Jacobian with respect to pose + * @param H2 optionally computed Jacobian with respect to the 3D point + * @return range (double) + */ + double range(const CalibratedCamera& camera, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const { + return pose_.range(camera.pose_, H1, H2); + } private: - /// @} - /// @name Advanced Interface - /// @{ + /// @} + /// @name Advanced Interface + /// @{ - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("CalibratedCamera", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(pose_); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("CalibratedCamera", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(pose_); + } - /// @} - }; + /// @} +}; } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h new file mode 100644 index 000000000..991d8c922 --- /dev/null +++ b/gtsam/geometry/EssentialMatrix.h @@ -0,0 +1,156 @@ +/* + * @file EssentialMatrix.h + * @brief EssentialMatrix class + * @author Frank Dellaert + * @date December 17, 2013 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * An essential matrix is like a Pose3, except with translation up to scale + * It is named after the 3*3 matrix aEb = [aTb]x aRb from computer vision, + * but here we choose instead to parameterize it as a (Rot3,Sphere2) pair. + * We can then non-linearly optimize immediately on this 5-dimensional manifold. + */ +class EssentialMatrix: public DerivedValue { + +private: + + Rot3 aRb_; ///< Rotation between a and b + Sphere2 aTb_; ///< translation direction from a to b + Matrix E_; ///< Essential matrix + +public: + + /// Static function to convert Point2 to homogeneous coordinates + static Vector Homogeneous(const Point2& p) { + return Vector(3) << p.x(), p.y(), 1; + } + + /// @name Constructors and named constructors + /// @{ + + /// Construct from rotation and translation + EssentialMatrix(const Rot3& aRb, const Sphere2& aTb) : + aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) { + } + + /// @} + + /// @name Testable + /// @{ + + /// print with optional string + void print(const std::string& s = "") const { + std::cout << s; + aRb_.print("R:\n"); + aTb_.print("d: "); + } + + /// assert equality up to a tolerance + bool equals(const EssentialMatrix& other, double tol = 1e-8) const { + return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol); + } + + /// @} + + /// @name Manifold + /// @{ + + /// Dimensionality of tangent space = 5 DOF + inline static size_t Dim() { + return 5; + } + + /// Return the dimensionality of the tangent space + virtual size_t dim() const { + return 5; + } + + /// 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); + } + + /// Compute the coordinates in the tangent space + virtual Vector localCoordinates(const EssentialMatrix& value) const { + return Vector(5) << 0, 0, 0, 0, 0; + } + + /// @} + + /// @name Essential matrix methods + /// @{ + + /// Rotation + const Rot3& rotation() const { + return aRb_; + } + + /// Direction + const Sphere2& direction() const { + return aTb_; + } + + /// Return 3*3 matrix representation + const Matrix& matrix() const { + return E_; + } + + /** + * @brief takes point in world coordinates and transforms it to pose with |t|==1 + * @param p point in world coordinates + * @param DE optional 3*5 Jacobian wrpt to E + * @param Dpoint optional 3*3 Jacobian wrpt point + * @return point in pose coordinates + */ + 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; + } + + /// 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); + } + + /// @} + +}; + +} // gtsam + diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 39b5b679e..c162171c0 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -31,398 +31,503 @@ namespace gtsam { - /** - * A pinhole camera class that has a Pose3 and a Calibration. - * @addtogroup geometry - * \nosubgrouping - */ - template - class PinholeCamera : public DerivedValue > { - private: - Pose3 pose_; - Calibration K_; +/** + * A pinhole camera class that has a Pose3 and a Calibration. + * @addtogroup geometry + * \nosubgrouping + */ +template +class PinholeCamera: public DerivedValue > { +private: + Pose3 pose_; + Calibration K_; - public: +public: /// @name Standard Constructors /// @{ - /** default constructor */ - PinholeCamera() {} + /** default constructor */ + PinholeCamera() { + } - /** constructor with pose */ - explicit PinholeCamera(const Pose3& pose):pose_(pose){} + /** constructor with pose */ + explicit PinholeCamera(const Pose3& pose) : + pose_(pose) { + } - /** constructor with pose and calibration */ - PinholeCamera(const Pose3& pose, const Calibration& K):pose_(pose),K_(K) {} + /** constructor with pose and calibration */ + PinholeCamera(const Pose3& pose, const Calibration& K) : + pose_(pose), K_(K) { + } - /// @} - /// @name Named Constructors - /// @{ + /// @} + /// @name Named Constructors + /// @{ - /** - * Create a level camera at the given 2D pose and height - * @param K the calibration - * @param pose2 specifies the location and viewing direction - * (theta 0 = looking in direction of positive X axis) - * @param height camera height - */ - static PinholeCamera Level(const Calibration &K, const Pose2& pose2, double height) { - const double st = sin(pose2.theta()), ct = cos(pose2.theta()); - const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); - const Rot3 wRc(x, y, z); - const Point3 t(pose2.x(), pose2.y(), height); - const Pose3 pose3(wRc, t); - return PinholeCamera(pose3, K); + /** + * Create a level camera at the given 2D pose and height + * @param K the calibration + * @param pose2 specifies the location and viewing direction + * (theta 0 = looking in direction of positive X axis) + * @param height camera height + */ + static PinholeCamera Level(const Calibration &K, const Pose2& pose2, + double height) { + const double st = sin(pose2.theta()), ct = cos(pose2.theta()); + const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); + const Rot3 wRc(x, y, z); + const Point3 t(pose2.x(), pose2.y(), height); + const Pose3 pose3(wRc, t); + return PinholeCamera(pose3, K); + } + + /// PinholeCamera::level with default calibration + static PinholeCamera Level(const Pose2& pose2, double height) { + return PinholeCamera::Level(Calibration(), pose2, height); + } + + /** + * Create a camera at the given eye position looking at a target point in the scene + * with the specified up direction vector. + * @param eye specifies the camera position + * @param target the point to look at + * @param upVector specifies the camera up direction vector, + * doesn't need to be on the image plane nor orthogonal to the viewing axis + * @param K optional calibration parameter + */ + static PinholeCamera Lookat(const Point3& eye, const Point3& target, + const Point3& upVector, const Calibration& K = Calibration()) { + Point3 zc = target - eye; + zc = zc / zc.norm(); + Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down + xc = xc / xc.norm(); + Point3 yc = zc.cross(xc); + Pose3 pose3(Rot3(xc, yc, zc), eye); + return PinholeCamera(pose3, K); + } + + /// @} + /// @name Advanced Constructors + /// @{ + + explicit PinholeCamera(const Vector &v) { + pose_ = Pose3::Expmap(v.head(Pose3::Dim())); + if (v.size() > Pose3::Dim()) { + K_ = Calibration(v.tail(Calibration::Dim())); } + } - /// PinholeCamera::level with default calibration - static PinholeCamera Level(const Pose2& pose2, double height) { - return PinholeCamera::Level(Calibration(), pose2, height); + PinholeCamera(const Vector &v, const Vector &K) : + pose_(Pose3::Expmap(v)), K_(K) { + } + + /// @} + /// @name Testable + /// @{ + + /// assert equality up to a tolerance + bool equals(const PinholeCamera &camera, double tol = 1e-9) const { + return pose_.equals(camera.pose(), tol) + && K_.equals(camera.calibration(), tol); + } + + /// print + void print(const std::string& s = "PinholeCamera") const { + pose_.print(s + ".pose"); + K_.print(s + ".calibration"); + } + + /// @} + /// @name Standard Interface + /// @{ + + virtual ~PinholeCamera() { + } + + /// return pose + inline Pose3& pose() { + return pose_; + } + + /// return pose + inline const Pose3& pose() const { + return pose_; + } + + /// return calibration + inline Calibration& calibration() { + return K_; + } + + /// return calibration + inline const Calibration& calibration() const { + return K_; + } + + /// @} + /// @name Group ?? Frank says this might not make sense + /// @{ + + /// compose two cameras: TODO Frank says this might not make sense + inline const PinholeCamera compose(const PinholeCamera &c, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + PinholeCamera result(pose_.compose(c.pose(), H1, H2), K_); + if (H1) { + H1->conservativeResize(Dim(), Dim()); + H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), + Calibration::Dim()); + H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); } - - /** - * Create a camera at the given eye position looking at a target point in the scene - * with the specified up direction vector. - * @param eye specifies the camera position - * @param target the point to look at - * @param upVector specifies the camera up direction vector, - * doesn't need to be on the image plane nor orthogonal to the viewing axis - * @param K optional calibration parameter - */ - static PinholeCamera Lookat(const Point3& eye, const Point3& target, const Point3& upVector, const Calibration& K = Calibration()) { - Point3 zc = target-eye; - zc = zc/zc.norm(); - Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down - xc = xc/xc.norm(); - Point3 yc = zc.cross(xc); - Pose3 pose3(Rot3(xc,yc,zc), eye); - return PinholeCamera(pose3, K); + if (H2) { + H2->conservativeResize(Dim(), Dim()); + H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), + Calibration::Dim()); + H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); } + return result; + } - /// @} - /// @name Advanced Constructors - /// @{ + /// between two cameras: TODO Frank says this might not make sense + inline const PinholeCamera between(const PinholeCamera& c, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + PinholeCamera result(pose_.between(c.pose(), H1, H2), K_); + if (H1) { + H1->conservativeResize(Dim(), Dim()); + H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), + Calibration::Dim()); + H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); + } + if (H2) { + H2->conservativeResize(Dim(), Dim()); + H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), + Calibration::Dim()); + H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); + } + return result; + } - explicit PinholeCamera(const Vector &v) { - pose_ = Pose3::Expmap(v.head(Pose3::Dim())); - if (v.size() > Pose3::Dim()) { - K_ = Calibration(v.tail(Calibration::Dim())); + /// inverse camera: TODO Frank says this might not make sense + inline const PinholeCamera inverse( + boost::optional H1 = boost::none) const { + PinholeCamera result(pose_.inverse(H1), K_); + if (H1) { + H1->conservativeResize(Dim(), Dim()); + H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), + Calibration::Dim()); + H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); + } + return result; + } + + /// compose two cameras: TODO Frank says this might not make sense + inline const PinholeCamera compose(const Pose3 &c) const { + return PinholeCamera(pose_.compose(c), K_); + } + + /// @} + /// @name Manifold + /// @{ + + /// move a cameras according to d + PinholeCamera retract(const Vector& d) const { + if ((size_t) d.size() == pose_.dim()) + return PinholeCamera(pose().retract(d), calibration()); + else + return PinholeCamera(pose().retract(d.head(pose().dim())), + calibration().retract(d.tail(calibration().dim()))); + } + + /// return canonical coordinate + Vector localCoordinates(const PinholeCamera& T2) const { + Vector d(dim()); + d.head(pose().dim()) = pose().localCoordinates(T2.pose()); + d.tail(calibration().dim()) = calibration().localCoordinates( + T2.calibration()); + return d; + } + + /// Manifold dimension + inline size_t dim() const { + return pose_.dim() + K_.dim(); + } + + /// Manifold dimension + inline static size_t Dim() { + return Pose3::Dim() + Calibration::Dim(); + } + + /// @} + /// @name Transformations and measurement functions + /// @{ + + /** + * projects a 3-dimensional point in camera coordinates into the + * camera and returns a 2-dimensional point, no calibration applied + * @param P A point in camera coordinates + * @param Dpoint is the 2*3 Jacobian w.r.t. P + */ + inline static Point2 project_to_camera(const Point3& P, + boost::optional Dpoint = boost::none) { + if (Dpoint) { + double d = 1.0 / P.z(), d2 = d * d; + *Dpoint = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); + } + return Point2(P.x() / P.z(), P.y() / P.z()); + } + + /// Project a point into the image and check depth + inline std::pair projectSafe(const Point3& pw) const { + const Point3 pc = pose_.transform_to(pw); + const Point2 pn = project_to_camera(pc); + return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); + } + + /** project a point from world coordinate to the image + * @param pw is a point in world coordinates + * @param Dpose is the Jacobian w.r.t. pose3 + * @param Dpoint is the Jacobian w.r.t. point3 + * @param Dcal is the Jacobian w.r.t. calibration + */ + inline Point2 project( + const Point3& pw, // + boost::optional Dpose = boost::none, + boost::optional Dpoint = boost::none, + boost::optional Dcal = boost::none) const { + + // Transform to camera coordinates and check cheirality + const Point3 pc = pose_.transform_to(pw); + +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (pc.z() <= 0) + throw CheiralityException(); +#endif + + // Project to normalized image coordinates + const Point2 pn = project_to_camera(pc); + + if (Dpose || Dpoint) { + // optimized version of derivatives, see CalibratedCamera.nb + const double z = pc.z(), d = 1.0 / z; + const double u = pn.x(), v = pn.y(); + Matrix Dpn_pose(2, 6), Dpn_point(2, 3); + if (Dpose) { + double uv = u * v, uu = u * u, vv = v * v; + Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; } - } - - PinholeCamera(const Vector &v, const Vector &K) : - pose_(Pose3::Expmap(v)), K_(K) { - } - - /// @} - /// @name Testable - /// @{ - - /// assert equality up to a tolerance - bool equals (const PinholeCamera &camera, double tol = 1e-9) const { - return pose_.equals(camera.pose(), tol) && - K_.equals(camera.calibration(), tol) ; - } - - /// print - void print(const std::string& s = "PinholeCamera") const { - pose_.print(s+".pose"); - K_.print(s+".calibration"); - } - - /// @} - /// @name Standard Interface - /// @{ - - virtual ~PinholeCamera() {} - - /// return pose - inline Pose3& pose() { return pose_; } - - /// return pose - inline const Pose3& pose() const { return pose_; } - - /// return calibration - inline Calibration& calibration() { return K_; } - - /// return calibration - inline const Calibration& calibration() const { return K_; } - - /// @} - /// @name Group ?? Frank says this might not make sense - /// @{ - - /// compose two cameras: TODO Frank says this might not make sense - inline const PinholeCamera compose(const PinholeCamera &c, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - PinholeCamera result( pose_.compose(c.pose(), H1, H2), K_ ); - if(H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); + if (Dpoint) { + const Matrix R(pose_.rotation().matrix()); + Dpn_point << // + R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // + /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); + Dpn_point *= d; } - if(H2) { - H2->conservativeResize(Dim(), Dim()); - H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); - H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// between two cameras: TODO Frank says this might not make sense - inline const PinholeCamera between(const PinholeCamera& c, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - PinholeCamera result( pose_.between(c.pose(), H1, H2), K_ ); - if(H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - if(H2) { - H2->conservativeResize(Dim(), Dim()); - H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); - H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// inverse camera: TODO Frank says this might not make sense - inline const PinholeCamera inverse(boost::optional H1=boost::none) const { - PinholeCamera result( pose_.inverse(H1), K_ ); - if(H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// compose two cameras: TODO Frank says this might not make sense - inline const PinholeCamera compose(const Pose3 &c) const { - return PinholeCamera( pose_.compose(c), K_ ); - } - - /// @} - /// @name Manifold - /// @{ - - /// move a cameras according to d - PinholeCamera retract(const Vector& d) const { - if ((size_t) d.size() == pose_.dim() ) - return PinholeCamera(pose().retract(d), calibration()) ; - else - return PinholeCamera(pose().retract(d.head(pose().dim())), - calibration().retract(d.tail(calibration().dim()))) ; - } - - /// return canonical coordinate - Vector localCoordinates(const PinholeCamera& T2) const { - Vector d(dim()); - d.head(pose().dim()) = pose().localCoordinates(T2.pose()); - d.tail(calibration().dim()) = calibration().localCoordinates(T2.calibration()); - return d; - } - - /// Manifold dimension - inline size_t dim() const { return pose_.dim() + K_.dim(); } - - /// Manifold dimension - inline static size_t Dim() { return Pose3::Dim() + Calibration::Dim(); } - - /// @} - /// @name Transformations and measurement functions - /// @{ - - /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point, no calibration applied - * With optional 2by3 derivative - */ - inline static Point2 project_to_camera(const Point3& P, - boost::optional H1 = boost::none){ - if (H1) { - double d = 1.0 / P.z(), d2 = d * d; - *H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); - } - return Point2(P.x() / P.z(), P.y() / P.z()); - } - - /// Project a point into the image and check depth - inline std::pair projectSafe(const Point3& pw) const { - const Point3 pc = pose_.transform_to(pw) ; - const Point2 pn = project_to_camera(pc) ; - return std::make_pair(K_.uncalibrate(pn), pc.z()>0); - } - - /** project a point from world coordinate to the image - * @param pw is a point in the world coordinate - * @param H1 is the jacobian w.r.t. pose3 - * @param H2 is the jacobian w.r.t. point3 - * @param H3 is the jacobian w.r.t. calibration - */ - inline Point2 project(const Point3& pw, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const { - - if (!H1 && !H2 && !H3) { - const Point3 pc = pose_.transform_to(pw) ; - if ( pc.z() <= 0 ) throw CheiralityException(); - const Point2 pn = project_to_camera(pc) ; - return K_.uncalibrate(pn); - } - - // world to camera coordinate - Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ; - const Point3 pc = pose_.transform_to(pw, Hc1, Hc2) ; - if( pc.z() <= 0 ) throw CheiralityException(); - - // camera to normalized image coordinate - Matrix Hn; // 2*3 - const Point2 pn = project_to_camera(pc, Hn) ; // uncalibration - Matrix Hk, Hi; // 2*2 - const Point2 pi = K_.uncalibrate(pn, Hk, Hi); + Matrix Dpi_pn; // 2*2 + const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - // chain the jacobian matrices - const Matrix tmp = Hi*Hn ; - if (H1) *H1 = tmp * Hc1 ; - if (H2) *H2 = tmp * Hc2 ; - if (H3) *H3 = Hk; + // chain the Jacobian matrices + if (Dpose) + *Dpose = Dpi_pn * Dpn_pose; + if (Dpoint) + *Dpoint = Dpi_pn * Dpn_point; return pi; + } else + return K_.uncalibrate(pn, Dcal); + } + + /** project a point at infinity from world coordinate to the image + * @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf) + * @param Dpose is the Jacobian w.r.t. pose3 + * @param Dpoint is the Jacobian w.r.t. point3 + * @param Dcal is the Jacobian w.r.t. calibration + */ + inline Point2 projectPointAtInfinity( + const Point3& pw, // + boost::optional Dpose = boost::none, + boost::optional Dpoint = boost::none, + boost::optional Dcal = boost::none) const { + + if (!Dpose && !Dpoint && !Dcal) { + const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) + const Point2 pn = project_to_camera(pc); // project the point to the camera + return K_.uncalibrate(pn); } - /** project a point from world coordinate to the image - * @param pw is a point in the world coordinate - * @param H1 is the jacobian w.r.t. [pose3 calibration] - * @param H2 is the jacobian w.r.t. point3 - */ - inline Point2 project2(const Point3& pw, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + // world to camera coordinate + Matrix Dpc_rot /* 3*3 */, Dpc_point /* 3*3 */; + const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point); - if (!H1 && !H2) { - const Point3 pc = pose_.transform_to(pw) ; - if ( pc.z() <= 0 ) throw CheiralityException(); - const Point2 pn = project_to_camera(pc) ; - return K_.uncalibrate(pn); - } + Matrix Dpc_pose = Matrix::Zero(3, 6); + Dpc_pose.block(0, 0, 3, 3) = Dpc_rot; - Matrix Htmp1, Htmp2, Htmp3; - const Point2 pi = this->project(pw, Htmp1, Htmp2, Htmp3); - if (H1) { - *H1 = Matrix(2, this->dim()); - H1->leftCols(pose().dim()) = Htmp1 ; // jacobian wrt pose3 - H1->rightCols(calibration().dim()) = Htmp3 ; // jacobian wrt calib - } - if (H2) *H2 = Htmp2; - return pi; + // camera to normalized image coordinate + Matrix Dpn_pc; // 2*3 + const Point2 pn = project_to_camera(pc, Dpn_pc); + + // uncalibration + Matrix Dpi_pn; // 2*2 + const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); + + // chain the Jacobian matrices + const Matrix Dpi_pc = Dpi_pn * Dpn_pc; + if (Dpose) + *Dpose = Dpi_pc * Dpc_pose; + if (Dpoint) + *Dpoint = (Dpi_pc * Dpc_point).block(0, 0, 2, 2); // only 2dof are important for the point (direction-only) + return pi; + } + + /** project a point from world coordinate to the image + * @param pw is a point in the world coordinate + * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] + * @param Dpoint is the Jacobian w.r.t. point3 + */ + inline Point2 project2( + const Point3& pw, // + boost::optional Dcamera = boost::none, + boost::optional Dpoint = boost::none) const { + + if (!Dcamera && !Dpoint) { + const Point3 pc = pose_.transform_to(pw); + +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (pc.z() <= 0) + throw CheiralityException(); +#endif + const Point2 pn = project_to_camera(pc); + return K_.uncalibrate(pn); } - /// backproject a 2-dimensional point to a 3-dimensional point at given depth - inline Point3 backproject(const Point2& p, double depth) const { - const Point2 pn = K_.calibrate(p); - const Point3 pc(pn.x()*depth, pn.y()*depth, depth); - return pose_.transform_from(pc); + Matrix Dpose, Dp, Dcal; + const Point2 pi = this->project(pw, Dpose, Dp, Dcal); + if (Dcamera) { + *Dcamera = Matrix(2, this->dim()); + Dcamera->leftCols(pose().dim()) = Dpose; // Jacobian wrt pose3 + Dcamera->rightCols(calibration().dim()) = Dcal; // Jacobian wrt calib } + if (Dpoint) + *Dpoint = Dp; + return pi; + } - /** - * Calculate range to a landmark - * @param point 3D location of landmark - * @param H1 the optionally computed Jacobian with respect to pose - * @param H2 the optionally computed Jacobian with respect to the landmark - * @return range (double) - */ - double range(const Point3& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - double result = pose_.range(point, H1, H2); - if(H1) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*H1); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); - } - return result; + /// backproject a 2-dimensional point to a 3-dimensional point at given depth + inline Point3 backproject(const Point2& p, double depth) const { + const Point2 pn = K_.calibrate(p); + const Point3 pc(pn.x() * depth, pn.y() * depth, depth); + return pose_.transform_from(pc); + } + + /// backproject a 2-dimensional point to a 3-dimensional point at infinity + inline Point3 backprojectPointAtInfinity(const Point2& p) const { + const Point2 pn = K_.calibrate(p); + const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 + return pose_.rotation().rotate(pc); + } + + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dpoint the optionally computed Jacobian with respect to the landmark + * @return range (double) + */ + double range( + const Point3& point, // + boost::optional Dpose = boost::none, + boost::optional Dpoint = boost::none) const { + double result = pose_.range(point, Dpose, Dpoint); + if (Dpose) { + // Add columns of zeros to Jacobian for calibration + Matrix& H1r(*Dpose); + H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); + H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); } + return result; + } - /** - * Calculate range to another pose - * @param pose Other SO(3) pose - * @param H1 the optionally computed Jacobian with respect to pose - * @param H2 the optionally computed Jacobian with respect to the landmark - * @return range (double) - */ - double range(const Pose3& pose, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - double result = pose_.range(pose, H1, H2); - if(H1) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*H1); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); - } - return result; + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dpose2 the optionally computed Jacobian with respect to the other pose + * @return range (double) + */ + double range( + const Pose3& pose, // + boost::optional Dpose = boost::none, + boost::optional Dpose2 = boost::none) const { + double result = pose_.range(pose, Dpose, Dpose2); + if (Dpose) { + // Add columns of zeros to Jacobian for calibration + Matrix& H1r(*Dpose); + H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); + H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); } + return result; + } - /** - * Calculate range to another camera - * @param camera Other camera - * @param H1 the optionally computed Jacobian with respect to pose - * @param H2 the optionally computed Jacobian with respect to the landmark - * @return range (double) - */ - template - double range(const PinholeCamera& camera, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - double result = pose_.range(camera.pose_, H1, H2); - if(H1) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*H1); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); - } - if(H2) { - // Add columns of zeros to Jacobian for calibration - Matrix& H2r(*H2); - H2r.conservativeResize(Eigen::NoChange, camera.pose().dim() + camera.calibration().dim()); - H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) = Matrix::Zero(1, camera.calibration().dim()); - } - return result; + /** + * Calculate range to another camera + * @param camera Other camera + * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dother the optionally computed Jacobian with respect to the other camera + * @return range (double) + */ + template + double range( + const PinholeCamera& camera, // + boost::optional Dpose = boost::none, + boost::optional Dother = boost::none) const { + double result = pose_.range(camera.pose_, Dpose, Dother); + if (Dpose) { + // Add columns of zeros to Jacobian for calibration + Matrix& H1r(*Dpose); + H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); + H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); } + if (Dother) { + // Add columns of zeros to Jacobian for calibration + Matrix& H2r(*Dother); + H2r.conservativeResize(Eigen::NoChange, + camera.pose().dim() + camera.calibration().dim()); + H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) = + Matrix::Zero(1, camera.calibration().dim()); + } + return result; + } - /** - * Calculate range to another camera - * @param camera Other camera - * @param H1 the optionally computed Jacobian with respect to pose - * @param H2 the optionally computed Jacobian with respect to the landmark - * @return range (double) - */ - double range(const CalibratedCamera& camera, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - return pose_.range(camera.pose_, H1, H2); } + /** + * Calculate range to another camera + * @param camera Other camera + * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dother the optionally computed Jacobian with respect to the other camera + * @return range (double) + */ + double range( + const CalibratedCamera& camera, // + boost::optional Dpose = boost::none, + boost::optional Dother = boost::none) const { + return pose_.range(camera.pose_, Dpose, Dother); + } private: - /// @} - /// @name Advanced Interface - /// @{ + /// @} + /// @name Advanced Interface + /// @{ - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); - ar & BOOST_SERIALIZATION_NVP(pose_); - ar & BOOST_SERIALIZATION_NVP(K_); + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); + ar & BOOST_SERIALIZATION_NVP(pose_); + ar & BOOST_SERIALIZATION_NVP(K_); + } + /// @} } - /// @} - }; -} + ;} diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 6f219b9db..6fc9330ad 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -27,7 +27,7 @@ namespace gtsam { /** Explicit instantiation of base class to export members */ INSTANTIATE_LIE(Point2); -static const Matrix oneOne = Matrix_(1, 2, 1.0, 1.0); +static const Matrix oneOne = (Matrix(1, 2) << 1.0, 1.0); /* ************************************************************************* */ void Point2::print(const string& s) const { @@ -45,7 +45,7 @@ double Point2::norm(boost::optional H) const { if (H) { Matrix D_result_d; if (fabs(r) > 1e-10) - D_result_d = Matrix_(1, 2, x_ / r, y_ / r); + D_result_d = (Matrix(1, 2) << x_ / r, y_ / r); else D_result_d = oneOne; // TODO: really infinity, why 1 here?? *H = D_result_d; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 73f0b83f6..0838650ea 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -47,9 +47,6 @@ public: /// default constructor Point2(): x_(0), y_(0) {} - /// copy constructor - Point2(const Point2 &p) : x_(p.x_), y_(p.y_) {} - /// construct from doubles Point2(double x, double y): x_(x), y_(y) {} diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index c0a3d43d2..30a4434fe 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -40,7 +40,7 @@ static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); Matrix Pose2::matrix() const { Matrix R = r_.matrix(); R = stack(2, &R, &Z12); - Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0); + Matrix T = (Matrix(3, 1) << t_.x(), t_.y(), 1.0); return collect(2, &R, &T); } @@ -75,13 +75,13 @@ Vector Pose2::Logmap(const Pose2& p) { const Point2& t = p.t(); double w = R.theta(); if (std::abs(w) < 1e-10) - return Vector_(3, t.x(), t.y(), w); + return (Vector(3) << t.x(), t.y(), w); else { double c_1 = R.c()-1.0, s = R.s(); double det = c_1*c_1 + s*s; Point2 p = R_PI_2 * (R.unrotate(t) - t); Point2 v = (w/det) * p; - return Vector_(3, v.x(), v.y(), w); + return (Vector(3) << v.x(), v.y(), w); } } @@ -101,7 +101,7 @@ Vector Pose2::localCoordinates(const Pose2& p2) const { return Logmap(between(p2)); #else Pose2 r = between(p2); - return Vector_(3, r.x(), r.y(), r.theta()); + return (Vector(3) << r.x(), r.y(), r.theta()); #endif } @@ -110,7 +110,7 @@ Vector Pose2::localCoordinates(const Pose2& p2) const { // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) Matrix Pose2::AdjointMap() const { double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y(); - return Matrix_(3,3, + return (Matrix(3, 3) << c, -s, y, s, c, -x, 0.0, 0.0, 1.0 @@ -130,7 +130,7 @@ Point2 Pose2::transform_to(const Point2& point, Point2 d = point - t_; Point2 q = r_.unrotate(d); if (!H1 && !H2) return q; - if (H1) *H1 = Matrix_(2, 3, + if (H1) *H1 = (Matrix(2, 3) << -1.0, 0.0, q.y(), 0.0, -1.0, -q.x()); if (H2) *H2 = r_.transpose(); @@ -154,7 +154,7 @@ Point2 Pose2::transform_from(const Point2& p, const Point2 q = r_ * p; if (H1 || H2) { const Matrix R = r_.matrix(); - const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x()); + const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x()); if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q] if (H2) *H2 = R; // R } @@ -184,7 +184,7 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional H1, if (H1) { double dt1 = -s2 * x + c2 * y; double dt2 = -c2 * x - s2 * y; - *H1 = Matrix_(3,3, + *H1 = (Matrix(3, 3) << -c, -s, dt1, s, -c, dt2, 0.0, 0.0,-1.0); @@ -225,7 +225,7 @@ double Pose2::range(const Point2& point, if (!H1 && !H2) return d.norm(); Matrix H; double r = d.norm(H); - if (H1) *H1 = H * Matrix_(2, 3, + if (H1) *H1 = H * (Matrix(2, 3) << -r_.c(), r_.s(), 0.0, -r_.s(), -r_.c(), 0.0); if (H2) *H2 = H; @@ -240,10 +240,10 @@ double Pose2::range(const Pose2& pose2, if (!H1 && !H2) return d.norm(); Matrix H; double r = d.norm(H); - if (H1) *H1 = H * Matrix_(2, 3, + if (H1) *H1 = H * (Matrix(2, 3) << -r_.c(), r_.s(), 0.0, -r_.s(), -r_.c(), 0.0); - if (H2) *H2 = H * Matrix_(2, 3, + if (H2) *H2 = H * (Matrix(2, 3) << pose2.r_.c(), -pose2.r_.s(), 0.0, pose2.r_.s(), pose2.r_.c(), 0.0); return r; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index fd6fbe9ad..26244877b 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -172,7 +172,7 @@ public: * @return xihat, 3*3 element of Lie algebra that can be exponentiated */ static inline Matrix wedge(double vx, double vy, double w) { - return Matrix_(3,3, + return (Matrix(3,3) << 0.,-w, vx, w, 0., vy, 0., 0., 0.); diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index faec92a6b..adbe763b3 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -26,333 +26,348 @@ using namespace std; namespace gtsam { - /** Explicit instantiation of base class to export members */ - INSTANTIATE_LIE(Pose3); +/** Explicit instantiation of base class to export members */ +INSTANTIATE_LIE(Pose3); - /** instantiate concept checks */ - GTSAM_CONCEPT_POSE_INST(Pose3); +/** instantiate concept checks */ +GTSAM_CONCEPT_POSE_INST(Pose3); - static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3=-I3; - static const Matrix6 I6 = eye(6); +static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3; +static const Matrix6 I6 = eye(6); - /* ************************************************************************* */ - Pose3::Pose3(const Pose2& pose2) : - R_(Rot3::rodriguez(0, 0, pose2.theta())), - t_(Point3(pose2.x(), pose2.y(), 0)) { - } +/* ************************************************************************* */ +Pose3::Pose3(const Pose2& pose2) : + R_(Rot3::rodriguez(0, 0, pose2.theta())), t_( + Point3(pose2.x(), pose2.y(), 0)) { +} - /* ************************************************************************* */ - // Calculate Adjoint map - // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) - // Experimental - unit tests of derivatives based on it do not check out yet - Matrix6 Pose3::AdjointMap() const { - const Matrix3 R = R_.matrix(); - const Vector3 t = t_.vector(); - Matrix3 A = skewSymmetric(t)*R; - Matrix6 adj; - adj << R, Z3, A, R; - return adj; - } +/* ************************************************************************* */ +// Calculate Adjoint map +// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) +// Experimental - unit tests of derivatives based on it do not check out yet +Matrix6 Pose3::AdjointMap() const { + const Matrix3 R = R_.matrix(); + const Vector3 t = t_.vector(); + Matrix3 A = skewSymmetric(t) * R; + Matrix6 adj; + adj << R, Z3, A, R; + return adj; +} - /* ************************************************************************* */ - Matrix6 Pose3::adjointMap(const Vector& xi) { - Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); - Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); - Matrix6 adj; - adj << w_hat, Z3, v_hat, w_hat; +/* ************************************************************************* */ +Matrix6 Pose3::adjointMap(const Vector& xi) { + Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); + Matrix6 adj; + adj << w_hat, Z3, v_hat, w_hat; - return adj; - } + return adj; +} - /* ************************************************************************* */ - Vector Pose3::adjoint(const Vector& xi, const Vector& y, boost::optional H) { - if (H) { - *H = zeros(6,6); - for (int i = 0; i<6; ++i) { - Vector dxi = zero(6); dxi(i) = 1.0; - Matrix Gi = adjointMap(dxi); - (*H).col(i) = Gi*y; - } - } - return adjointMap(xi)*y; - } - - /* ************************************************************************* */ - Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, boost::optional H) { - if (H) { - *H = zeros(6,6); - for (int i = 0; i<6; ++i) { - Vector dxi = zero(6); dxi(i) = 1.0; - Matrix GTi = adjointMap(dxi).transpose(); - (*H).col(i) = GTi*y; - } - } - Matrix adjT = adjointMap(xi).transpose(); - return adjointMap(xi).transpose() * 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, 0.0, 1.0/42.0, 0.0, -1.0/30); - static const int N = 5; // order of approximation - Matrix res = I6; - Matrix6 ad_i = I6; - Matrix6 ad_xi = adjointMap(xi); - double fac = 1.0; - for (int i = 1 ; i H) { + if (H) { + *H = zeros(6, 6); + for (int i = 0; i < 6; ++i) { + Vector dxi = zero(6); + dxi(i) = 1.0; + Matrix Gi = adjointMap(dxi); + (*H).col(i) = Gi * y; } } + return adjointMap(xi) * y; +} - /* ************************************************************************* */ - Vector6 Pose3::Logmap(const Pose3& p) { - Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); - double t = w.norm(); - if (t < 1e-10) { - Vector6 log; - log << w, T; - return log; - } - else { - Matrix3 W = skewSymmetric(w/t); - // Formula from Agrawal06iros, equation (14) - // simplified with Mathematica, and multiplying in T to avoid matrix math - double Tan = tan(0.5*t); - Vector3 WT = W*T; - Vector3 u = T - (0.5*t)*WT + (1 - t/(2.*Tan)) * (W * WT); - Vector6 log; - log << w, u; - return log; +/* ************************************************************************* */ +Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, + boost::optional H) { + if (H) { + *H = zeros(6, 6); + for (int i = 0; i < 6; ++i) { + Vector dxi = zero(6); + dxi(i) = 1.0; + Matrix GTi = adjointMap(dxi).transpose(); + (*H).col(i) = GTi * y; } } + Matrix adjT = adjointMap(xi).transpose(); + return adjointMap(xi).transpose() * y; +} - /* ************************************************************************* */ - Pose3 Pose3::retractFirstOrder(const Vector& xi) const { - Vector3 omega(sub(xi, 0, 3)); - Point3 v(sub(xi, 3, 6)); - Rot3 R = R_.retract(omega); // R is done exactly - Point3 t = t_ + R_ * v; // First order t approximation - return Pose3(R, t); +/* ************************************************************************* */ +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, + 0.0, 1.0 / 42.0, 0.0, -1.0 / 30); + static const int N = 5; // order of approximation + Matrix res = I6; + Matrix6 ad_i = I6; + Matrix6 ad_xi = adjointMap(xi); + double fac = 1.0; + for (int i = 1; i < N; ++i) { + ad_i = ad_xi * ad_i; + fac = fac * i; + res = res + B(i) / fac * ad_i; } + return res; +} - /* ************************************************************************* */ - // Different versions of retract - Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { - if(mode == Pose3::EXPMAP) { - // Lie group exponential map, traces out geodesic - return compose(Expmap(xi)); - } else if(mode == Pose3::FIRST_ORDER) { - // First order - return retractFirstOrder(xi); - } else { - // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently - // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation - assert(false); - exit(1); - } - } +/* ************************************************************************* */ +void Pose3::print(const string& s) const { + cout << s; + R_.print("R:\n"); + t_.print("t: "); +} - /* ************************************************************************* */ - // different versions of localCoordinates - Vector6 Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const { - if(mode == Pose3::EXPMAP) { - // Lie group logarithm map, exact inverse of exponential map - return Logmap(between(T)); - } else if(mode == Pose3::FIRST_ORDER) { - // R is always done exactly in all three retract versions below - Vector3 omega = R_.localCoordinates(T.rotation()); +/* ************************************************************************* */ +bool Pose3::equals(const Pose3& pose, double tol) const { + return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol); +} - // Incorrect version - // Independently computes the logmap of the translation and rotation - // Vector v = t_.localCoordinates(T.translation()); +/* ************************************************************************* */ +/** Modified from Murray94book version (which assumes w and v normalized?) */ +Pose3 Pose3::Expmap(const Vector& xi) { - // Correct first order t inverse - Point3 d = R_.unrotate(T.translation() - t_); + // get angular velocity omega and translational velocity v from twist xi + Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); - // TODO: correct second order t inverse - Vector6 local; - local << omega(0),omega(1),omega(2),d.x(),d.y(),d.z(); - return local; - } else { - assert(false); - exit(1); - } - } - - /* ************************************************************************* */ - Matrix4 Pose3::matrix() const { - const Matrix3 R = R_.matrix(); - const Vector3 T = t_.vector(); - Eigen::Matrix A14; - A14 << 0.0, 0.0, 0.0, 1.0; - Matrix4 mat; - mat << R, T, A14; - return mat; - } - - /* ************************************************************************* */ - Pose3 Pose3::transform_to(const Pose3& pose) const { - Rot3 cRv = R_ * Rot3(pose.R_.inverse()); - Point3 t = pose.transform_to(t_); - return Pose3(cRv, t); - } - - /* ************************************************************************* */ - Point3 Pose3::transform_from(const Point3& p, - boost::optional H1, boost::optional H2) const { - if (H1) { - const Matrix R = R_.matrix(); - Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z()); - H1->resize(3,6); - (*H1) << DR, R; - } - if (H2) *H2 = R_.matrix(); - return R_ * p + t_; - } - - /* ************************************************************************* */ - Point3 Pose3::transform_to(const Point3& p, - boost::optional H1, boost::optional H2) const { - const Point3 result = R_.unrotate(p - t_); - if (H1) { - const Point3& q = result; - Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); - H1->resize(3,6); - (*H1) << DR, _I3; - } - if (H2) *H2 = R_.transpose(); - return result; - } - - /* ************************************************************************* */ - Pose3 Pose3::compose(const Pose3& p2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = p2.inverse().AdjointMap(); - if (H2) *H2 = I6; - return (*this) * p2; - } - - /* ************************************************************************* */ - Pose3 Pose3::inverse(boost::optional H1) const { - if (H1) *H1 = -AdjointMap(); - Rot3 Rt = R_.inverse(); - return Pose3(Rt, Rt*(-t_)); - } - - /* ************************************************************************* */ - // between = compose(p2,inverse(p1)); - Pose3 Pose3::between(const Pose3& p2, boost::optional H1, - boost::optional H2) const { - Pose3 result = inverse()*p2; - if (H1) *H1 = -result.inverse().AdjointMap(); - if (H2) *H2 = I6; - return result; - } - - /* ************************************************************************* */ - double Pose3::range(const Point3& point, - boost::optional H1, - boost::optional H2) const { - if (!H1 && !H2) return transform_to(point).norm(); - Point3 d = transform_to(point, H1, H2); - double x = d.x(), y = d.y(), z = d.z(), - d2 = x * x + y * y + z * z, n = sqrt(d2); - Matrix D_result_d = Matrix_(1, 3, x / n, y / n, z / n); - if (H1) *H1 = D_result_d * (*H1); - if (H2) *H2 = D_result_d * (*H2); - return n; - } - - /* ************************************************************************* */ - double Pose3::range(const Pose3& point, - boost::optional H1, boost::optional H2) const { - double r = range(point.translation(), H1, H2); - if (H2) { - Matrix H2_ = *H2 * point.rotation().matrix(); - *H2 = zeros(1, 6); - insertSub(*H2, H2_, 0, 3); - } - return r; - } - - /* ************************************************************************* */ - boost::optional align(const vector& pairs) { - const size_t n = pairs.size(); - if (n<3) return boost::none; // we need at least three pairs - - // calculate centroids - Vector cp = zero(3),cq = zero(3); - BOOST_FOREACH(const Point3Pair& pair, pairs) { - cp += pair.first.vector(); - cq += pair.second.vector(); - } - double f = 1.0/n; - cp *= f; cq *= f; - - // Add to form H matrix - Matrix H = zeros(3,3); - BOOST_FOREACH(const Point3Pair& pair, pairs) { - Vector dp = pair.first.vector() - cp; - Vector dq = pair.second.vector() - cq; - H += dp * dq.transpose(); - } - - // Compute SVD - Matrix U,V; - Vector S; - svd(H,U,S,V); - - // Recover transform with correction from Eggert97machinevisionandapplications - Matrix UVtranspose = U * V.transpose(); - Matrix detWeighting = eye(3,3); - detWeighting(2,2) = UVtranspose.determinant(); - Rot3 R(Matrix(V * detWeighting * U.transpose())); - Point3 t = Point3(cq) - R * Point3(cp); + double theta = w.norm(); + if (theta < 1e-10) { + static const Rot3 I; + return Pose3(I, v); + } else { + Point3 n(w / theta); // axis unit vector + Rot3 R = Rot3::rodriguez(n.vector(), theta); + double vn = n.dot(v); // translation parallel to n + Point3 n_cross_v = n.cross(v); // points towards axis + Point3 t = (n_cross_v - R * n_cross_v) / theta + vn * n; return Pose3(R, t); } +} - /* ************************************************************************* */ - std::ostream &operator<<(std::ostream &os, const Pose3& pose) { - os << pose.rotation() << "\n" << pose.translation() << endl; - return os; +/* ************************************************************************* */ +Vector6 Pose3::Logmap(const Pose3& p) { + Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); + double t = w.norm(); + if (t < 1e-10) { + Vector6 log; + log << w, T; + return log; + } else { + Matrix3 W = skewSymmetric(w / t); + // Formula from Agrawal06iros, equation (14) + // simplified with Mathematica, and multiplying in T to avoid matrix math + double Tan = tan(0.5 * t); + Vector3 WT = W * T; + Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT); + Vector6 log; + log << w, u; + return log; } +} + +/* ************************************************************************* */ +Pose3 Pose3::retractFirstOrder(const Vector& xi) const { + Vector3 omega(sub(xi, 0, 3)); + Point3 v(sub(xi, 3, 6)); + Rot3 R = R_.retract(omega); // R is done exactly + Point3 t = t_ + R_ * v; // First order t approximation + return Pose3(R, t); +} + +/* ************************************************************************* */ +// Different versions of retract +Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { + if (mode == Pose3::EXPMAP) { + // Lie group exponential map, traces out geodesic + return compose(Expmap(xi)); + } else if (mode == Pose3::FIRST_ORDER) { + // First order + return retractFirstOrder(xi); + } else { + // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently + // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation + assert(false); + exit(1); + } +} + +/* ************************************************************************* */ +// different versions of localCoordinates +Vector6 Pose3::localCoordinates(const Pose3& T, + Pose3::CoordinatesMode mode) const { + if (mode == Pose3::EXPMAP) { + // Lie group logarithm map, exact inverse of exponential map + return Logmap(between(T)); + } else if (mode == Pose3::FIRST_ORDER) { + // R is always done exactly in all three retract versions below + Vector3 omega = R_.localCoordinates(T.rotation()); + + // Incorrect version + // Independently computes the logmap of the translation and rotation + // Vector v = t_.localCoordinates(T.translation()); + + // Correct first order t inverse + Point3 d = R_.unrotate(T.translation() - t_); + + // TODO: correct second order t inverse + Vector6 local; + local << omega(0), omega(1), omega(2), d.x(), d.y(), d.z(); + return local; + } else { + assert(false); + exit(1); + } +} + +/* ************************************************************************* */ +Matrix4 Pose3::matrix() const { + const Matrix3 R = R_.matrix(); + const Vector3 T = t_.vector(); + Eigen::Matrix A14; + A14 << 0.0, 0.0, 0.0, 1.0; + Matrix4 mat; + mat << R, T, A14; + return mat; +} + +/* ************************************************************************* */ +Pose3 Pose3::transform_to(const Pose3& pose) const { + Rot3 cRv = R_ * Rot3(pose.R_.inverse()); + Point3 t = pose.transform_to(t_); + return Pose3(cRv, t); +} + +/* ************************************************************************* */ +Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, + boost::optional Dpoint) const { + if (Dpose) { + const Matrix R = R_.matrix(); + Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); + Dpose->resize(3, 6); + (*Dpose) << DR, R; + } + if (Dpoint) + *Dpoint = R_.matrix(); + return R_ * p + t_; +} + +/* ************************************************************************* */ +Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, + boost::optional Dpoint) const { + const Point3 result = R_.unrotate(p - t_); + if (Dpose) { + const Point3& q = result; + Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); + Dpose->resize(3, 6); + (*Dpose) << DR, _I3; + } + if (Dpoint) + *Dpoint = R_.transpose(); + return result; +} + +/* ************************************************************************* */ +Pose3 Pose3::compose(const Pose3& p2, boost::optional H1, + boost::optional H2) const { + if (H1) + *H1 = p2.inverse().AdjointMap(); + if (H2) + *H2 = I6; + return (*this) * p2; +} + +/* ************************************************************************* */ +Pose3 Pose3::inverse(boost::optional H1) const { + if (H1) + *H1 = -AdjointMap(); + Rot3 Rt = R_.inverse(); + return Pose3(Rt, Rt * (-t_)); +} + +/* ************************************************************************* */ +// between = compose(p2,inverse(p1)); +Pose3 Pose3::between(const Pose3& p2, boost::optional H1, + boost::optional H2) const { + Pose3 result = inverse() * p2; + if (H1) + *H1 = -result.inverse().AdjointMap(); + if (H2) + *H2 = I6; + return result; +} + +/* ************************************************************************* */ +double Pose3::range(const Point3& point, boost::optional H1, + boost::optional H2) const { + if (!H1 && !H2) + return transform_to(point).norm(); + Point3 d = transform_to(point, H1, H2); + double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt( + d2); + Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n); + if (H1) + *H1 = D_result_d * (*H1); + if (H2) + *H2 = D_result_d * (*H2); + return n; +} + +/* ************************************************************************* */ +double Pose3::range(const Pose3& point, boost::optional H1, + boost::optional H2) const { + double r = range(point.translation(), H1, H2); + if (H2) { + Matrix H2_ = *H2 * point.rotation().matrix(); + *H2 = zeros(1, 6); + insertSub(*H2, H2_, 0, 3); + } + return r; +} + +/* ************************************************************************* */ +boost::optional align(const vector& pairs) { + const size_t n = pairs.size(); + if (n < 3) + return boost::none; // we need at least three pairs + + // calculate centroids + Vector cp = zero(3), cq = zero(3); + BOOST_FOREACH(const Point3Pair& pair, pairs){ + cp += pair.first.vector(); + cq += pair.second.vector(); +} + double f = 1.0 / n; + cp *= f; + cq *= f; + + // Add to form H matrix + Matrix H = zeros(3, 3); + BOOST_FOREACH(const Point3Pair& pair, pairs){ + Vector dp = pair.first.vector() - cp; + Vector dq = pair.second.vector() - cq; + H += dp * dq.transpose(); +} + +// Compute SVD + Matrix U, V; + Vector S; + svd(H, U, S, V); + + // Recover transform with correction from Eggert97machinevisionandapplications + Matrix UVtranspose = U * V.transpose(); + Matrix detWeighting = eye(3, 3); + detWeighting(2, 2) = UVtranspose.determinant(); + Rot3 R(Matrix(V * detWeighting * U.transpose())); + Point3 t = Point3(cq) - R * Point3(cp); + return Pose3(R, t); +} + +/* ************************************************************************* */ +std::ostream &operator<<(std::ostream &os, const Pose3& pose) { + os << pose.rotation() << "\n" << pose.translation() << endl; + return os; +} } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 4556f2200..825389243 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -15,7 +15,6 @@ */ // \callgraph - #pragma once #include @@ -32,111 +31,124 @@ namespace gtsam { - class Pose2; // forward declare +class Pose2; +// forward declare + +/** + * A 3D pose (R,t) : (Rot3,Point3) + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Pose3: public DerivedValue { +public: + static const size_t dimension = 6; + + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Translation; + +private: + + Rot3 R_; ///< Rotation gRp, between global and pose frame + Point3 t_; ///< Translation gTp, from global origin to pose frame origin + +public: + + /// @name Standard Constructors + /// @{ + + /** Default constructor is origin */ + Pose3() { + } + + /** Copy constructor */ + Pose3(const Pose3& pose) : + R_(pose.R_), t_(pose.t_) { + } + + /** Construct from R,t */ + Pose3(const Rot3& R, const Point3& t) : + R_(R), t_(t) { + } + + /** Construct from Pose2 */ + explicit Pose3(const Pose2& pose2); + + /** Constructor from 4*4 matrix */ + Pose3(const Matrix &T) : + R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1), + T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) { + } + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + void print(const std::string& s = "") const; + + /// assert equality up to a tolerance + bool equals(const Pose3& pose, double tol = 1e-9) const; + + /// @} + /// @name Group + /// @{ + + /// identity for group operation + static Pose3 identity() { + return Pose3(); + } + + /// inverse transformation with derivatives + Pose3 inverse(boost::optional H1 = boost::none) const; + + ///compose this transformation onto another (first *this and then p2) + Pose3 compose(const Pose3& p2, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const; + + /// compose syntactic sugar + Pose3 operator*(const Pose3& T) const { + return Pose3(R_ * T.R_, t_ + R_ * T.t_); + } /** - * A 3D pose (R,t) : (Rot3,Point3) - * @addtogroup geometry - * \nosubgrouping + * Return relative pose between p1 and p2, in p1 coordinate frame + * as well as optionally the derivatives */ - class GTSAM_EXPORT Pose3 : public DerivedValue { - public: - static const size_t dimension = 6; + Pose3 between(const Pose3& p2, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const; - /** Pose Concept requirements */ - typedef Rot3 Rotation; - typedef Point3 Translation; + /// @} + /// @name Manifold + /// @{ - private: - Rot3 R_; - Point3 t_; + /** Enum to indicate which method should be used in Pose3::retract() and + * Pose3::localCoordinates() + */ + enum CoordinatesMode { + EXPMAP, ///< The correct exponential map, computationally expensive. + FIRST_ORDER ///< A fast first-order approximation to the exponential map. + }; - public: + /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes + static size_t Dim() { + return dimension; + } - /// @name Standard Constructors - /// @{ + /// Dimensionality of the tangent space = 6 DOF + size_t dim() const { + return dimension; + } - /** Default constructor is origin */ - Pose3() {} + /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map + Pose3 retractFirstOrder(const Vector& d) const; - /** Copy constructor */ - Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {} + /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose + Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = + POSE3_DEFAULT_COORDINATES_MODE) const; - /** Construct from R,t */ - Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {} - - /** Construct from Pose2 */ - explicit Pose3(const Pose2& pose2); - - /** Constructor from 4*4 matrix */ - Pose3(const Matrix &T) : - R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), - T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} - - /// @} - /// @name Testable - /// @{ - - /// print with optional string - void print(const std::string& s = "") const; - - /// assert equality up to a tolerance - bool equals(const Pose3& pose, double tol = 1e-9) const; - - /// @} - /// @name Group - /// @{ - - /// identity for group operation - static Pose3 identity() { return Pose3(); } - - /// inverse transformation with derivatives - Pose3 inverse(boost::optional H1=boost::none) const; - - ///compose this transformation onto another (first *this and then p2) - Pose3 compose(const Pose3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - - /// compose syntactic sugar - Pose3 operator*(const Pose3& T) const { - return Pose3(R_*T.R_, t_ + R_*T.t_); - } - - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - * as well as optionally the derivatives - */ - Pose3 between(const Pose3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - - /// @} - /// @name Manifold - /// @{ - - /** Enum to indicate which method should be used in Pose3::retract() and - * Pose3::localCoordinates() - */ - enum CoordinatesMode { - EXPMAP, ///< The correct exponential map, computationally expensive. - FIRST_ORDER ///< A fast first-order approximation to the exponential map. - }; - - /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes - static size_t Dim() { return dimension; } - - /// Dimensionality of the tangent space = 6 DOF - size_t dim() const { return dimension; } - - /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map - Pose3 retractFirstOrder(const Vector& d) const; - - /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose - Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const; - - /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose - Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const; + /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose + Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const; /// @} /// @name Lie Group @@ -207,7 +219,7 @@ namespace gtsam { * @return xihat, 4*4 element of Lie algebra that can be exponentiated */ static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) { - return Matrix_(4,4, + return (Matrix(4,4) << 0.,-wz, wy, vx, wz, 0.,-wx, vy, -wy, wx, 0., vz, @@ -218,16 +230,28 @@ namespace gtsam { /// @name Group Action on Point3 /// @{ - /** receives the point in Pose coordinates and transforms it to world coordinates */ + /** + * @brief takes point in Pose coordinates and transforms it to world coordinates + * @param p point in Pose coordinates + * @param Dpose optional 3*6 Jacobian wrpt this pose + * @param Dpoint optional 3*3 Jacobian wrpt point + * @return point in world coordinates + */ Point3 transform_from(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + boost::optional Dpose=boost::none, boost::optional Dpoint=boost::none) const; /** syntactic sugar for transform_from */ inline Point3 operator*(const Point3& p) const { return transform_from(p); } - /** receives the point in world coordinates and transforms it to Pose coordinates */ + /** + * @brief takes point in world coordinates and transforms it to Pose coordinates + * @param p point in world coordinates + * @param Dpose optional 3*6 Jacobian wrpt this pose + * @param Dpoint optional 3*3 Jacobian wrpt point + * @return point in Pose coordinates + */ Point3 transform_to(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + boost::optional Dpose=boost::none, boost::optional Dpoint=boost::none) const; /// @} /// @name Standard Interface @@ -305,7 +329,7 @@ namespace gtsam { } /// @} - }; // Pose3 class + };// Pose3 class /** * wedge for Pose3: @@ -314,16 +338,16 @@ namespace gtsam { * v = 3D velocity * @return xihat, 4*4 element of Lie algebra that can be exponentiated */ - template <> - inline Matrix wedge(const Vector& xi) { - return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); - } +template<> +inline Matrix wedge(const Vector& xi) { + return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); +} - /** - * Calculate pose between a vector of 3D point correspondences (p,q) - * where q = Pose3::transform_from(p) = t + R*p - */ - typedef std::pair Point3Pair; - GTSAM_EXPORT boost::optional align(const std::vector& pairs); +/** + * Calculate pose between a vector of 3D point correspondences (p,q) + * where q = Pose3::transform_from(p) = t + R*p + */ +typedef std::pair Point3Pair; +GTSAM_EXPORT boost::optional align(const std::vector& pairs); } // namespace gtsam diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 73d45d1a6..27f6f9cd8 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -65,12 +65,12 @@ Rot2& Rot2::normalize() { /* ************************************************************************* */ Matrix Rot2::matrix() const { - return Matrix_(2, 2, c_, -s_, s_, c_); + return (Matrix(2, 2) << c_, -s_, s_, c_); } /* ************************************************************************* */ Matrix Rot2::transpose() const { - return Matrix_(2, 2, c_, s_, -s_, c_); + return (Matrix(2, 2) << c_, s_, -s_, c_); } /* ************************************************************************* */ @@ -78,7 +78,7 @@ Matrix Rot2::transpose() const { Point2 Rot2::rotate(const Point2& p, boost::optional H1, boost::optional H2) const { const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y()); - if (H1) *H1 = Matrix_(2, 1, -q.y(), q.x()); + if (H1) *H1 = (Matrix(2, 1) << -q.y(), q.x()); if (H2) *H2 = matrix(); return q; } @@ -88,7 +88,7 @@ Point2 Rot2::rotate(const Point2& p, boost::optional H1, Point2 Rot2::unrotate(const Point2& p, boost::optional H1, boost::optional H2) const { const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y()); - if (H1) *H1 = Matrix_(2, 1, q.y(), -q.x()); // R_{pi/2}q + if (H1) *H1 = (Matrix(2, 1) << q.y(), -q.x()); // R_{pi/2}q if (H2) *H2 = transpose(); return q; } @@ -97,10 +97,10 @@ Point2 Rot2::unrotate(const Point2& p, Rot2 Rot2::relativeBearing(const Point2& d, boost::optional H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); if(fabs(n) > 1e-5) { - if (H) *H = Matrix_(1, 2, -y / d2, x / d2); + if (H) *H = (Matrix(1, 2) << -y / d2, x / d2); return Rot2::fromCosSin(x / n, y / n); } else { - if (H) *H = Matrix_(1,2, 0.0, 0.0); + if (H) *H = (Matrix(1, 2) << 0.0, 0.0); return Rot2(); } } diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp new file mode 100644 index 000000000..0a3b4fd9f --- /dev/null +++ b/gtsam/geometry/Rot3.cpp @@ -0,0 +1,188 @@ +/* ---------------------------------------------------------------------------- + + * 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 Rot3.cpp + * @brief Rotation, common code between Rotation matrix and Quaternion + * @author Alireza Fathi + * @author Christian Potthast + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +static const Matrix3 I3 = Matrix3::Identity(); + +/* ************************************************************************* */ +void Rot3::print(const std::string& s) const { + gtsam::print((Matrix)matrix(), s); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Point3& w, double theta) { + return rodriguez((Vector)w.vector(),theta); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Sphere2& w, double theta) { + return rodriguez(w.point3(),theta); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Vector& w) { + double t = w.norm(); + if (t < 1e-10) return Rot3(); + return rodriguez(w/t, t); +} + +/* ************************************************************************* */ +bool Rot3::equals(const Rot3 & R, double tol) const { + return equal_with_abs_tol(matrix(), R.matrix(), tol); +} + +/* ************************************************************************* */ +Point3 Rot3::operator*(const Point3& p) const { + return rotate(p); +} + +/* ************************************************************************* */ +Sphere2 Rot3::rotate(const Sphere2& p, + boost::optional HR, boost::optional Hp) const { + Sphere2 q = rotate(p.point3(Hp)); + if (Hp) + (*Hp) = q.basis().transpose() * matrix() * (*Hp); + if (HR) + (*HR) = -q.basis().transpose() * matrix() * p.skew(); + return q; +} + +/* ************************************************************************* */ +Sphere2 Rot3::operator*(const Sphere2& p) const { + return rotate(p); +} + +/* ************************************************************************* */ +// see doc/math.lyx, SO(3) section +Point3 Rot3::unrotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + const Matrix Rt(transpose()); + Point3 q(Rt*p.vector()); // q = Rt*p + if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); + if (H2) *H2 = Rt; + return q; +} + +/* ************************************************************************* */ +/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) +Matrix3 Rot3::dexpL(const Vector3& v) { + if(zero(v)) return eye(3); + Matrix x = skewSymmetric(v); + Matrix x2 = x*x; + double theta = v.norm(), vi = theta/2.0; + double s1 = sin(vi)/vi; + double s2 = (theta - sin(theta))/(theta*theta*theta); + Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2; + return res; +} + +/* ************************************************************************* */ +/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) +Matrix3 Rot3::dexpInvL(const Vector3& v) { + if(zero(v)) return eye(3); + Matrix x = skewSymmetric(v); + Matrix x2 = x*x; + double theta = v.norm(), vi = theta/2.0; + double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); + Matrix res = eye(3) + 0.5*x - s2*x2; + return res; +} + + +/* ************************************************************************* */ +Point3 Rot3::column(int index) const{ + if(index == 3) + return r3(); + else if(index == 2) + return r2(); + else if(index == 1) + return r1(); // default returns r1 + else + throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3"); +} + +/* ************************************************************************* */ +Vector3 Rot3::xyz() const { + Matrix I;Vector3 q; + boost::tie(I,q)=RQ(matrix()); + return q; +} + +/* ************************************************************************* */ +Vector3 Rot3::ypr() const { + Vector3 q = xyz(); + return Vector3(q(2),q(1),q(0)); +} + +/* ************************************************************************* */ +Vector3 Rot3::rpy() const { + return xyz(); +} + +/* ************************************************************************* */ +Vector Rot3::quaternion() const { + Quaternion q = toQuaternion(); + Vector v(4); + v(0) = q.w(); + v(1) = q.x(); + v(2) = q.y(); + v(3) = q.z(); + return v; +} + +/* ************************************************************************* */ +pair RQ(const Matrix3& A) { + + double x = -atan2(-A(2, 1), A(2, 2)); + Rot3 Qx = Rot3::Rx(-x); + Matrix3 B = A * Qx.matrix(); + + double y = -atan2(B(2, 0), B(2, 2)); + Rot3 Qy = Rot3::Ry(-y); + Matrix3 C = B * Qy.matrix(); + + double z = -atan2(-C(1, 0), C(1, 1)); + Rot3 Qz = Rot3::Rz(-z); + Matrix3 R = C * Qz.matrix(); + + Vector xyz = Vector3(x, y, z); + return make_pair(R, xyz); +} + +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Rot3& R) { + os << "\n"; + os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n"; + os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n"; + os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n"; + return os; +} + +/* ************************************************************************* */ + +} // namespace gtsam + diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 53895d7bb..5bb382a3e 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -42,6 +42,7 @@ #include #include #include +#include namespace gtsam { @@ -149,7 +150,10 @@ namespace gtsam { static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);} /** Create from Quaternion coefficients */ - static Rot3 quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3(q); } + static Rot3 quaternion(double w, double x, double y, double z) { + Quaternion q(w, x, y, z); + return Rot3(q); + } /** * Rodriguez' formula to compute an incremental rotation matrix @@ -159,6 +163,22 @@ namespace gtsam { */ static Rot3 rodriguez(const Vector& w, double theta); + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param w is the rotation axis, unit length + * @param theta rotation angle + * @return incremental rotation matrix + */ + static Rot3 rodriguez(const Point3& w, double theta); + + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param w is the rotation axis + * @param theta rotation angle + * @return incremental rotation matrix + */ + static Rot3 rodriguez(const Sphere2& w, double theta); + /** * Rodriguez' formula to compute an incremental rotation matrix * @param v a vector of incremental roll,pitch,yaw @@ -293,6 +313,17 @@ namespace gtsam { Point3 unrotate(const Point3& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; + /// @} + /// @name Group Action on Sphere2 + /// @{ + + /// rotate 3D direction from rotated coordinate frame to world frame + Sphere2 rotate(const Sphere2& p, boost::optional HR = boost::none, + boost::optional Hp = boost::none) const; + + /// rotate 3D direction from rotated coordinate frame to world frame + Sphere2 operator*(const Sphere2& p) const; + /// @} /// @name Standard Interface /// @{ @@ -303,11 +334,12 @@ namespace gtsam { /** return 3*3 transpose (inverse) rotation matrix */ Matrix3 transpose() const; - /** returns column vector specified by index */ + /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; - Point3 r1() const; - Point3 r2() const; - Point3 r3() const; + + Point3 r1() const; ///< first column + Point3 r2() const; ///< second column + Point3 r3() const; ///< third column /** * Use RQ to calculate xyz angle representation diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 36f4ac258..03e7c0e0d 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -69,11 +69,6 @@ Rot3::Rot3(const Matrix& R) { /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} -/* ************************************************************************* */ -void Rot3::print(const std::string& s) const { - gtsam::print((Matrix)matrix(), s); -} - /* ************************************************************************* */ Rot3 Rot3::Rx(double t) { double st = sin(t), ct = cos(t); @@ -148,18 +143,6 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { -swy + C02, swx + C12, c + C22); } -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector& w) { - double t = w.norm(); - if (t < 1e-10) return Rot3(); - return rodriguez(w/t, t); -} - -/* ************************************************************************* */ -bool Rot3::equals(const Rot3 & R, double tol) const { - return equal_with_abs_tol(matrix(), R.matrix(), tol); -} - /* ************************************************************************* */ Rot3 Rot3::compose (const Rot3& R2, boost::optional H1, boost::optional H2) const { @@ -169,7 +152,9 @@ Rot3 Rot3::compose (const Rot3& R2, } /* ************************************************************************* */ -Point3 Rot3::operator*(const Point3& p) const { return rotate(p); } +Rot3 Rot3::operator*(const Rot3& R2) const { + return Rot3(Matrix3(rot_*R2.rot_)); +} /* ************************************************************************* */ Rot3 Rot3::inverse(boost::optional H1) const { @@ -183,12 +168,6 @@ Rot3 Rot3::between (const Rot3& R2, if (H1) *H1 = -(R2.transpose()*rot_); if (H2) *H2 = I3; return Rot3(Matrix3(rot_.transpose()*R2.rot_)); - //return between_default(*this, R2); -} - -/* ************************************************************************* */ -Rot3 Rot3::operator*(const Rot3& R2) const { - return Rot3(Matrix3(rot_*R2.rot_)); } /* ************************************************************************* */ @@ -201,16 +180,6 @@ Point3 Rot3::rotate(const Point3& p, return Point3(rot_ * p.vector()); } -/* ************************************************************************* */ -// see doc/math.lyx, SO(3) section -Point3 Rot3::unrotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - Point3 q(rot_.transpose()*p.vector()); // q = Rt*p - if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); - if (H2) *H2 = transpose(); - return q; -} - /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation Vector3 Rot3::Logmap(const Rot3& R) { @@ -308,32 +277,6 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const } } -/* ************************************************************************* */ -/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s1 = sin(vi)/vi; - double s2 = (theta - sin(theta))/(theta*theta*theta); - Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2; - return res; -} - -/* ************************************************************************* */ -/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpInvL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); - Matrix res = eye(3) + 0.5*x - s2*x2; - return res; -} - - /* ************************************************************************* */ Matrix3 Rot3::matrix() const { return rot_; @@ -344,11 +287,6 @@ Matrix3 Rot3::transpose() const { return rot_.transpose(); } -/* ************************************************************************* */ -Point3 Rot3::column(int index) const{ - return Point3(rot_.col(index)); -} - /* ************************************************************************* */ Point3 Rot3::r1() const { return Point3(rot_.col(0)); } @@ -358,68 +296,11 @@ Point3 Rot3::r2() const { return Point3(rot_.col(1)); } /* ************************************************************************* */ Point3 Rot3::r3() const { return Point3(rot_.col(2)); } -/* ************************************************************************* */ -Vector3 Rot3::xyz() const { - Matrix3 I;Vector3 q; - boost::tie(I,q)=RQ(rot_); - return q; -} - -/* ************************************************************************* */ -Vector3 Rot3::ypr() const { - Vector3 q = xyz(); - return Vector3(q(2),q(1),q(0)); -} - -/* ************************************************************************* */ -Vector3 Rot3::rpy() const { - return xyz(); -} - /* ************************************************************************* */ Quaternion Rot3::toQuaternion() const { return Quaternion(rot_); } -/* ************************************************************************* */ -Vector Rot3::quaternion() const { - Quaternion q = toQuaternion(); - Vector v(4); - v(0) = q.w(); - v(1) = q.x(); - v(2) = q.y(); - v(3) = q.z(); - return v; -} - -/* ************************************************************************* */ -pair RQ(const Matrix3& A) { - - double x = -atan2(-A(2, 1), A(2, 2)); - Rot3 Qx = Rot3::Rx(-x); - Matrix3 B = A * Qx.matrix(); - - double y = -atan2(B(2, 0), B(2, 2)); - Rot3 Qy = Rot3::Ry(-y); - Matrix3 C = B * Qy.matrix(); - - double z = -atan2(-C(1, 0), C(1, 1)); - Rot3 Qz = Rot3::Rz(-z); - Matrix3 R = C * Qz.matrix(); - - Vector xyz = Vector3(x, y, z); - return make_pair(R, xyz); -} - -/* ************************************************************************* */ -ostream &operator<<(ostream &os, const Rot3& R) { - os << "\n"; - os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n"; - os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n"; - os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n"; - return os; -} - /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 8e0f46e92..4836e8680 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -87,18 +87,6 @@ namespace gtsam { Rot3 Rot3::rodriguez(const Vector& w, double theta) { return Quaternion(Eigen::AngleAxisd(theta, w)); } - /* ************************************************************************* */ - Rot3 Rot3::rodriguez(const Vector& w) { - double t = w.norm(); - if (t < 1e-10) return Rot3(); - return rodriguez(w/t, t); - } - - /* ************************************************************************* */ - bool Rot3::equals(const Rot3 & R, double tol) const { - return equal_with_abs_tol(matrix(), R.matrix(), tol); - } - /* ************************************************************************* */ Rot3 Rot3::compose(const Rot3& R2, boost::optional H1, boost::optional H2) const { @@ -108,9 +96,8 @@ namespace gtsam { } /* ************************************************************************* */ - Point3 Rot3::operator*(const Point3& p) const { - Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z()); - return Point3(r(0), r(1), r(2)); + Rot3 Rot3::operator*(const Rot3& R2) const { + return Rot3(quaternion_ * R2.quaternion_); } /* ************************************************************************* */ @@ -127,11 +114,6 @@ namespace gtsam { return between_default(*this, R2); } - /* ************************************************************************* */ - Rot3 Rot3::operator*(const Rot3& R2) const { - return Rot3(quaternion_ * R2.quaternion_); - } - /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, boost::optional H1, boost::optional H2) const { @@ -142,17 +124,6 @@ namespace gtsam { return Point3(r.x(), r.y(), r.z()); } - /* ************************************************************************* */ - // see doc/math.lyx, SO(3) section - Point3 Rot3::unrotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - const Matrix Rt(transpose()); - Point3 q(Rt*p.vector()); // q = Rt*p - if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); - if (H2) *H2 = Rt; - return q; - } - /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation Vector3 Rot3::Logmap(const Rot3& R) { @@ -175,22 +146,10 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix3 Rot3::matrix() const { return quaternion_.toRotationMatrix(); } + Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} /* ************************************************************************* */ - Matrix3 Rot3::transpose() const { return quaternion_.toRotationMatrix().transpose(); } - - /* ************************************************************************* */ - Point3 Rot3::column(int index) const{ - if(index == 3) - return r3(); - else if(index == 2) - return r2(); - else if(index == 1) - return r1(); // default returns r1 - else - throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3"); - } + Matrix3 Rot3::transpose() const {return quaternion_.toRotationMatrix().transpose();} /* ************************************************************************* */ Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); } @@ -201,55 +160,10 @@ namespace gtsam { /* ************************************************************************* */ Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); } - /* ************************************************************************* */ - Vector3 Rot3::xyz() const { - Matrix I;Vector3 q; - boost::tie(I,q)=RQ(matrix()); - return q; - } - - /* ************************************************************************* */ - Vector3 Rot3::ypr() const { - Vector3 q = xyz(); - return Vector3(q(2),q(1),q(0)); - } - - /* ************************************************************************* */ - Vector3 Rot3::rpy() const { - Vector3 q = xyz(); - return Vector3(q(0),q(1),q(2)); - } - /* ************************************************************************* */ Quaternion Rot3::toQuaternion() const { return quaternion_; } - /* ************************************************************************* */ - pair RQ(const Matrix3& A) { - - double x = -atan2(-A(2, 1), A(2, 2)); - Rot3 Qx = Rot3::Rx(-x); - Matrix3 B = A * Qx.matrix(); - - double y = -atan2(B(2, 0), B(2, 2)); - Rot3 Qy = Rot3::Ry(-y); - Matrix3 C = B * Qy.matrix(); - - double z = -atan2(-C(1, 0), C(1, 1)); - Rot3 Qz = Rot3::Rz(-z); - Matrix3 R = C * Qz.matrix(); - - Vector xyz = Vector3(x, y, z); - return make_pair(R, xyz); - } - - /* ************************************************************************* */ - ostream &operator<<(ostream &os, const Rot3& R) { - os << "\n"; - os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n"; - os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n"; - os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n"; - return os; - } + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp new file mode 100644 index 000000000..e2ee24cab --- /dev/null +++ b/gtsam/geometry/Sphere2.cpp @@ -0,0 +1,130 @@ +/* ---------------------------------------------------------------------------- + + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file Sphere2.h + * @date Feb 02, 2011 + * @author Can Erdogan + * @author Frank Dellaert + * @brief The Sphere2 class - basically a point on a unit sphere + */ + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +Matrix Sphere2::basis() const { + + // Return cached version if exists + if (B_.rows() == 3) + return B_; + + // Get the axis of rotation with the minimum projected length of the point + Point3 axis; + double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z()); + if ((mx <= my) && (mx <= mz)) + axis = Point3(1.0, 0.0, 0.0); + else if ((my <= mx) && (my <= mz)) + axis = Point3(0.0, 1.0, 0.0); + else if ((mz <= mx) && (mz <= my)) + axis = Point3(0.0, 0.0, 1.0); + else + assert(false); + + // Create the two basis vectors + Point3 b1 = p_.cross(axis); + b1 = b1 / b1.norm(); + Point3 b2 = p_.cross(b1); + b2 = b2 / b2.norm(); + + // Create the basis matrix + B_ = Matrix(3, 2); + B_ << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + return B_; +} + +/* ************************************************************************* */ +/// The print fuction +void Sphere2::print(const std::string& s) const { + cout << s << ":" << p_ << endl; +} + +/* ************************************************************************* */ +Matrix Sphere2::skew() const { + return skewSymmetric(p_.x(), p_.y(), p_.z()); +} + +/* ************************************************************************* */ +Vector Sphere2::error(const Sphere2& q, boost::optional H) const { + Matrix Bt = basis().transpose(); + Vector xi = Bt * q.p_.vector(); + if (H) + *H = Bt * q.basis(); + return xi; +} + +/* ************************************************************************* */ +double Sphere2::distance(const Sphere2& q, boost::optional H) const { + Vector xi = error(q,H); + double theta = xi.norm(); + if (H) + *H = (xi.transpose() / theta) * (*H); + return theta; +} + +/* ************************************************************************* */ +Sphere2 Sphere2::retract(const Vector& v) const { + + // Get the vector form of the point and the basis matrix + Vector p = Point3::Logmap(p_); + Matrix B = basis(); + + // Compute the 3D xi_hat vector + Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1); + Vector newPoint = p + xi_hat; + + // Project onto the manifold, i.e. the closest point on the circle to the new location; + // same as putting it onto the unit circle + Vector projected = newPoint / newPoint.norm(); + + return Sphere2(Point3::Expmap(projected)); +} + +/* ************************************************************************* */ +Vector Sphere2::localCoordinates(const Sphere2& y) const { + + // Make sure that the angle different between x and y is less than 90. Otherwise, + // we can project x + xi_hat from the tangent space at x to y. + double cosAngle = y.p_.dot(p_); + assert(cosAngle > 0.0 && "Can not retract from x to y."); + + // Get the basis matrix + Matrix B = basis(); + + // Create the vector forms of p and q (the Point3 of y). + Vector p = Point3::Logmap(p_); + Vector q = Point3::Logmap(y.p_); + + // Compute the basis coefficients [v0,v1] = (B'q)/(p'q). + 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)); + return result; +} +/* ************************************************************************* */ + +} diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h new file mode 100644 index 000000000..5d1bbd7b2 --- /dev/null +++ b/gtsam/geometry/Sphere2.h @@ -0,0 +1,119 @@ +/* ---------------------------------------------------------------------------- + + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file Sphere2.h + * @date Feb 02, 2011 + * @author Can Erdogan + * @author Frank Dellaert + * @brief Develop a Sphere2 class - basically a point on a unit sphere + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/// Represents a 3D point on a unit sphere. +class Sphere2: public DerivedValue { + +private: + + Point3 p_; ///< The location of the point on the unit sphere + mutable Matrix B_; ///< Cached basis + +public: + + /// @name Constructors + /// @{ + + /// Default constructor + Sphere2() : + p_(1.0, 0.0, 0.0) { + } + + /// Construct from point + Sphere2(const Point3& p) : + p_(p / p.norm()) { + } + + /// Construct from x,y,z + Sphere2(double x, double y, double z) : + p_(x, y, z) { + p_ = p_ / p_.norm(); + } + + /// @} + + /// @name Testable + /// @{ + + /// The print fuction + void print(const std::string& s = std::string()) const; + + /// The equals function with tolerance + bool equals(const Sphere2& s, double tol = 1e-9) const { + return p_.equals(s.p_, tol); + } + /// @} + + /// @name Other functionality + /// @{ + + /// Returns the local coordinate frame to tangent plane + Matrix basis() const; + + /// Return skew-symmetric associated with 3D point on unit sphere + Matrix skew() const; + + /// Return unit-norm Point3 + Point3 point3(boost::optional H = boost::none) const { + if (H) + *H = basis(); + return p_; + } + + /// Signed, vector-valued error between two directions + Vector error(const Sphere2& q, + boost::optional H = boost::none) const; + + /// Distance between two directions + double distance(const Sphere2& q, + boost::optional H = boost::none) const; + + /// @} + + /// @name Manifold + /// @{ + + /// Dimensionality of tangent space = 2 DOF + inline static size_t Dim() { + return 2; + } + + /// Dimensionality of tangent space = 2 DOF + inline size_t dim() const { + return 2; + } + + /// The retract function + Sphere2 retract(const Vector& v) const; + + /// The local coordinates function + Vector localCoordinates(const Sphere2& s) const; + + /// @} +}; + +} // namespace gtsam + diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index f44f68ac4..ed531a2bd 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -64,7 +64,7 @@ namespace gtsam { // optimized version, see StereoCamera.nb if (H1) { const double v1 = v/fy, v2 = fx*v1, dx=d*x; - *H1 = Matrix_(3, 6, + *H1 = (Matrix(3, 6) << uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL, uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR, fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v @@ -72,7 +72,7 @@ namespace gtsam { } if (H2) { const Matrix R(leftCamPose_.rotation().matrix()); - *H2 = d * Matrix_(3, 3, + *H2 = d * (Matrix(3, 3) << fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v @@ -90,7 +90,7 @@ namespace gtsam { double d = 1.0 / P.z(), d2 = d*d; const Cal3_S2Stereo& K = *K_; double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); - return Matrix_(3, 3, + return (Matrix(3, 3) << f_x*d, 0.0, -d2*f_x* P.x(), f_x*d, 0.0, -d2*f_x*(P.x() - b), 0.0, f_y*d, -d2*f_y* P.y() diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index acaad08d7..8ce2e49bf 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -147,6 +147,11 @@ namespace gtsam { return Point2(uL_, v_); } + /** convenient function to get a Point2 from the right image */ + inline Point2 right(){ + return Point2(uR_, v_); + } + private: /// @} diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index e354796fa..7cec17b34 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -25,38 +25,47 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Cal3Bundler) GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler) -static Cal3Bundler K(500, 1e-3, 1e-3); +static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000); static Point2 p(2,3); /* ************************************************************************* */ -TEST( Cal3Bundler, calibrate) +TEST( Cal3Bundler, uncalibrate) { Vector v = K.vector() ; double r = p.x()*p.x() + p.y()*p.y() ; double g = v[0]*(1+v[1]*r+v[2]*r*r) ; - Point2 q_hat (g*p.x(), g*p.y()) ; - Point2 q = K.uncalibrate(p); - CHECK(assert_equal(q,q_hat)); + Point2 expected (1000+g*p.x(), 2000+g*p.y()) ; + Point2 actual = K.uncalibrate(p); + CHECK(assert_equal(actual,expected)); } +TEST( Cal3Bundler, calibrate ) +{ + Point2 pn(0.5, 0.5); + Point2 pi = K.uncalibrate(pn); + Point2 pn_hat = K.calibrate(pi); + CHECK( pn.equals(pn_hat, 1e-5)); +} +/* ************************************************************************* */ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); } /* ************************************************************************* */ -TEST( Cal3Bundler, Duncalibrate1) +TEST( Cal3Bundler, Duncalibrate) { - Matrix computed; - K.uncalibrate(p, computed, boost::none); - Matrix numerical = numericalDerivative21(uncalibrate_, K, p); - CHECK(assert_equal(numerical,computed,1e-7)); -} - -/* ************************************************************************* */ -TEST( Cal3Bundler, Duncalibrate2) -{ - Matrix computed; K.uncalibrate(p, boost::none, computed); - Matrix numerical = numericalDerivative22(uncalibrate_, K, p); - CHECK(assert_equal(numerical,computed,1e-7)); + Matrix Dcal, Dp; + Point2 actual = K.uncalibrate(p, Dcal, Dp); + Point2 expected(2182, 3773); + CHECK(assert_equal(expected,actual,1e-7)); + Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p); + Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p); + CHECK(assert_equal(numerical1,Dcal,1e-7)); + CHECK(assert_equal(numerical2,Dp,1e-7)); + CHECK(assert_equal(numerical1,K.D2d_calibration(p),1e-7)); + CHECK(assert_equal(numerical2,K.D2d_intrinsic(p),1e-7)); + Matrix Dcombined(2,5); + Dcombined << Dp, Dcal; + CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7)); } /* ************************************************************************* */ @@ -65,6 +74,16 @@ TEST( Cal3Bundler, assert_equal) CHECK(assert_equal(K,K,1e-7)); } +/* ************************************************************************* */ +TEST( Cal3Bundler, retract) +{ + Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000); + Vector d(3); + d << 10, 1e-3, 1e-3; + Cal3Bundler actual = K.retract(d); + CHECK(assert_equal(expected,actual,1e-7)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 093103377..de5ca1ed1 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -27,7 +27,7 @@ using namespace gtsam; GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) -static const Pose3 pose1(Matrix_(3,3, +static const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp new file mode 100644 index 000000000..1a2965089 --- /dev/null +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -0,0 +1,70 @@ +/* + * @file testEssentialMatrix.cpp + * @brief Test EssentialMatrix class + * @author Frank Dellaert + * @date December 17, 2013 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(EssentialMatrix) +GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix) + +//************************************************************************* +// Create two cameras and corresponding essential matrix E +Rot3 aRb = Rot3::yaw(M_PI_2); +Point3 aTb(0.1, 0, 0); + +//************************************************************************* +TEST (EssentialMatrix, equality) { + EssentialMatrix actual(aRb, aTb), expected(aRb, aTb); + EXPECT(assert_equal(expected, actual)); +} + +//************************************************************************* +TEST (EssentialMatrix, retract1) { + EssentialMatrix expected(aRb.retract((Vector(3) << 0.1, 0, 0)), aTb); + EssentialMatrix trueE(aRb, aTb); + EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0)); + EXPECT(assert_equal(expected, actual)); +} + +//************************************************************************* +TEST (EssentialMatrix, retract2) { + EssentialMatrix expected(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0))); + EssentialMatrix trueE(aRb, aTb); + EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0)); + EXPECT(assert_equal(expected, actual)); +} + +//************************************************************************* +Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { return E.transform_to(point); } +TEST (EssentialMatrix, transform_to) { + // test with a more complicated EssentialMatrix + Rot3 aRb2 = Rot3::yaw(M_PI/3.0)*Rot3::pitch(M_PI_4)*Rot3::roll(M_PI/6.0); + Point3 aTb2(19.2, 3.7, 5.9); + EssentialMatrix E(aRb2, aTb2); + //EssentialMatrix E(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0))); + static Point3 P(0.2,0.7,-2); + Matrix actH1, actH2; + E.transform_to(P,actH1,actH2); + Matrix expH1 = numericalDerivative21(transform_to_, E,P), + expH2 = numericalDerivative22(transform_to_, E,P); + EXPECT(assert_equal(expH1, actH1, 1e-8)); + EXPECT(assert_equal(expH2, actH2, 1e-8)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp new file mode 100644 index 000000000..594db159b --- /dev/null +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -0,0 +1,210 @@ +/* ---------------------------------------------------------------------------- + + * 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 testPinholeCamera.cpp + * @author Frank Dellaert + * @brief test PinholeCamera class + */ + +#include +#include + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static const Cal3_S2 K(625, 625, 0, 0, 0); + +static const Pose3 pose1((Matrix)(Matrix(3,3) << + 1., 0., 0., + 0.,-1., 0., + 0., 0.,-1. + ), + Point3(0,0,0.5)); + +typedef PinholeCamera Camera; +static const Camera camera(pose1, K); + +static const Point3 point1(-0.08,-0.08, 0.0); +static const Point3 point2(-0.08, 0.08, 0.0); +static const Point3 point3( 0.08, 0.08, 0.0); +static const Point3 point4( 0.08,-0.08, 0.0); + +static const Point3 point1_inf(-0.16,-0.16, -1.0); +static const Point3 point2_inf(-0.16, 0.16, -1.0); +static const Point3 point3_inf( 0.16, 0.16, -1.0); +static const Point3 point4_inf( 0.16,-0.16, -1.0); + +/* ************************************************************************* */ +TEST( PinholeCamera, constructor) +{ + CHECK(assert_equal( camera.calibration(), K)); + CHECK(assert_equal( camera.pose(), pose1)); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, level2) +{ + // Create a level camera, looking in Y-direction + Pose2 pose2(0.4,0.3,M_PI/2.0); + Camera camera = Camera::Level(K, pose2, 0.1); + + // expected + Point3 x(1,0,0),y(0,0,-1),z(0,1,0); + Rot3 wRc(x,y,z); + Pose3 expected(wRc,Point3(0.4,0.3,0.1)); + CHECK(assert_equal( camera.pose(), expected)); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, lookat) +{ + // Create a level camera, looking in Y-direction + Point3 C(10.0,0.0,0.0); + Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); + + // expected + Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); + Pose3 expected(Rot3(xc,yc,zc),C); + CHECK(assert_equal( camera.pose(), expected)); + + Point3 C2(30.0,0.0,10.0); + Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + + Matrix R = camera2.pose().rotation().matrix(); + Matrix I = trans(R)*R; + CHECK(assert_equal(I, eye(3))); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, project) +{ + CHECK(assert_equal( camera.project(point1), Point2(-100, 100) )); + CHECK(assert_equal( camera.project(point2), Point2(-100, -100) )); + CHECK(assert_equal( camera.project(point3), Point2( 100, -100) )); + CHECK(assert_equal( camera.project(point4), Point2( 100, 100) )); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, backproject) +{ + CHECK(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); + CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); + CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); + CHECK(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, backprojectInfinity) +{ + CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); + CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); + CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); + CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, backproject2) +{ + Point3 origin; + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down + Camera camera(Pose3(rot, origin), K); + + Point3 actual = camera.backproject(Point2(), 1.); + Point3 expected(0., 1., 0.); + pair x = camera.projectSafe(expected); + + CHECK(assert_equal(expected, actual)); + CHECK(assert_equal(Point2(), x.first)); + CHECK(x.second); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, backprojectInfinity2) +{ + Point3 origin; + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down + Camera camera(Pose3(rot, origin), K); + + Point3 actual = camera.backprojectPointAtInfinity(Point2()); + Point3 expected(0., 1., 0.); + Point2 x = camera.projectPointAtInfinity(expected); + + CHECK(assert_equal(expected, actual)); + CHECK(assert_equal(Point2(), x)); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, backprojectInfinity3) +{ + Point3 origin; + Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity + Camera camera(Pose3(rot, origin), K); + + Point3 actual = camera.backprojectPointAtInfinity(Point2()); + Point3 expected(0., 0., 1.); + Point2 x = camera.projectPointAtInfinity(expected); + + CHECK(assert_equal(expected, actual)); + CHECK(assert_equal(Point2(), x)); +} + +/* ************************************************************************* */ +static Point2 project3(const Pose3& pose, const Point3& point, const Cal3_S2& cal) { + return Camera(pose,cal).project(point); +} + +/* ************************************************************************* */ +static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { + Point3 point(point2D.x(), point2D.y(), 1.0); + return Camera(pose,cal).projectPointAtInfinity(point); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, Dproject_point_pose) +{ + Matrix Dpose, Dpoint, Dcal; + Point2 result = camera.project(point1, Dpose, Dpoint, Dcal); + Matrix numerical_pose = numericalDerivative31(project3, pose1, point1, K); + Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K); + Matrix numerical_cal = numericalDerivative33(project3, pose1, point1, K); + CHECK(assert_equal(result, Point2(-100, 100) )); + CHECK(assert_equal(Dpose, numerical_pose, 1e-7)); + CHECK(assert_equal(Dpoint, numerical_point,1e-7)); + CHECK(assert_equal(Dcal, numerical_cal,1e-7)); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, Dproject_point_pose_Infinity) +{ + Matrix Dpose, Dpoint, Dcal; + Point2 point2D(-0.08,-0.08); + Point3 point3D(point1.x(), point1.y(), 1.0); + Point2 result = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); + Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point2D, K); + Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K); + Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point2D, K); + CHECK(assert_equal(Dpose, numerical_pose, 1e-7)); + CHECK(assert_equal(Dpoint, numerical_point,1e-7)); + CHECK(assert_equal(Dcal, numerical_cal,1e-7)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index b942d0eea..66ee5a387 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -46,8 +46,8 @@ TEST(Point2, Lie) { EXPECT(assert_equal(-eye(2), H1)); EXPECT(assert_equal(eye(2), H2)); - EXPECT(assert_equal(Point2(5,7), p1.retract(Vector_(2, 4.,5.)))); - EXPECT(assert_equal(Vector_(2, 3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point2(5,7), p1.retract((Vector(2) << 4., 5.)))); + EXPECT(assert_equal((Vector(2) << 3.,3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ @@ -101,11 +101,11 @@ TEST( Point2, norm ) { // exception, for (0,0) derivative is [Inf,Inf] but we return [1,1] actual = x1.norm(actualH); EXPECT_DOUBLES_EQUAL(0, actual, 1e-9); - expectedH = Matrix_(1, 2, 1.0, 1.0); + expectedH = (Matrix(1, 2) << 1.0, 1.0); EXPECT(assert_equal(expectedH,actualH)); actual = x2.norm(actualH); - EXPECT_DOUBLES_EQUAL(sqrt(2), actual, 1e-9); + EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9); expectedH = numericalDerivative11(norm_proxy, x2); EXPECT(assert_equal(expectedH,actualH)); } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 7be69f03b..ef4a3894c 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -39,7 +39,7 @@ TEST(Point3, Lie) { EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5,7,9), p1.retract(Vector_(3, 4.,5.,6.)))); + EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vector(3) << 4., 5., 6.)))); EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2))); } diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 779550324..a5646f647 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -68,7 +68,7 @@ TEST(Pose2, retract) { #else Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01)); #endif - Pose2 actual = pose.retract(Vector_(3, 0.01, -0.015, 0.99)); + Pose2 actual = pose.retract((Vector(3) << 0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -76,7 +76,7 @@ TEST(Pose2, retract) { TEST(Pose2, expmap) { Pose2 pose(M_PI/2.0, Point2(1, 2)); Pose2 expected(1.00811, 2.01528, 2.5608); - Pose2 actual = expmap_default(pose, Vector_(3, 0.01, -0.015, 0.99)); + Pose2 actual = expmap_default(pose, (Vector(3) << 0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -84,7 +84,7 @@ TEST(Pose2, expmap) { TEST(Pose2, expmap2) { Pose2 pose(M_PI/2.0, Point2(1, 2)); Pose2 expected(1.00811, 2.01528, 2.5608); - Pose2 actual = expmap_default(pose, Vector_(3, 0.01, -0.015, 0.99)); + Pose2 actual = expmap_default(pose, (Vector(3) << 0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -92,14 +92,14 @@ TEST(Pose2, expmap2) { TEST(Pose2, expmap3) { // do an actual series exponential map // see e.g. http://www.cis.upenn.edu/~cis610/cis610lie1.ps - Matrix A = Matrix_(3,3, + Matrix A = (Matrix(3,3) << 0.0, -0.99, 0.01, 0.99, 0.0, -0.015, 0.0, 0.0, 0.0); Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0; Matrix expected = eye(3) + A + A2 + A3 + A4; - Vector v = Vector_(3, 0.01, -0.015, 0.99); + Vector v = (Vector(3) << 0.01, -0.015, 0.99); Pose2 pose = Pose2::Expmap(v); Pose2 pose2(v); EXPECT(assert_equal(pose, pose2)); @@ -110,7 +110,7 @@ TEST(Pose2, expmap3) { /* ************************************************************************* */ TEST(Pose2, expmap0a) { Pose2 expected(0.0101345, -0.0149092, 0.018); - Pose2 actual = Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)); + Pose2 actual = Pose2::Expmap((Vector(3) << 0.01, -0.015, 0.018)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -118,7 +118,7 @@ TEST(Pose2, expmap0a) { TEST(Pose2, expmap0b) { // a quarter turn Pose2 expected(1.0, 1.0, M_PI/2); - Pose2 actual = Pose2::Expmap(Vector_(3, M_PI/2, 0.0, M_PI/2)); + Pose2 actual = Pose2::Expmap((Vector(3) << M_PI/2, 0.0, M_PI/2)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -126,7 +126,7 @@ TEST(Pose2, expmap0b) { TEST(Pose2, expmap0c) { // a half turn Pose2 expected(0.0, 2.0, M_PI); - Pose2 actual = Pose2::Expmap(Vector_(3, M_PI, 0.0, M_PI)); + Pose2 actual = Pose2::Expmap((Vector(3) << M_PI, 0.0, M_PI)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -134,7 +134,7 @@ TEST(Pose2, expmap0c) { TEST(Pose2, expmap0d) { // a full turn Pose2 expected(0, 0, 0); - Pose2 actual = Pose2::Expmap(Vector_(3, 2*M_PI, 0.0, 2*M_PI)); + Pose2 actual = Pose2::Expmap((Vector(3) << 2*M_PI, 0.0, 2*M_PI)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -143,7 +143,7 @@ TEST(Pose2, expmap0d) { // test case for screw motion in the plane namespace screw { double w=0.3; - Vector xi = Vector_(3, 0.0, w, w); + Vector xi = (Vector(3) << 0.0, w, w); Rot2 expectedR = Rot2::fromAngle(w); Point2 expectedT(-0.0446635, 0.29552); Pose2 expected(expectedR, expectedT); @@ -161,7 +161,7 @@ TEST(Pose3, expmap_c) TEST(Pose2, expmap_c_full) { double w=0.3; - Vector xi = Vector_(3, 0.0, w, w); + Vector xi = (Vector(3) << 0.0, w, w); Rot2 expectedR = Rot2::fromAngle(w); Point2 expectedT(-0.0446635, 0.29552); Pose2 expected(expectedR, expectedT); @@ -175,9 +175,9 @@ TEST(Pose2, logmap) { Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); #ifdef SLOW_BUT_CORRECT_EXPMAP - Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018); + Vector expected = (Vector(3) << 0.00986473, -0.0150896, 0.018); #else - Vector expected = Vector_(3, 0.01, -0.015, 0.018); + Vector expected = (Vector(3) << 0.01, -0.015, 0.018); #endif Vector actual = pose0.localCoordinates(pose); EXPECT(assert_equal(expected, actual, 1e-5)); @@ -187,7 +187,7 @@ TEST(Pose2, logmap) { TEST(Pose2, logmap_full) { Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); - Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018); + Vector expected = (Vector(3) << 0.00986473, -0.0150896, 0.018); Vector actual = logmap_default(pose0, pose); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -204,8 +204,8 @@ TEST( Pose2, transform_to ) // expected Point2 expected(2,2); - Matrix expectedH1 = Matrix_(2,3, -1.0, 0.0, 2.0, 0.0, -1.0, -2.0); - Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0); + Matrix expectedH1 = (Matrix(2,3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.0); + Matrix expectedH2 = (Matrix(2,2) << 0.0, 1.0, -1.0, 0.0); // actual Matrix actualH1, actualH2; @@ -236,8 +236,8 @@ TEST (Pose2, transform_from) Point2 expected(0., 2.); EXPECT(assert_equal(expected, actual)); - Matrix H1_expected = Matrix_(2, 3, 0., -1., -2., 1., 0., -1.); - Matrix H2_expected = Matrix_(2, 2, 0., -1., 1., 0.); + Matrix H1_expected = (Matrix(2, 3) << 0., -1., -2., 1., 0., -1.); + Matrix H2_expected = (Matrix(2, 2) << 0., -1., 1., 0.); Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt); EXPECT(assert_equal(H1_expected, H1)); @@ -261,7 +261,7 @@ TEST(Pose2, compose_a) Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5))); EXPECT(assert_equal(expected, actual)); - Matrix expectedH1 = Matrix_(3,3, + Matrix expectedH1 = (Matrix(3,3) << 0.0, 1.0, 0.0, -1.0, 0.0, 2.0, 0.0, 0.0, 1.0 @@ -348,14 +348,14 @@ TEST(Pose2, inverse ) namespace { /* ************************************************************************* */ Vector homogeneous(const Point2& p) { - return Vector_(3, p.x(), p.y(), 1.0); + return (Vector(3) << p.x(), p.y(), 1.0); } /* ************************************************************************* */ Matrix matrix(const Pose2& gTl) { Matrix gRl = gTl.r().matrix(); Point2 gt = gTl.t(); - return Matrix_(3, 3, + return (Matrix(3, 3) << gRl(0, 0), gRl(0, 1), gt.x(), gRl(1, 0), gRl(1, 1), gt.y(), 0.0, 0.0, 1.0); @@ -368,7 +368,7 @@ TEST( Pose2, matrix ) Point2 origin, t(1,2); Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y Matrix gMl = matrix(gTl); - EXPECT(assert_equal(Matrix_(3,3, + EXPECT(assert_equal((Matrix(3,3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0), @@ -376,7 +376,7 @@ TEST( Pose2, matrix ) Rot2 gR1 = gTl.r(); EXPECT(assert_equal(homogeneous(t),gMl*homogeneous(origin))); Point2 x_axis(1,0), y_axis(0,1); - EXPECT(assert_equal(Matrix_(2,2, + EXPECT(assert_equal((Matrix(2,2) << 0.0, -1.0, 1.0, 0.0), gR1.matrix())); @@ -387,7 +387,7 @@ TEST( Pose2, matrix ) // check inverse pose Matrix lMg = matrix(gTl.inverse()); - EXPECT(assert_equal(Matrix_(3,3, + EXPECT(assert_equal((Matrix(3,3) << 0.0, 1.0,-2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0), @@ -421,7 +421,7 @@ TEST( Pose2, between ) 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 @@ -432,7 +432,7 @@ TEST( Pose2, between ) // 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 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index ce193a0e5..175a11ff1 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -104,7 +104,7 @@ TEST( Pose3, expmap_a_full2) TEST(Pose3, expmap_b) { Pose3 p1(Rot3(), Point3(100, 0, 0)); - Pose3 p2 = p1.retract(Vector_(6,0.0, 0.0, 0.1, 0.0, 0.0, 0.0)); + Pose3 p2 = p1.retract((Vector(6) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0)); Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0)); EXPECT(assert_equal(expected, p2,1e-2)); } @@ -113,7 +113,7 @@ TEST(Pose3, expmap_b) // test case for screw motion in the plane namespace screw { double a=0.3, c=cos(a), s=sin(a), w=0.3; - Vector xi = Vector_(6, 0.0, 0.0, w, w, 0.0, 1.0); + Vector xi = (Vector(6) << 0.0, 0.0, w, w, 0.0, 1.0); Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); Point3 expectedT(0.29552, 0.0446635, 1); Pose3 expected(expectedR, expectedT); @@ -163,13 +163,13 @@ Pose3 Agrawal06iros(const Vector& xi) { TEST(Pose3, expmaps_galore_full) { Vector xi; Pose3 actual; - xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); + xi = (Vector(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6); actual = Pose3::Expmap(xi); EXPECT(assert_equal(expm(xi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6)); - xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6); + xi = (Vector(6) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6); for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { Vector txi = xi*theta; actual = Pose3::Expmap(txi); @@ -181,7 +181,7 @@ TEST(Pose3, expmaps_galore_full) } // Works with large v as well, but expm needs 10 iterations! - xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0); + xi = (Vector(6) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0); actual = Pose3::Expmap(xi); EXPECT(assert_equal(expm(xi,10), actual,1e-5)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); @@ -194,7 +194,7 @@ TEST(Pose3, Adjoint_compose_full) // To debug derivatives of compose, assert that // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 const Pose3& T1 = T; - Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); + Vector x = (Vector(6) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8); Pose3 expected = T1 * Pose3::Expmap(x) * T2; Vector y = T2.inverse().Adjoint(x); Pose3 actual = T1 * T2 * Pose3::Expmap(y); @@ -510,7 +510,7 @@ TEST(Pose3, subgroups) { // Frank - Below only works for correct "Agrawal06iros style expmap // lines in canonical coordinates correspond to Abelian subgroups in SE(3) - Vector d = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); + Vector d = (Vector(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6); // exp(-d)=inverse(exp(d)) EXPECT(assert_equal(Pose3::Expmap(-d),Pose3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -675,7 +675,7 @@ TEST(Pose3, align_2) { /// exp(xi) exp(y) = exp(xi + x) /// Hence, y = log (exp(-xi)*exp(xi+x)) -Vector xi = Vector_(6, 0.1, 0.2, 0.3, 1.0, 2.0, 3.0); +Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0); Vector testDerivExpmapInv(const LieVector& dxi) { Vector y = Pose3::Logmap(Pose3::Expmap(-xi)*Pose3::Expmap(xi+dxi)); @@ -723,8 +723,8 @@ Vector testDerivAdjointTranspose(const LieVector& xi, const LieVector& v) { } TEST( Pose3, adjointTranspose) { - Vector xi = Vector_(6, 0.01, 0.02, 0.03, 1.0, 2.0, 3.0); - Vector v = Vector_(6, 0.04, 0.05, 0.06, 4.0, 5.0, 6.0); + Vector xi = (Vector(6) << 0.01, 0.02, 0.03, 1.0, 2.0, 3.0); + Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0); Vector expected = testDerivAdjointTranspose(xi, v); Matrix actualH; diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 5874af9a0..7b80e8ee9 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -57,7 +57,7 @@ TEST( Rot3, constructor) /* ************************************************************************* */ TEST( Rot3, constructor2) { - Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.); + Matrix R = (Matrix(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.); Rot3 actual(R); Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33); CHECK(assert_equal(actual,expected)); @@ -101,7 +101,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { TEST( Rot3, rodriguez) { Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); - Vector w = Vector_(3, epsilon, 0., 0.); + Vector w = (Vector(3) << epsilon, 0., 0.); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } @@ -109,7 +109,7 @@ TEST( Rot3, rodriguez) /* ************************************************************************* */ TEST( Rot3, rodriguez2) { - Vector axis = Vector_(3,0.,1.,0.); // rotation around Y + Vector axis = (Vector(3) << 0., 1., 0.); // rotation around Y double angle = 3.14 / 4.0; Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 expected(0.707388, 0, 0.706825, @@ -121,7 +121,7 @@ TEST( Rot3, rodriguez2) /* ************************************************************************* */ TEST( Rot3, rodriguez3) { - Vector w = Vector_(3, 0.1, 0.2, 0.3); + Vector w = (Vector(3) << 0.1, 0.2, 0.3); Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); @@ -130,7 +130,7 @@ TEST( Rot3, rodriguez3) /* ************************************************************************* */ TEST( Rot3, rodriguez4) { - Vector axis = Vector_(3,0.,0.,1.); // rotation around Z + Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z double angle = M_PI/2.0; Rot3 actual = Rot3::rodriguez(axis, angle); double c=cos(angle),s=sin(angle); @@ -156,7 +156,7 @@ TEST(Rot3, log) Rot3 R; #define CHECK_OMEGA(X,Y,Z) \ - w = Vector_(3, (double)X, (double)Y, double(Z)); \ + w = (Vector(3) << (double)X, (double)Y, double(Z)); \ R = Rot3::rodriguez(w); \ EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12)); @@ -190,7 +190,7 @@ TEST(Rot3, log) // Check 360 degree rotations #define CHECK_OMEGA_ZERO(X,Y,Z) \ - w = Vector_(3, (double)X, (double)Y, double(Z)); \ + w = (Vector(3) << (double)X, (double)Y, double(Z)); \ R = Rot3::rodriguez(w); \ EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); @@ -217,7 +217,7 @@ TEST(Rot3, manifold_caley) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = Vector_(3, 0.1, 0.2, 0.3); + Vector d = (Vector(3) << 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -245,7 +245,7 @@ TEST(Rot3, manifold_slow_caley) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = Vector_(3, 0.1, 0.2, 0.3); + Vector d = (Vector(3) << 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -277,7 +277,7 @@ TEST(Rot3, manifold_expmap) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = Vector_(3, 0.1, 0.2, 0.3); + Vector d = (Vector(3) << 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -403,7 +403,7 @@ TEST( Rot3, between ) } /* ************************************************************************* */ -Vector w = Vector_(3, 0.1, 0.27, -0.2); +Vector w = (Vector(3) << 0.1, 0.27, -0.2); // Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes? // We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 @@ -473,7 +473,7 @@ TEST( Rot3, yaw_pitch_roll ) Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3); CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3))); - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),expected.ypr())); + CHECK(assert_equal((Vector)(Vector(3) << 0.1, 0.2, 0.3),expected.ypr())); } /* ************************************************************************* */ @@ -483,25 +483,25 @@ TEST( Rot3, RQ) Matrix actualK; Vector actual; boost::tie(actualK, actual) = RQ(R.matrix()); - Vector expected = Vector_(3, 0.14715, 0.385821, 0.231671); + Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671); CHECK(assert_equal(I3,actualK)); CHECK(assert_equal(expected,actual,1e-6)); // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] CHECK(assert_equal(expected,R.xyz(),1e-6)); - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); + CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); + CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal((Vector)(Vector(3) << 0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.0,0.1),Rot3::roll (0.1).ypr())); // Try RQ to recover calibration from 3*3 sub-block of projection matrix - Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); + Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); Matrix A = K * R.matrix(); boost::tie(actualK, actual) = RQ(A); CHECK(assert_equal(K,actualK)); @@ -510,11 +510,11 @@ TEST( Rot3, RQ) /* ************************************************************************* */ TEST( Rot3, expmapStability ) { - Vector w = Vector_(3, 78e-9, 5e-8, 97e-7); + Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7); double theta = w.norm(); double theta2 = theta*theta; Rot3 actualR = Rot3::Expmap(w); - Matrix W = Matrix_(3,3, 0.0, -w(2), w(1), + Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1), w(2), 0.0, -w(0), -w(1), w(0), 0.0 ); Matrix W2 = W*W; @@ -526,7 +526,7 @@ TEST( Rot3, expmapStability ) { /* ************************************************************************* */ TEST( Rot3, logmapStability ) { - Vector w = Vector_(3, 1e-8, 0.0, 0.0); + Vector w = (Vector(3) << 1e-8, 0.0, 0.0); Rot3 R = Rot3::Expmap(w); // double tr = R.r1().x()+R.r2().y()+R.r3().z(); // std::cout.precision(5000); @@ -541,13 +541,13 @@ TEST( Rot3, logmapStability ) { TEST(Rot3, quaternion) { // NOTE: This is also verifying the ability to convert Vector to Quaternion Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782); - Rot3 R1 = Rot3(Matrix_(3,3, + Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) << 0.271018623057411, 0.278786459830371, 0.921318086098018, 0.578529366719085, 0.717799701969298, -0.387385285854279, -0.769319620053772, 0.637998195662053, 0.033250932803219)); Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053); - Rot3 R2 = Rot3(Matrix_(3,3, + Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) << -0.207341903877828, 0.250149415542075, 0.945745528564780, 0.881304914479026, -0.371869043667957, 0.291573424846290, 0.424630407073532, 0.893945571198514, -0.143353873763946)); diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index d44b8f34c..5db99e4e3 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -52,7 +52,7 @@ TEST( Rot3, constructor) /* ************************************************************************* */ TEST( Rot3, constructor2) { - Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.); + Matrix R = (Matrix(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.); Rot3 actual(R); Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33); CHECK(assert_equal(actual,expected)); @@ -88,7 +88,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { TEST( Rot3, rodriguez) { Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); - Vector w = Vector_(3, epsilon, 0., 0.); + Vector w = (Vector(3) << epsilon, 0., 0.); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } @@ -96,7 +96,7 @@ TEST( Rot3, rodriguez) /* ************************************************************************* */ TEST( Rot3, rodriguez2) { - Vector axis = Vector_(3,0.,1.,0.); // rotation around Y + Vector axis = (Vector(3) << 0.,1.,0.); // rotation around Y double angle = 3.14 / 4.0; Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 expected(0.707388, 0, 0.706825, @@ -108,7 +108,7 @@ TEST( Rot3, rodriguez2) /* ************************************************************************* */ TEST( Rot3, rodriguez3) { - Vector w = Vector_(3, 0.1, 0.2, 0.3); + Vector w = (Vector(3) << 0.1, 0.2, 0.3); Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); @@ -117,7 +117,7 @@ TEST( Rot3, rodriguez3) /* ************************************************************************* */ TEST( Rot3, rodriguez4) { - Vector axis = Vector_(3,0.,0.,1.); // rotation around Z + Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z double angle = M_PI_2; Rot3 actual = Rot3::rodriguez(axis, angle); double c=cos(angle),s=sin(angle); @@ -138,35 +138,35 @@ TEST( Rot3, expmap) /* ************************************************************************* */ TEST(Rot3, log) { - Vector w1 = Vector_(3, 0.1, 0.0, 0.0); + Vector w1 = (Vector(3) << 0.1, 0.0, 0.0); Rot3 R1 = Rot3::rodriguez(w1); CHECK(assert_equal(w1, Rot3::Logmap(R1))); - Vector w2 = Vector_(3, 0.0, 0.1, 0.0); + Vector w2 = (Vector(3) << 0.0, 0.1, 0.0); Rot3 R2 = Rot3::rodriguez(w2); CHECK(assert_equal(w2, Rot3::Logmap(R2))); - Vector w3 = Vector_(3, 0.0, 0.0, 0.1); + Vector w3 = (Vector(3) << 0.0, 0.0, 0.1); Rot3 R3 = Rot3::rodriguez(w3); CHECK(assert_equal(w3, Rot3::Logmap(R3))); - Vector w = Vector_(3, 0.1, 0.4, 0.2); + Vector w = (Vector(3) << 0.1, 0.4, 0.2); Rot3 R = Rot3::rodriguez(w); CHECK(assert_equal(w, Rot3::Logmap(R))); - Vector w5 = Vector_(3, 0.0, 0.0, 0.0); + Vector w5 = (Vector(3) << 0.0, 0.0, 0.0); Rot3 R5 = Rot3::rodriguez(w5); CHECK(assert_equal(w5, Rot3::Logmap(R5))); - Vector w6 = Vector_(3, boost::math::constants::pi(), 0.0, 0.0); + Vector w6 = (Vector(3) << boost::math::constants::pi(), 0.0, 0.0); Rot3 R6 = Rot3::rodriguez(w6); CHECK(assert_equal(w6, Rot3::Logmap(R6))); - Vector w7 = Vector_(3, 0.0, boost::math::constants::pi(), 0.0); + Vector w7 = (Vector(3) << 0.0, boost::math::constants::pi(), 0.0); Rot3 R7 = Rot3::rodriguez(w7); CHECK(assert_equal(w7, Rot3::Logmap(R7))); - Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi()); + Vector w8 = (Vector(3) << 0.0, 0.0, boost::math::constants::pi()); Rot3 R8 = Rot3::rodriguez(w8); CHECK(assert_equal(w8, Rot3::Logmap(R8))); } @@ -190,7 +190,7 @@ TEST(Rot3, manifold) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = Vector_(3, 0.1, 0.2, 0.3); + Vector d = (Vector(3) << 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -298,7 +298,7 @@ TEST( Rot3, between ) Rot3 r1 = Rot3::Rz(M_PI/3.0); Rot3 r2 = Rot3::Rz(2.0*M_PI/3.0); - Matrix expectedr1 = Matrix_(3,3, + Matrix expectedr1 = (Matrix(3, 3) << 0.5, -sqrt(3.0)/2.0, 0.0, sqrt(3.0)/2.0, 0.5, 0.0, 0.0, 0.0, 1.0); @@ -381,25 +381,25 @@ TEST( Rot3, RQ) Matrix actualK; Vector actual; boost::tie(actualK, actual) = RQ(R.matrix()); - Vector expected = Vector_(3, 0.14715, 0.385821, 0.231671); + Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671); CHECK(assert_equal(eye(3),actualK)); CHECK(assert_equal(expected,actual,1e-6)); // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] CHECK(assert_equal(expected,R.xyz(),1e-6)); - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); + CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); + CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal((Vector)(Vector(3) <<0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(3) <<0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(3) <<0.0,0.0,0.1),Rot3::roll (0.1).ypr())); // Try RQ to recover calibration from 3*3 sub-block of projection matrix - Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); + Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); Matrix A = K * R.matrix(); boost::tie(actualK, actual) = RQ(A); CHECK(assert_equal(K,actualK)); @@ -408,11 +408,11 @@ TEST( Rot3, RQ) /* ************************************************************************* */ TEST( Rot3, expmapStability ) { - Vector w = Vector_(3, 78e-9, 5e-8, 97e-7); + Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7); double theta = w.norm(); double theta2 = theta*theta; Rot3 actualR = Rot3::Expmap(w); - Matrix W = Matrix_(3,3, 0.0, -w(2), w(1), + Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1), w(2), 0.0, -w(0), -w(1), w(0), 0.0 ); Matrix W2 = W*W; @@ -425,7 +425,7 @@ TEST( Rot3, expmapStability ) { // Does not work with Quaternions ///* ************************************************************************* */ //TEST( Rot3, logmapStability ) { -// Vector w = Vector_(3, 1e-8, 0.0, 0.0); +// Vector w = (Vector(3) << 1e-8, 0.0, 0.0); // Rot3 R = Rot3::Expmap(w); //// double tr = R.r1().x()+R.r2().y()+R.r3().z(); //// std::cout.precision(5000); @@ -440,13 +440,13 @@ TEST( Rot3, expmapStability ) { TEST(Rot3, quaternion) { // NOTE: This is also verifying the ability to convert Vector to Quaternion Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782); - Rot3 R1 = Rot3(Matrix_(3,3, + Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) << 0.271018623057411, 0.278786459830371, 0.921318086098018, 0.578529366719085, 0.717799701969298, -0.387385285854279, -0.769319620053772, 0.637998195662053, 0.033250932803219)); Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053); - Rot3 R2 = Rot3(Matrix_(3,3, + Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) << -0.207341903877828, 0.250149415542075, 0.945745528564780, 0.881304914479026, -0.371869043667957, 0.291573424846290, 0.424630407073532, 0.893945571198514, -0.143353873763946)); diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 3f6e4377f..9ced28dca 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -28,7 +28,7 @@ using namespace gtsam; static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose1(Matrix_(3,3, +static const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. @@ -144,7 +144,7 @@ TEST( SimpleCamera, simpleCamera) Point3 T(1000,2000,1500); SimpleCamera expected(Pose3(R.inverse(),T),K); // H&Z example, 2nd edition, page 163 - Matrix P = Matrix_(3,4, + Matrix P = (Matrix(3,4) << 3.53553e2, 3.39645e2, 2.77744e2, -1.44946e6, -1.03528e2, 2.33212e1, 4.59607e2, -6.32525e5, 7.07107e-1, -3.53553e-1,6.12372e-1, -9.18559e2); diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp new file mode 100644 index 000000000..af435bcea --- /dev/null +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -0,0 +1,246 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSphere2.cpp + * @date Feb 03, 2012 + * @author Can Erdogan + * @brief Tests the Sphere2 class + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace boost::assign; +using namespace gtsam; +using namespace std; + +GTSAM_CONCEPT_TESTABLE_INST(Sphere2) +GTSAM_CONCEPT_MANIFOLD_INST(Sphere2) + +//******************************************************************************* +Point3 point3_(const Sphere2& p) { + return p.point3(); +} +TEST(Sphere2, point3) { + vector ps; + ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0) + / sqrt(2); + Matrix actualH, expectedH; + BOOST_FOREACH(Point3 p,ps) { + Sphere2 s(p); + expectedH = numericalDerivative11(point3_, s); + EXPECT(assert_equal(p, s.point3(actualH), 1e-8)); + EXPECT(assert_equal(expectedH, actualH, 1e-9)); + } +} + +//******************************************************************************* +static Sphere2 rotate_(const Rot3& R, const Sphere2& p) { + return R * p; +} + +TEST(Sphere2, rotate) { + Rot3 R = Rot3::yaw(0.5); + Sphere2 p(1, 0, 0); + Sphere2 expected = Sphere2(R.column(1)); + Sphere2 actual = R * p; + EXPECT(assert_equal(expected, actual, 1e-8)); + Matrix actualH, expectedH; + // Use numerical derivatives to calculate the expected Jacobian + { + expectedH = numericalDerivative21(rotate_,R,p); + R.rotate(p, actualH, boost::none); + EXPECT(assert_equal(expectedH, actualH, 1e-9)); + } + { + expectedH = numericalDerivative22(rotate_,R,p); + R.rotate(p, boost::none, actualH); + EXPECT(assert_equal(expectedH, actualH, 1e-9)); + } +} + +//******************************************************************************* +TEST(Sphere2, error) { + Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // + r = p.retract((Vector(2) << 0.8, 0)); + EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8)); + EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5)); + EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5)); + + Matrix actual, expected; + // Use numerical derivatives to calculate the expected Jacobian + { + expected = numericalDerivative11( + boost::bind(&Sphere2::error, &p, _1, boost::none), q); + p.error(q, actual); + EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); + } + { + expected = numericalDerivative11( + boost::bind(&Sphere2::error, &p, _1, boost::none), r); + p.error(r, actual); + EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); + } +} + +//******************************************************************************* +TEST(Sphere2, distance) { + Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // + r = p.retract((Vector(2) << 0.8, 0)); + EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8); + EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8); + EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8); + + Matrix actual, expected; + // Use numerical derivatives to calculate the expected Jacobian + { + expected = numericalGradient( + boost::bind(&Sphere2::distance, &p, _1, boost::none), q); + p.distance(q, actual); + EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); + } + { + expected = numericalGradient( + boost::bind(&Sphere2::distance, &p, _1, boost::none), r); + p.distance(r, actual); + EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); + } +} + +//******************************************************************************* +TEST(Sphere2, localCoordinates0) { + Sphere2 p; + Vector expected = zero(2); + Vector actual = p.localCoordinates(p); + EXPECT(assert_equal(expected, actual, 1e-8)); +} + +//******************************************************************************* +TEST(Sphere2, basis) { + Sphere2 p; + Matrix expected(3, 2); + expected << 0, 0, 0, -1, 1, 0; + Matrix actual = p.basis(); + EXPECT(assert_equal(expected, actual, 1e-8)); +} + +//******************************************************************************* +TEST(Sphere2, retract) { + Sphere2 p; + Vector v(2); + v << 0.5, 0; + Sphere2 expected(Point3(1, 0, 0.5)); + Sphere2 actual = p.retract(v); + EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); +} + +//******************************************************************************* +/// Returns a random vector +inline static Vector randomVector(const Vector& minLimits, + const Vector& maxLimits) { + + // Get the number of dimensions and create the return vector + size_t numDims = dim(minLimits); + Vector vector = zero(numDims); + + // Create the random vector + for (size_t i = 0; i < numDims; i++) { + double range = maxLimits(i) - minLimits(i); + vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); + } + return vector; +} + +//******************************************************************************* +// Let x and y be two Sphere2's. +// The equality x.localCoordinates(x.retract(v)) == v should hold. +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); + for (size_t i = 0; i < numIterations; i++) { + + // Sleep for the random number generator (TODO?: Better create all of them first). + sleep(0); + + // Create the two Sphere2s. + // NOTE: You can not create two totally random Sphere2's because you cannot always compute + // between two any Sphere2's. (For instance, they might be at the different sides of the circle). + Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); +// Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); + Vector v12 = randomVector(minXiLimit, maxXiLimit); + Sphere2 s2 = s1.retract(v12); + + // Check if the local coordinates and retract return the same results. + Vector actual_v12 = s1.localCoordinates(s2); + EXPECT(assert_equal(v12, actual_v12, 1e-3)); + Sphere2 actual_s2 = s1.retract(actual_v12); + EXPECT(assert_equal(s2, actual_s2, 1e-3)); + } +} + +//******************************************************************************* +//TEST( Pose2, between ) +//{ +// // < +// // +// // ^ +// // +// // *--0--*--* +// Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y +// Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x +// +// Matrix actualH1,actualH2; +// Pose2 expected(M_PI/2.0, Point2(2,2)); +// Pose2 actual1 = gT1.between(gT2); +// Pose2 actual2 = gT1.between(gT2,actualH1,actualH2); +// EXPECT(assert_equal(expected,actual1)); +// EXPECT(assert_equal(expected,actual2)); +// +// Matrix expectedH1 = Matrix_(3,3, +// 0.0,-1.0,-2.0, +// 1.0, 0.0,-2.0, +// 0.0, 0.0,-1.0 +// ); +// Matrix numericalH1 = numericalDerivative21(testing::between, gT1, gT2); +// EXPECT(assert_equal(expectedH1,actualH1)); +// EXPECT(assert_equal(numericalH1,actualH1)); +// // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx +// EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1)); +// +// Matrix expectedH2 = Matrix_(3,3, +// 1.0, 0.0, 0.0, +// 0.0, 1.0, 0.0, +// 0.0, 0.0, 1.0 +// ); +// Matrix numericalH2 = numericalDerivative22(testing::between, gT1, gT2); +// EXPECT(assert_equal(expectedH2,actualH2)); +// EXPECT(assert_equal(numericalH2,actualH2)); +// +//} + +/* ************************************************************************* */ +int main() { + srand(time(NULL)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 5a96b1769..a7a82c9de 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -74,7 +74,7 @@ TEST( StereoCamera, project) /* ************************************************************************* */ -static Pose3 camera1(Matrix_(3,3, +static Pose3 camera1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index cdd0ce742..598febcc8 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -11,26 +11,65 @@ /** * @file testStereoPoint2.cpp - * * @brief Tests for the StereoPoint2 class * * @date Nov 4, 2011 * @author Alex Cunningham */ +#include +#include +#include #include -#include -#include - +using namespace std; using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) GTSAM_CONCEPT_LIE_INST(StereoPoint2) -/* ************************************************************************* */ -TEST(testStereoPoint2, test) { +/* ************************************************************************* */ +TEST(StereoPoint2, constructor) { + StereoPoint2 p1(1, 2, 3), p2 = p1; + EXPECT(assert_equal(p1, p2)); +} + +/* ************************************************************************* */ +TEST(StereoPoint2, Lie) { + StereoPoint2 p1(1, 2, 3), p2(4, 5, 6); + Matrix H1, H2; + + EXPECT(assert_equal(StereoPoint2(5,7,9), p1.compose(p2))); + + EXPECT(assert_equal(StereoPoint2(3,3,3), p1.between(p2))); + + EXPECT(assert_equal(StereoPoint2(5,7,9), p1.retract((Vector(3) << 4., 5., 6.)))); + EXPECT(assert_equal((Vector(3) << 3., 3., 3.), p1.localCoordinates(p2))); +} + +/* ************************************************************************* */ +TEST( StereoPoint2, expmap) { + Vector d(3); + d(0) = 1; + d(1) = -1; + d(2) = -3; + StereoPoint2 a(4, 5, 6), b = a.retract(d), c(5, 4, 3); + EXPECT(assert_equal(b,c)); +} + +/* ************************************************************************* */ +TEST( StereoPoint2, arithmetic) { + EXPECT(assert_equal( StereoPoint2(5,6,7), StereoPoint2(4,5,6)+StereoPoint2(1,1,1))); + EXPECT(assert_equal( StereoPoint2(3,4,5), StereoPoint2(4,5,6)-StereoPoint2(1,1,1))); +} + +/* ************************************************************************* */ +TEST(testStereoPoint2, left_right) { + StereoPoint2 p1(1,2,3); + + EXPECT(assert_equal( Point2(1,3), p1.point2())); + EXPECT(assert_equal( Point2(2,3), p1.right())); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/timeCalibratedCamera.cpp b/gtsam/geometry/tests/timeCalibratedCamera.cpp index 2a5ab9064..1833579d9 100644 --- a/gtsam/geometry/tests/timeCalibratedCamera.cpp +++ b/gtsam/geometry/tests/timeCalibratedCamera.cpp @@ -27,7 +27,7 @@ int main() { int n = 100000; - const Pose3 pose1(Matrix_(3,3, + const Pose3 pose1((Matrix)(Mat(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. @@ -37,43 +37,6 @@ int main() const CalibratedCamera camera(pose1); const Point3 point1(-0.08,-0.08, 0.0); - /** - * NOTE: because we only have combined derivative functions now, - * parts of this test are no longer useful. - */ - - // Aug 8, iMac 3.06GHz Core i3 - // 378870 calls/second - // 2.63943 musecs/call - // AFTER collapse: - // 1.57399e+06 calls/second - // 0.63533 musecs/call -// { -// Matrix computed; -// long timeLog = clock(); -// for(int i = 0; i < n; i++) -// computed = Dproject_pose(camera, point1); -// long timeLog2 = clock(); -// double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; -// cout << ((double)n/seconds) << " calls/second" << endl; -// cout << ((double)seconds*1000000/n) << " musecs/call" << endl; -// } - - // Aug 8, iMac 3.06GHz Core i3 - // AFTER collapse: - // 1.55383e+06 calls/second - // 0.64357 musecs/call -// { -// Matrix computed; -// long timeLog = clock(); -// for(int i = 0; i < n; i++) -// computed = Dproject_point(camera, point1); -// long timeLog2 = clock(); -// double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; -// cout << ((double)n/seconds) << " calls/second" << endl; -// cout << ((double)seconds*1000000/n) << " musecs/call" << endl; -// } - // Aug 8, iMac 3.06GHz Core i3 // 371153 calls/second // 2.69431 musecs/call diff --git a/gtsam/geometry/tests/timePinholeCamera.cpp b/gtsam/geometry/tests/timePinholeCamera.cpp new file mode 100644 index 000000000..de1fa1279 --- /dev/null +++ b/gtsam/geometry/tests/timePinholeCamera.cpp @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + + * 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 timePinholeCamera.cpp + * @brief time PinholeCamera derivatives + * @author Frank Dellaert + */ + +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +int main() +{ + int n = 1000000; + + const Pose3 pose1((Matrix)(Mat(3,3) << + 1., 0., 0., + 0.,-1., 0., + 0., 0.,-1. + ), + Point3(0,0,0.5)); + +// static Cal3_S2 K(500, 100, 0.1, 320, 240); +// static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); + static Cal3Bundler K(500, 1e-3, 2.0*1e-3); + const PinholeCamera camera(pose1,K); + const Point3 point1(-0.08,-0.08, 0.0); + + /** + * NOTE: because we only have combined derivative functions now, + * parts of this test are no longer useful. + */ + + // Oct 12 2013, iMac 3.06GHz Core i3 + // Original: 0.14737 musecs/call + // After collapse: 0.11469 musecs/call + // Cal3DS2: 0.14201 musecs/call + // After Cal3DS2 fix: 0.12231 musecs/call + // Cal3Bundler: 0.12000 musecs/call + // Cal3Bundler fix: 0.14637 musecs/call + { + long timeLog = clock(); + for(int i = 0; i < n; i++) + camera.project(point1); + long timeLog2 = clock(); + double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; + cout << ((double)n/seconds) << " calls/second" << endl; + cout << ((double)seconds*1000000/n) << " musecs/call" << endl; + } + + // Oct 12 2013, iMac 3.06GHz Core i3 + // Original: 3.8720 musecs/call + // After collapse: 2.6269 musecs/call + // Cal3DS2: 4.3330 musecs/call + // After Cal3DS2 fix: 3.2857 musecs/call + // Cal3Bundler: 2.6556 musecs/call + // Cal3Bundler fix: 2.1613 musecs/call + { + Matrix Dpose, Dpoint; + long timeLog = clock(); + for(int i = 0; i < n; i++) + camera.project(point1, Dpose, Dpoint); + long timeLog2 = clock(); + double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; + cout << ((double)n/seconds) << " calls/second" << endl; + cout << ((double)seconds*1000000/n) << " musecs/call" << endl; + } + + // Oct 12 2013, iMac 3.06GHz Core i3 + // Original: 4.0119 musecs/call + // After collapse: 2.5698 musecs/call + // Cal3DS2: 4.8325 musecs/call + // After Cal3DS2 fix: 3.4483 musecs/call + // Cal3Bundler: 2.5112 musecs/call + // Cal3Bundler fix: 2.0946 musecs/call + { + Matrix Dpose, Dpoint, Dcal; + long timeLog = clock(); + for(int i = 0; i < n; i++) + camera.project(point1, Dpose, Dpoint, Dcal); + long timeLog2 = clock(); + double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; + cout << ((double)n/seconds) << " calls/second" << endl; + cout << ((double)seconds*1000000/n) << " musecs/call" << endl; + } + + return 0; +} diff --git a/gtsam/geometry/tests/timePose2.cpp b/gtsam/geometry/tests/timePose2.cpp index 139601664..a8f2f7137 100644 --- a/gtsam/geometry/tests/timePose2.cpp +++ b/gtsam/geometry/tests/timePose2.cpp @@ -58,7 +58,7 @@ Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2, if (H1) { double dt1 = -s2 * x + c2 * y; double dt2 = -c2 * x - s2 * y; - *H1 = Matrix_(3,3, + *H1 = (Mat(3,3) << -c, -s, dt1, s, -c, dt2, 0.0, 0.0,-1.0); diff --git a/gtsam/geometry/tests/timePose3.cpp b/gtsam/geometry/tests/timePose3.cpp index a3bbdf8d9..9538ad4b4 100644 --- a/gtsam/geometry/tests/timePose3.cpp +++ b/gtsam/geometry/tests/timePose3.cpp @@ -37,8 +37,8 @@ int main() 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_(6,x,y,z,0.1,0.2,-0.1); - Pose3 T = Pose3::Expmap(Vector_(6,0.1,0.1,0.2,0.1, 0.4, 0.2)), T2 = T.retract(v); + Vector v = (Vec(6) << x, y, z, 0.1, 0.2, -0.1); + Pose3 T = Pose3::Expmap((Vec(6) << 0.1, 0.1, 0.2, 0.1, 0.4, 0.2)), T2 = T.retract(v); Matrix H1,H2; TEST(retract, T.retract(v)) diff --git a/gtsam/geometry/tests/timeStereoCamera.cpp b/gtsam/geometry/tests/timeStereoCamera.cpp index 6ff7a90c2..bfe4b5aaa 100644 --- a/gtsam/geometry/tests/timeStereoCamera.cpp +++ b/gtsam/geometry/tests/timeStereoCamera.cpp @@ -27,7 +27,7 @@ int main() { int n = 100000; - const Pose3 pose1(Matrix_(3,3, + const Pose3 pose1((Matrix)(Mat(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1.