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-e3fd810bbb7frelease/4.3a0
Frank Dellaert
Richard Roberts
parent
faca0ecd04
commit
bd3126f7b4
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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; \
|
||||
} \
|
||||
}; \
|
||||
|
|
|
|||
|
|
@ -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,12 +108,82 @@ 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.
|
||||
*
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
||||
|
|
|
|||
|
|
@ -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(); \
|
||||
|
|
|
|||
|
|
@ -251,8 +251,13 @@ 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,
|
||||
template<typename MatrixQR, typename HCoeffs,
|
||||
typename MatrixQRScalar = typename MatrixQR::Scalar,
|
||||
bool InnerStrideIsOne = (MatrixQR::InnerStrideAtCompileTime == 1 && HCoeffs::InnerStrideAtCompileTime == 1)>
|
||||
struct householder_qr_inplace_blocked
|
||||
{
|
||||
// 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)
|
||||
{
|
||||
|
|
@ -301,6 +306,7 @@ void householder_qr_inplace_blocked(MatrixQR& mat, HCoeffs& hCoeffs,
|
|||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@
|
|||
#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 {
|
||||
|
||||
|
|
@ -44,18 +44,20 @@ namespace internal {
|
|||
|
||||
#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(); \
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -24,131 +24,135 @@
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Bundler::Cal3Bundler() : f_(1), k1_(0), k2_(0) {}
|
||||
Cal3Bundler::Cal3Bundler() :
|
||||
f_(1), k1_(0), k2_(0), u0_(0), v0_(0) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Bundler::Cal3Bundler(const Vector &v) : f_(v(0)), k1_(v(1)), k2_(v(2)) {}
|
||||
Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) :
|
||||
f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Bundler::Cal3Bundler(const double f, const double k1, const double k2) : f_(f), k1_(k1), k2_(k2) {}
|
||||
Matrix Cal3Bundler::K() const {
|
||||
Matrix3 K;
|
||||
K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1;
|
||||
return K;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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::k() const { return Vector_(4, k1_, k2_, 0, 0) ; }
|
||||
Vector Cal3Bundler::vector() const {
|
||||
return (Vector(3) << f_, k1_, k2_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Cal3Bundler::vector() const { return Vector_(3, f_, k1_, k2_) ; }
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Cal3Bundler::print(const std::string& s) const { gtsam::print(vector(), s + ".K") ; }
|
||||
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)
|
||||
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&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
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 r2 = r*r ;
|
||||
const double g = 1 + k1_ * r + k2_*r2 ; // save one multiply
|
||||
const double fg = f_*g ;
|
||||
const double g = 1. + (k1_ + k2_ * r) * r;
|
||||
const double u = g * x, v = g * y;
|
||||
|
||||
// semantic meaningful version
|
||||
//if (H1) *H1 = D2d_calibration(p) ;
|
||||
//if (H2) *H2 = D2d_intrinsic(p) ;
|
||||
|
||||
// unrolled version, much faster
|
||||
if ( H1 || H2 ) {
|
||||
|
||||
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) ;
|
||||
// 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;
|
||||
}
|
||||
|
||||
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) ;
|
||||
}
|
||||
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(fg * x, fg * y) ;
|
||||
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);
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -13,7 +13,7 @@
|
|||
* @file Cal3Bundler.h
|
||||
* @brief Calibration used by Bundler
|
||||
* @date Sep 25, 2010
|
||||
* @author ydjian
|
||||
* @author Yong Dian Jian
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -31,30 +31,27 @@ namespace gtsam {
|
|||
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
|
||||
/// 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
|
||||
Vector vector() const;
|
||||
|
||||
/// focal length x
|
||||
inline double fx() const {
|
||||
return f_;
|
||||
}
|
||||
|
||||
/// 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;
|
||||
|
||||
///TODO: comment
|
||||
/// @deprecated might be removed in next release, use uncalibrate
|
||||
Matrix D2d_calibration(const Point2& p) const;
|
||||
|
||||
///TODO: comment
|
||||
/// @deprecated might be removed in next release, use uncalibrate
|
||||
Matrix D2d_intrinsic_calibration(const Point2& p) const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
/// Update calibration with tangent space delta
|
||||
Cal3Bundler retract(const Vector& d) const;
|
||||
|
||||
///TODO: comment
|
||||
/// 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,16 +154,17 @@ 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",
|
||||
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_);
|
||||
}
|
||||
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -28,10 +28,10 @@ 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"); }
|
||||
|
|
@ -49,23 +49,52 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
|
|||
Point2 Cal3DS2::uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
|
||||
// parameters
|
||||
const double fx = fx_, fy = fy_, s = s_;
|
||||
const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
|
||||
|
||||
// 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 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);
|
||||
|
||||
const double x2 = g*x + dx ;
|
||||
const double y2 = g*y + dy ;
|
||||
const double pnx = g*x + dx;
|
||||
const double pny = g*y + dy;
|
||||
|
||||
if (H1) *H1 = D2d_calibration(p) ;
|
||||
if (H2) *H2 = D2d_intrinsic(p) ;
|
||||
// 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;
|
||||
|
||||
return Point2(fx_* x2 + s_ * y2 + u0_, fy_ * y2 + v0_) ;
|
||||
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_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -119,8 +148,8 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
|
|||
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,
|
||||
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;
|
||||
|
|
@ -138,7 +167,7 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
|
|||
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) );
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -33,7 +33,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3_S2::Cal3_S2(const std::string &path) {
|
||||
Cal3_S2::Cal3_S2(const std::string &path) :
|
||||
fx_(320), fy_(320), s_(0), u0_(320), v0_(140) {
|
||||
|
||||
char buffer[200];
|
||||
buffer[0] = 0;
|
||||
|
|
@ -57,24 +58,37 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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;
|
||||
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_);
|
||||
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_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -53,8 +53,9 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/// constructor from vector
|
||||
Cal3_S2(const Vector &d): fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)){}
|
||||
|
||||
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
|
||||
|
|
@ -86,19 +87,29 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/// focal length x
|
||||
inline double fx() const { return fx_;}
|
||||
inline double fx() const {
|
||||
return fx_;
|
||||
}
|
||||
|
||||
/// focal length y
|
||||
inline double fy() const { return fy_;}
|
||||
inline double fy() const {
|
||||
return fy_;
|
||||
}
|
||||
|
||||
/// skew
|
||||
inline double skew() const { return s_;}
|
||||
inline double skew() const {
|
||||
return s_;
|
||||
}
|
||||
|
||||
/// image center in x
|
||||
inline double px() const { return u0_;}
|
||||
inline double px() const {
|
||||
return u0_;
|
||||
}
|
||||
|
||||
/// image center in y
|
||||
inline double py() const { return v0_;}
|
||||
inline double py() const {
|
||||
return v0_;
|
||||
}
|
||||
|
||||
/// return the principal point
|
||||
Point2 principalPoint() const {
|
||||
|
|
@ -114,32 +125,38 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/// return calibration matrix K
|
||||
Matrix K() 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 Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
|
||||
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 (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
|
||||
* @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&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none) const;
|
||||
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
|
||||
|
|
@ -175,7 +192,8 @@ namespace gtsam {
|
|||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("Cal3_S2",
|
||||
ar
|
||||
& boost::serialization::make_nvp("Cal3_S2",
|
||||
boost::serialization::base_object<Value>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(fx_);
|
||||
ar & BOOST_SERIALIZATION_NVP(fy_);
|
||||
|
|
|
|||
|
|
@ -27,22 +27,23 @@ CalibratedCamera::CalibratedCamera(const Pose3& 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,11 +59,10 @@ 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
|
||||
|
|
@ -72,27 +72,26 @@ Point2 CalibratedCamera::project(const Point3& point,
|
|||
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) {
|
||||
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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -26,7 +26,9 @@ namespace gtsam {
|
|||
|
||||
class GTSAM_EXPORT CheiralityException: public std::runtime_error {
|
||||
public:
|
||||
CheiralityException() : std::runtime_error("Cheirality Exception") {}
|
||||
CheiralityException() :
|
||||
std::runtime_error("Cheirality Exception") {
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -46,7 +48,8 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/// default constructor
|
||||
CalibratedCamera() {}
|
||||
CalibratedCamera() {
|
||||
}
|
||||
|
||||
/// construct with pose
|
||||
explicit CalibratedCamera(const Pose3& pose);
|
||||
|
|
@ -76,27 +79,31 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/// destructor
|
||||
virtual ~CalibratedCamera() {}
|
||||
virtual ~CalibratedCamera() {
|
||||
}
|
||||
|
||||
/// return pose
|
||||
inline const Pose3& pose() const { 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 {
|
||||
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 {
|
||||
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 {
|
||||
inline const CalibratedCamera inverse(boost::optional<Matrix&> H1 =
|
||||
boost::none) const {
|
||||
return CalibratedCamera(pose_.inverse(H1));
|
||||
}
|
||||
|
||||
|
|
@ -119,11 +126,14 @@ namespace gtsam {
|
|||
Vector localCoordinates(const CalibratedCamera& T2) const;
|
||||
|
||||
/// Lie group dimensionality
|
||||
inline size_t dim() const { return 6 ; }
|
||||
inline size_t dim() const {
|
||||
return 6;
|
||||
}
|
||||
|
||||
/// Lie group dimensionality
|
||||
inline static size_t Dim() { return 6 ; }
|
||||
|
||||
inline static size_t Dim() {
|
||||
return 6;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// measurement functions and derivatives
|
||||
|
|
@ -132,18 +142,16 @@ namespace gtsam {
|
|||
/// @}
|
||||
/// @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
|
||||
* @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&> D_intrinsic_pose = boost::none,
|
||||
boost::optional<Matrix&> D_intrinsic_point = boost::none) const;
|
||||
Point2 project(const Point3& point, boost::optional<Matrix&> Dpose =
|
||||
boost::none, boost::optional<Matrix&> Dpoint = boost::none) const;
|
||||
|
||||
/**
|
||||
* projects a 3-dimensional point in camera coordinates into the
|
||||
|
|
@ -165,10 +173,10 @@ namespace gtsam {
|
|||
* @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,
|
||||
double range(const Point3& point, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const {
|
||||
return pose_.range(point, H1, H2); }
|
||||
return pose_.range(point, H1, H2);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate range to another pose
|
||||
|
|
@ -177,10 +185,10 @@ namespace gtsam {
|
|||
* @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,
|
||||
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_.range(pose, H1, H2);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate range to another camera
|
||||
|
|
@ -189,10 +197,10 @@ namespace gtsam {
|
|||
* @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); }
|
||||
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:
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -48,13 +48,18 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/** default constructor */
|
||||
PinholeCamera() {}
|
||||
PinholeCamera() {
|
||||
}
|
||||
|
||||
/** constructor with pose */
|
||||
explicit PinholeCamera(const Pose3& pose):pose_(pose){}
|
||||
explicit PinholeCamera(const Pose3& pose) :
|
||||
pose_(pose) {
|
||||
}
|
||||
|
||||
/** constructor with pose and calibration */
|
||||
PinholeCamera(const Pose3& pose, const Calibration& K):pose_(pose),K_(K) {}
|
||||
PinholeCamera(const Pose3& pose, const Calibration& K) :
|
||||
pose_(pose), K_(K) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Named Constructors
|
||||
|
|
@ -67,7 +72,8 @@ namespace gtsam {
|
|||
* (theta 0 = looking in direction of positive X axis)
|
||||
* @param height camera height
|
||||
*/
|
||||
static PinholeCamera Level(const Calibration &K, const Pose2& pose2, double 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);
|
||||
|
|
@ -90,7 +96,8 @@ namespace gtsam {
|
|||
* 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()) {
|
||||
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
|
||||
|
|
@ -121,8 +128,8 @@ namespace gtsam {
|
|||
|
||||
/// 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) ;
|
||||
return pose_.equals(camera.pose(), tol)
|
||||
&& K_.equals(camera.calibration(), tol);
|
||||
}
|
||||
|
||||
/// print
|
||||
|
|
@ -135,19 +142,28 @@ namespace gtsam {
|
|||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
virtual ~PinholeCamera() {}
|
||||
virtual ~PinholeCamera() {
|
||||
}
|
||||
|
||||
/// return pose
|
||||
inline Pose3& pose() { return pose_; }
|
||||
inline Pose3& pose() {
|
||||
return pose_;
|
||||
}
|
||||
|
||||
/// return pose
|
||||
inline const Pose3& pose() const { return pose_; }
|
||||
inline const Pose3& pose() const {
|
||||
return pose_;
|
||||
}
|
||||
|
||||
/// return calibration
|
||||
inline Calibration& calibration() { return K_; }
|
||||
inline Calibration& calibration() {
|
||||
return K_;
|
||||
}
|
||||
|
||||
/// return calibration
|
||||
inline const Calibration& calibration() const { return K_; }
|
||||
inline const Calibration& calibration() const {
|
||||
return K_;
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Group ?? Frank says this might not make sense
|
||||
|
|
@ -155,17 +171,19 @@ namespace gtsam {
|
|||
|
||||
/// 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 {
|
||||
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->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->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
|
||||
Calibration::Dim());
|
||||
H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
|
||||
}
|
||||
return result;
|
||||
|
|
@ -173,28 +191,32 @@ namespace gtsam {
|
|||
|
||||
/// 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 {
|
||||
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->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->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 {
|
||||
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->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
|
||||
Calibration::Dim());
|
||||
H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
|
||||
}
|
||||
return result;
|
||||
|
|
@ -222,15 +244,20 @@ namespace gtsam {
|
|||
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());
|
||||
d.tail(calibration().dim()) = calibration().localCoordinates(
|
||||
T2.calibration());
|
||||
return d;
|
||||
}
|
||||
|
||||
/// Manifold dimension
|
||||
inline size_t dim() const { return pose_.dim() + K_.dim(); }
|
||||
inline size_t dim() const {
|
||||
return pose_.dim() + K_.dim();
|
||||
}
|
||||
|
||||
/// Manifold dimension
|
||||
inline static size_t Dim() { return Pose3::Dim() + Calibration::Dim(); }
|
||||
inline static size_t Dim() {
|
||||
return Pose3::Dim() + Calibration::Dim();
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Transformations and measurement functions
|
||||
|
|
@ -239,13 +266,14 @@ namespace gtsam {
|
|||
/**
|
||||
* projects a 3-dimensional point in camera coordinates into the
|
||||
* camera and returns a 2-dimensional point, no calibration applied
|
||||
* With optional 2by3 derivative
|
||||
* @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&> H1 = boost::none){
|
||||
if (H1) {
|
||||
boost::optional<Matrix&> Dpoint = boost::none) {
|
||||
if (Dpoint) {
|
||||
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);
|
||||
*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());
|
||||
}
|
||||
|
|
@ -258,68 +286,131 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** 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
|
||||
* @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&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none,
|
||||
boost::optional<Matrix&> H3 = boost::none) const {
|
||||
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 {
|
||||
|
||||
if (!H1 && !H2 && !H3) {
|
||||
// Transform to camera coordinates and check cheirality
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
if ( pc.z() <= 0 ) throw CheiralityException();
|
||||
|
||||
#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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
// uncalibration
|
||||
Matrix Dpi_pn; // 2*2
|
||||
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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();
|
||||
Matrix Dpc_rot /* 3*3 */, Dpc_point /* 3*3 */;
|
||||
const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point);
|
||||
|
||||
Matrix Dpc_pose = Matrix::Zero(3, 6);
|
||||
Dpc_pose.block(0, 0, 3, 3) = Dpc_rot;
|
||||
|
||||
// camera to normalized image coordinate
|
||||
Matrix Hn; // 2*3
|
||||
const Point2 pn = project_to_camera(pc, Hn) ;
|
||||
Matrix Dpn_pc; // 2*3
|
||||
const Point2 pn = project_to_camera(pc, Dpn_pc);
|
||||
|
||||
// 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
|
||||
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 H1 is the jacobian w.r.t. [pose3 calibration]
|
||||
* @param H2 is the jacobian w.r.t. point3
|
||||
* @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&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const {
|
||||
inline Point2 project2(
|
||||
const Point3& pw, //
|
||||
boost::optional<Matrix&> Dcamera = boost::none,
|
||||
boost::optional<Matrix&> Dpoint = boost::none) const {
|
||||
|
||||
if (!H1 && !H2) {
|
||||
if (!Dcamera && !Dpoint) {
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
if ( pc.z() <= 0 ) throw CheiralityException();
|
||||
|
||||
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
|
||||
if (pc.z() <= 0)
|
||||
throw CheiralityException();
|
||||
#endif
|
||||
const Point2 pn = project_to_camera(pc);
|
||||
return K_.uncalibrate(pn);
|
||||
}
|
||||
|
||||
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
|
||||
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 (H2) *H2 = Htmp2;
|
||||
if (Dpoint)
|
||||
*Dpoint = Dp;
|
||||
return pi;
|
||||
}
|
||||
|
||||
|
|
@ -330,20 +421,28 @@ namespace gtsam {
|
|||
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 H1 the optionally computed Jacobian with respect to pose
|
||||
* @param H2 the optionally computed Jacobian with respect to the 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&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
double result = pose_.range(point, H1, H2);
|
||||
if(H1) {
|
||||
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(*H1);
|
||||
Matrix& H1r(*Dpose);
|
||||
H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
|
||||
H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
|
||||
}
|
||||
|
|
@ -353,17 +452,18 @@ namespace gtsam {
|
|||
/**
|
||||
* 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
|
||||
* @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&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
double result = pose_.range(pose, H1, H2);
|
||||
if(H1) {
|
||||
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(*H1);
|
||||
Matrix& H1r(*Dpose);
|
||||
H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
|
||||
H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
|
||||
}
|
||||
|
|
@ -373,26 +473,29 @@ namespace gtsam {
|
|||
/**
|
||||
* 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
|
||||
* @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&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
double result = pose_.range(camera.pose_, H1, H2);
|
||||
if(H1) {
|
||||
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(*H1);
|
||||
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(H2) {
|
||||
if (Dother) {
|
||||
// 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());
|
||||
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;
|
||||
}
|
||||
|
|
@ -400,14 +503,16 @@ namespace gtsam {
|
|||
/**
|
||||
* 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
|
||||
* @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&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
return pose_.range(camera.pose_, H1, H2); }
|
||||
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:
|
||||
|
||||
|
|
@ -424,5 +529,5 @@ private:
|
|||
ar & BOOST_SERIALIZATION_NVP(K_);
|
||||
}
|
||||
/// @}
|
||||
};
|
||||
}
|
||||
;}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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) {}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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.);
|
||||
|
|
|
|||
|
|
@ -37,8 +37,8 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Pose3::Pose3(const Pose2& pose2) :
|
||||
R_(Rot3::rodriguez(0, 0, pose2.theta())),
|
||||
t_(Point3(pose2.x(), pose2.y(), 0)) {
|
||||
R_(Rot3::rodriguez(0, 0, pose2.theta())), t_(
|
||||
Point3(pose2.x(), pose2.y(), 0)) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -65,11 +65,13 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3::adjoint(const Vector& xi, const Vector& y, boost::optional<Matrix&> H) {
|
||||
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;
|
||||
Vector dxi = zero(6);
|
||||
dxi(i) = 1.0;
|
||||
Matrix Gi = adjointMap(dxi);
|
||||
(*H).col(i) = Gi * y;
|
||||
}
|
||||
|
|
@ -78,11 +80,13 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, boost::optional<Matrix&> H) {
|
||||
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;
|
||||
Vector dxi = zero(6);
|
||||
dxi(i) = 1.0;
|
||||
Matrix GTi = adjointMap(dxi).transpose();
|
||||
(*H).col(i) = GTi * y;
|
||||
}
|
||||
|
|
@ -94,7 +98,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
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 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;
|
||||
|
|
@ -131,8 +136,7 @@ namespace gtsam {
|
|||
if (theta < 1e-10) {
|
||||
static const Rot3 I;
|
||||
return Pose3(I, v);
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
Point3 n(w / theta); // axis unit vector
|
||||
Rot3 R = Rot3::rodriguez(n.vector(), theta);
|
||||
double vn = n.dot(v); // translation parallel to n
|
||||
|
|
@ -150,8 +154,7 @@ namespace gtsam {
|
|||
Vector6 log;
|
||||
log << w, T;
|
||||
return log;
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
Matrix3 W = skewSymmetric(w / t);
|
||||
// Formula from Agrawal06iros, equation (14)
|
||||
// simplified with Mathematica, and multiplying in T to avoid matrix math
|
||||
|
|
@ -192,7 +195,8 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// different versions of localCoordinates
|
||||
Vector6 Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const {
|
||||
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));
|
||||
|
|
@ -236,43 +240,48 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transform_from(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) {
|
||||
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());
|
||||
H1->resize(3,6);
|
||||
(*H1) << DR, R;
|
||||
Dpose->resize(3, 6);
|
||||
(*Dpose) << DR, R;
|
||||
}
|
||||
if (H2) *H2 = R_.matrix();
|
||||
if (Dpoint)
|
||||
*Dpoint = R_.matrix();
|
||||
return R_ * p + t_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transform_to(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
|
||||
boost::optional<Matrix&> Dpoint) const {
|
||||
const Point3 result = R_.unrotate(p - t_);
|
||||
if (H1) {
|
||||
if (Dpose) {
|
||||
const Point3& q = result;
|
||||
Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
|
||||
H1->resize(3,6);
|
||||
(*H1) << DR, _I3;
|
||||
Dpose->resize(3, 6);
|
||||
(*Dpose) << DR, _I3;
|
||||
}
|
||||
if (H2) *H2 = R_.transpose();
|
||||
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;
|
||||
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();
|
||||
if (H1)
|
||||
*H1 = -AdjointMap();
|
||||
Rot3 Rt = R_.inverse();
|
||||
return Pose3(Rt, Rt * (-t_));
|
||||
}
|
||||
|
|
@ -282,28 +291,32 @@ namespace gtsam {
|
|||
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;
|
||||
if (H1)
|
||||
*H1 = -result.inverse().AdjointMap();
|
||||
if (H2)
|
||||
*H2 = I6;
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose3::range(const Point3& point,
|
||||
boost::optional<Matrix&> H1,
|
||||
double Pose3::range(const Point3& point, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
if (!H1 && !H2) return transform_to(point).norm();
|
||||
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);
|
||||
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 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();
|
||||
|
|
@ -316,7 +329,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
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
|
||||
if (n < 3)
|
||||
return boost::none; // we need at least three pairs
|
||||
|
||||
// calculate centroids
|
||||
Vector cp = zero(3), cq = zero(3);
|
||||
|
|
@ -325,7 +339,8 @@ namespace gtsam {
|
|||
cq += pair.second.vector();
|
||||
}
|
||||
double f = 1.0 / n;
|
||||
cp *= f; cq *= f;
|
||||
cp *= f;
|
||||
cq *= f;
|
||||
|
||||
// Add to form H matrix
|
||||
Matrix H = zeros(3, 3);
|
||||
|
|
|
|||
|
|
@ -15,7 +15,6 @@
|
|||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/config.h>
|
||||
|
|
@ -32,7 +31,8 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class Pose2; // forward declare
|
||||
class Pose2;
|
||||
// forward declare
|
||||
|
||||
/**
|
||||
* A 3D pose (R,t) : (Rot3,Point3)
|
||||
|
|
@ -48,8 +48,9 @@ namespace gtsam {
|
|||
typedef Point3 Translation;
|
||||
|
||||
private:
|
||||
Rot3 R_;
|
||||
Point3 t_;
|
||||
|
||||
Rot3 R_; ///< Rotation gRp, between global and pose frame
|
||||
Point3 t_; ///< Translation gTp, from global origin to pose frame origin
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -57,21 +58,27 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/** Default constructor is origin */
|
||||
Pose3() {}
|
||||
Pose3() {
|
||||
}
|
||||
|
||||
/** Copy constructor */
|
||||
Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {}
|
||||
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) {}
|
||||
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)) {}
|
||||
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
|
||||
|
|
@ -88,14 +95,15 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
static Pose3 identity() { return Pose3(); }
|
||||
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,
|
||||
Pose3 compose(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/// compose syntactic sugar
|
||||
|
|
@ -107,8 +115,7 @@ namespace gtsam {
|
|||
* 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,
|
||||
Pose3 between(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/// @}
|
||||
|
|
@ -124,16 +131,21 @@ namespace gtsam {
|
|||
};
|
||||
|
||||
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
|
||||
static size_t Dim() { return dimension; }
|
||||
static size_t Dim() {
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/// Dimensionality of the tangent space = 6 DOF
|
||||
size_t dim() const { return dimension; }
|
||||
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;
|
||||
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;
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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,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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
@ -180,18 +151,6 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
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");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
}
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
|
|
@ -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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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)));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
Loading…
Reference in New Issue