12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged changes from the trunk back into geometry. This triggered Eigen merge, as well as Vector/Matrix in base. Next up: slam, unstable. 

git-svn-id: https://svn.cc.gatech.edu/borg/gtsam/branches/2.4@20422 898a188c-9671-0410-8e00-e3fd810bbb7f
release/4.3a0
Frank Dellaert 2013-12-21 19:55:07 +00:00 committed by Richard Roberts
parent faca0ecd04
commit bd3126f7b4
60 changed files with 3262 additions and 1837 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam/3rdparty/CMakeLists.txt vendored
View File

@ -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()

4
gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h vendored
View File

@ -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 <iostream>
namespace Eigen {
@ -60,7 +60,7 @@ template<> struct mkl_llt<EIGTYPE> \
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; \
} \
}; \

80
gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h vendored
View File

@ -25,10 +25,14 @@ namespace Eigen {
* \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished()
*/
template<typename XprType>
struct CommaInitializer
struct CommaInitializer :
public internal::dense_xpr_base<CommaInitializer<XprType> >::type
{
typedef typename XprType::Scalar Scalar;
typedef typename XprType::Index Index;
typedef typename internal::dense_xpr_base<CommaInitializer<XprType> >::type Base;
EIGEN_DENSE_PUBLIC_INTERFACE(CommaInitializer)
typedef typename internal::conditional<internal::must_nest_by_value<XprType>::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<int LoadMode>
inline const PacketScalar packet(Index row, Index col) const
{
return m_xpr.template packet<LoadMode>(row, col);
}
template<int LoadMode>
inline void writePacket(Index row, Index col, const PacketScalar& x)
{
m_xpr.const_cast_derived().template writePacket<LoadMode>(row, col, x);
}
template<int LoadMode>
inline const PacketScalar packet(Index index) const
{
return m_xpr.template packet<LoadMode>(index);
}
template<int LoadMode>
inline void writePacket(Index index, const PacketScalar& x)
{
m_xpr.const_cast_derived().template writePacket<LoadMode>(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<typename XprType>
struct traits<CommaInitializer<XprType> > : traits<XprType>
{
};
}
/** \anchor MatrixBaseCommaInitRef
* Convenient operator to set the coefficients of a matrix.

2
gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur_MKL.h vendored
View File

@ -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 {

2
gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur_MKL.h vendored
View File

@ -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 {

2
gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_MKL.h vendored
View File

@ -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 {

2
gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU_MKL.h vendored
View File

@ -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 {

8
gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h vendored
View File

@ -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<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynami
\
m_nonzero_pivots = 0; \
m_maxpivot = RealScalar(0);\
m_colsPermutation.resize(cols); \
m_colsPermutation.resize((int)cols); \
m_colsPermutation.indices().setZero(); \
\
lapack_int lda = m_qr.outerStride(), i; \
lapack_int lda = (lapack_int) m_qr.outerStride(), i; \
lapack_int matrix_order = MKLCOLROW; \
LAPACKE_##MKLPREFIX##geqp3( matrix_order, rows, cols, (MKLTYPE*)m_qr.data(), lda, (lapack_int*)m_colsPermutation.indices().data(), (MKLTYPE*)m_hCoeffs.data()); \
LAPACKE_##MKLPREFIX##geqp3( matrix_order, (lapack_int)rows, (lapack_int)cols, (MKLTYPE*)m_qr.data(), lda, (lapack_int*)m_colsPermutation.indices().data(), (MKLTYPE*)m_hCoeffs.data()); \
m_isInitialized = true; \
m_maxpivot=m_qr.diagonal().cwiseAbs().maxCoeff(); \
m_hCoeffs.adjointInPlace(); \

94
gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h vendored
View File

@ -251,56 +251,62 @@ void householder_qr_inplace_unblocked(MatrixQR& mat, HCoeffs& hCoeffs, typename
}
/** \internal */
template<typename MatrixQR, typename HCoeffs>
void householder_qr_inplace_blocked(MatrixQR& mat, HCoeffs& hCoeffs,
typename MatrixQR::Index maxBlockSize=32,
typename MatrixQR::Scalar* tempData = 0)
template<typename MatrixQR, typename HCoeffs,
typename MatrixQRScalar = typename MatrixQR::Scalar,
bool InnerStrideIsOne = (MatrixQR::InnerStrideAtCompileTime == 1 && HCoeffs::InnerStrideAtCompileTime == 1)>
struct householder_qr_inplace_blocked
{
typedef typename MatrixQR::Index Index;
typedef typename MatrixQR::Scalar Scalar;
typedef Block<MatrixQR,Dynamic,Dynamic> BlockType;
Index rows = mat.rows();
Index cols = mat.cols();
Index size = (std::min)(rows, cols);
typedef Matrix<Scalar,Dynamic,1,ColMajor,MatrixQR::MaxColsAtCompileTime,1> 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<MatrixQR,Dynamic,Dynamic> 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<HCoeffs,Dynamic,1> hCoeffsSegment = hCoeffs.segment(k,bs);
householder_qr_inplace_unblocked(A11_21, hCoeffsSegment, tempData);
if(tcols)
typedef Matrix<Scalar,Dynamic,1,ColMajor,MatrixQR::MaxColsAtCompileTime,1> 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<HCoeffs,Dynamic,1> 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<typename _MatrixType, typename Rhs>
struct solve_retval<HouseholderQR<_MatrixType>, Rhs>
@ -352,7 +358,7 @@ HouseholderQR<MatrixType>& HouseholderQR<MatrixType>::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<MatrixType, HCoeffsType>::run(m_qr, m_hCoeffs, 48, m_temp.data());
m_isInitialized = true;
return *this;

30
gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR_MKL.h vendored
View File

@ -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<typename MatrixQR, typename HCoeffs> \
void householder_qr_inplace_blocked(MatrixQR& mat, HCoeffs& hCoeffs, \
typename MatrixQR::Index maxBlockSize=32, \
EIGTYPE* tempData = 0) \
struct householder_qr_inplace_blocked<MatrixQR, HCoeffs, EIGTYPE, true> \
{ \
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)

2
gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h vendored
View File

@ -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 {

1
gtsam/3rdparty/gtsam_eigen_includes.h.in vendored
View File

@ -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>

7
gtsam/base/Matrix.cpp
View File

@ -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<m; ++i) {
// only scale the first v.size() rows of A to support augmented Matrix
for (size_t i=0; i<v.size(); ++i) {
const double& vi = v(i);
if (!isnan(vi) && !isinf(vi))
A.row(i) *= vi;
}
} else {
for (size_t i=0; i<m; ++i)
for (int i=0; i<m; ++i)
A.row(i) *= v(i);
}
}

2
gtsam/base/Matrix.h
View File

@ -336,7 +336,7 @@ void inplace_QR(MATRIX& A) {
HCoeffsType hCoeffs(size);
RowVectorType temp(cols);
Eigen::internal::householder_qr_inplace_blocked(A, hCoeffs, 48, temp.data());
Eigen::internal::householder_qr_inplace_blocked<MATRIX, HCoeffsType>::run(A, hCoeffs, 48, temp.data());
zeroBelowDiagonal(A);
}

4
gtsam/base/Vector.cpp
View File

@ -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<m; ++i) {
if(isnan(vec1[i]) ^ isnan(vec2[i]))
if(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
return false;
if(fabs(vec1[i] - vec2[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<m; ++i) {
if(isnan(vec1[i]) ^ isnan(vec2[i]))
if(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
return false;
if(fabs(vec1[i] - vec2[i]) > tol)
return false;

10
gtsam/geometry/CMakeLists.txt
View File

@ -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)

204
gtsam/geometry/Cal3Bundler.cpp
View File

@ -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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> Dcal, boost::optional<Matrix&> 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<double, 2, 3> 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<double, 2, 2> 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();
}
}

134
gtsam/geometry/Cal3Bundler.h
View File

@ -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<Cal3Bundler> {
class GTSAM_EXPORT Cal3Bundler: public DerivedValue<Cal3Bundler> {
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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Matrix&> Dcal =
boost::none, boost::optional<Matrix&> 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<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Cal3Bundler",
boost::serialization::base_object<Value>(*this));
void serialize(Archive & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("Cal3Bundler",
boost::serialization::base_object<Value>(*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

137
gtsam/geometry/Cal3DS2.cpp
View File

@ -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<Matrix&> H1,
boost::optional<Matrix&> 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(); }

12
gtsam/geometry/Cal3DS2.h
View File

@ -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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const ;
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ;
/// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& p, const double tol=1e-5) const;

102
gtsam/geometry/Cal3_S2.cpp
View File

@ -22,58 +22,72 @@
#include <iostream>
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<Matrix&> Dcal,
boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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_));
}
/* ************************************************************************* */

266
gtsam/geometry/Cal3_S2.h
View File

@ -26,166 +26,184 @@
namespace gtsam {
/**
* @brief The most common 5DOF 3D->2D calibration
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3_S2 : public DerivedValue<Cal3_S2> {
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<Cal3_S2> {
private:
double fx_, fy_, s_, u0_, v0_;
public:
public:
typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
typedef boost::shared_ptr<Cal3_S2> 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<Matrix&> H1 =
boost::none, boost::optional<Matrix&> 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<Matrix&> Dcal =
boost::none, boost::optional<Matrix&> 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<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Cal3_S2",
boost::serialization::base_object<Value>(*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<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("Cal3_S2",
boost::serialization::base_object<Value>(*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

51
gtsam/geometry/CalibratedCamera.cpp
View File

@ -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<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> H2) const {
boost::optional<Matrix&> Dpose, boost::optional<Matrix&> 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());
}
/* ************************************************************************* */

342
gtsam/geometry/CalibratedCamera.h
View File

@ -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<CalibratedCamera> {
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<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Matrix&> 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<CalibratedCamera> {
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<Matrix&> Dpose =
boost::none, boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> D_intrinsic_pose = boost::none,
boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1 =
boost::none, boost::optional<Matrix&> 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<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("CalibratedCamera",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(pose_);
}
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("CalibratedCamera",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(pose_);
}
/// @}
};
/// @}
};
}

156
gtsam/geometry/EssentialMatrix.h Normal file
View File

@ -0,0 +1,156 @@
/*
* @file EssentialMatrix.h
* @brief EssentialMatrix class
* @author Frank Dellaert
* @date December 17, 2013
*/
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Sphere2.h>
#include <gtsam/geometry/Point2.h>
#include <iostream>
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<EssentialMatrix> {
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<Matrix&> DE = boost::none,
boost::optional<Matrix&> 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<Matrix&> 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

815
gtsam/geometry/PinholeCamera.h
View File

@ -31,398 +31,503 @@
namespace gtsam {
/**
* A pinhole camera class that has a Pose3 and a Calibration.
* @addtogroup geometry
* \nosubgrouping
*/
template <typename Calibration>
class PinholeCamera : public DerivedValue<PinholeCamera<Calibration> > {
private:
Pose3 pose_;
Calibration K_;
/**
* A pinhole camera class that has a Pose3 and a Calibration.
* @addtogroup geometry
* \nosubgrouping
*/
template<typename Calibration>
class PinholeCamera: public DerivedValue<PinholeCamera<Calibration> > {
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<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> 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<Point2, bool> 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<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> Dpoint = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> 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<Point2,bool> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> 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<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> Dpoint = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Matrix&> Dcamera = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> 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<class CalibrationB>
double range(const PinholeCamera<CalibrationB>& camera,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<class CalibrationB>
double range(
const PinholeCamera<CalibrationB>& camera, //
boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> 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<class Archive>
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<class Archive>
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_);
}
/// @}
}
/// @}
};
}
;}

4
gtsam/geometry/Point2.cpp
View File

@ -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<Matrix&> 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;

3
gtsam/geometry/Point2.h
View File

@ -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) {}

22
gtsam/geometry/Pose2.cpp
View File

@ -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<Matrix&> 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;

2
gtsam/geometry/Pose2.h
View File

@ -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.);

625
gtsam/geometry/Pose3.cpp
View File

@ -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<Matrix&> 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<Matrix&> 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<N; ++i) {
ad_i = ad_xi * ad_i;
fac = fac*i;
res = res + B(i)/fac*ad_i;
}
return res;
}
/* ************************************************************************* */
void Pose3::print(const string& s) const {
cout << s;
R_.print("R:\n");
t_.print("t: ");
}
/* ************************************************************************* */
bool Pose3::equals(const Pose3& pose, double tol) const {
return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol);
}
/* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */
Pose3 Pose3::Expmap(const Vector& xi) {
// 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));
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);
/* ************************************************************************* */
Vector Pose3::adjoint(const Vector& xi, const Vector& y,
boost::optional<Matrix&> 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<Matrix&> 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<double,1,4> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = p2.inverse().AdjointMap();
if (H2) *H2 = I6;
return (*this) * p2;
}
/* ************************************************************************* */
Pose3 Pose3::inverse(boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Pose3> align(const vector<Point3Pair>& 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<double, 1, 4> 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<Matrix&> Dpose,
boost::optional<Matrix&> 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<Matrix&> Dpose,
boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1)
*H1 = p2.inverse().AdjointMap();
if (H2)
*H2 = I6;
return (*this) * p2;
}
/* ************************************************************************* */
Pose3 Pose3::inverse(boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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<Pose3> align(const vector<Point3Pair>& 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

248
gtsam/geometry/Pose3.h
View File

@ -15,7 +15,6 @@
*/
// \callgraph
#pragma once
#include <gtsam/config.h>
@ -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<Pose3> {
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<Matrix&> H1 = boost::none) const;
///compose this transformation onto another (first *this and then p2)
Pose3 compose(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Pose3> {
public:
static const size_t dimension = 6;
Pose3 between(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none) const;
///compose this transformation onto another (first *this and then p2)
Pose3 compose(const Pose3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> 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<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
}
template<>
inline Matrix wedge<Pose3>(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<Point3,Point3> Point3Pair;
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
/**
* Calculate pose between a vector of 3D point correspondences (p,q)
* where q = Pose3::transform_from(p) = t + R*p
*/
typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
} // namespace gtsam

12
gtsam/geometry/Rot2.cpp
View File

@ -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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> H1,
Point2 Rot2::unrotate(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> 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();
}
}

188
gtsam/geometry/Rot3.cpp Normal file
View File

@ -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 <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>
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<Matrix&> HR, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix3, Vector3> 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

42
gtsam/geometry/Rot3.h
View File

@ -42,6 +42,7 @@
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Sphere2.h>
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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Matrix&> HR = boost::none,
boost::optional<Matrix&> 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

125
gtsam/geometry/Rot3M.cpp
View File

@ -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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix3, Vector3> 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

96
gtsam/geometry/Rot3Q.cpp
View File

@ -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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix3, Vector3> 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

130
gtsam/geometry/Sphere2.cpp Normal file
View File

@ -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 <gtsam/geometry/Sphere2.h>
#include <gtsam/geometry/Point2.h>
#include <iostream>
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<Matrix&> 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<Matrix&> 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;
}
/* ************************************************************************* */
}

119
gtsam/geometry/Sphere2.h Normal file
View File

@ -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 <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.h>
namespace gtsam {
/// Represents a 3D point on a unit sphere.
class Sphere2: public DerivedValue<Sphere2> {
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<Matrix&> H = boost::none) const {
if (H)
*H = basis();
return p_;
}
/// Signed, vector-valued error between two directions
Vector error(const Sphere2& q,
boost::optional<Matrix&> H = boost::none) const;
/// Distance between two directions
double distance(const Sphere2& q,
boost::optional<Matrix&> 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

6
gtsam/geometry/StereoCamera.cpp
View File

@ -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()

5
gtsam/geometry/StereoPoint2.h
View File

@ -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:
/// @}

55
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -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); }
/* ************************************************************************* */

2
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -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.

70
gtsam/geometry/tests/testEssentialMatrix.cpp Normal file
View File

@ -0,0 +1,70 @@
/*
* @file testEssentialMatrix.cpp
* @brief Test EssentialMatrix class
* @author Frank Dellaert
* @date December 17, 2013
*/
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
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);
}
/* ************************************************************************* */

210
gtsam/geometry/tests/testPinholeCamera.cpp Normal file
View File

@ -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 <cmath>
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
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<Cal3_S2> 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<Point2, bool> 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); }
/* ************************************************************************* */

8
gtsam/geometry/tests/testPoint2.cpp
View File

@ -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));
}

2
gtsam/geometry/tests/testPoint3.cpp
View File

@ -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)));
}

52
gtsam/geometry/tests/testPose2.cpp
View File

@ -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<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
Pose2 actual = expmap_default<Pose2>(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<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
Pose2 actual = expmap_default<Pose2>(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<Pose2>(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

20
gtsam/geometry/tests/testPose3.cpp
View File

@ -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<Pose3>(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<Pose3>(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;

50
gtsam/geometry/tests/testRot3M.cpp
View File

@ -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));

56
gtsam/geometry/tests/testRot3Q.cpp
View File

@ -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<double>(), 0.0, 0.0);
Vector w6 = (Vector(3) << boost::math::constants::pi<double>(), 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<double>(), 0.0);
Vector w7 = (Vector(3) << 0.0, boost::math::constants::pi<double>(), 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<double>());
Vector w8 = (Vector(3) << 0.0, 0.0, boost::math::constants::pi<double>());
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));

4
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -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);

246
gtsam/geometry/tests/testSphere2.cpp Normal file
View File

@ -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 <gtsam/geometry/Sphere2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
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<Point3> 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, Sphere2>(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<Sphere2>(
boost::bind(&Sphere2::error, &p, _1, boost::none), q);
p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
}
{
expected = numericalDerivative11<Sphere2>(
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<Sphere2>(
boost::bind(&Sphere2::distance, &p, _1, boost::none), q);
p.distance(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
}
{
expected = numericalGradient<Sphere2>(
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<Pose2,Pose2,Pose2>(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<Pose2,Pose2,Pose2>(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);
}
/* ************************************************************************* */

2
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -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.

51
gtsam/geometry/tests/testStereoPoint2.cpp
View File

@ -11,26 +11,65 @@
/**
* @file testStereoPoint2.cpp
*
* @brief Tests for the StereoPoint2 class
*
* @date Nov 4, 2011
* @author Alex Cunningham
*/
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/lieProxies.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/StereoPoint2.h>
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()));
}
/* ************************************************************************* */

39
gtsam/geometry/tests/timeCalibratedCamera.cpp
View File

@ -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

103
gtsam/geometry/tests/timePinholeCamera.cpp Normal file
View File

@ -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 <time.h>
#include <iostream>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
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<Cal3Bundler> 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;
}

2
gtsam/geometry/tests/timePose2.cpp
View File

@ -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);

4
gtsam/geometry/tests/timePose3.cpp
View File

@ -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))

2
gtsam/geometry/tests/timeStereoCamera.cpp
View File

@ -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.