Classes | Namespaces | Defines | Typedefs | Functions | Variables
utils.h File Reference

Universal, globally useful definitions in vslam namespace. More...

#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <vector>
#include <list>
#include <set>
#include <assert.h>
#include "ros/ros.h"
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <boost/tr1/memory.hpp>
#include <boost/tr1/cmath.hpp>
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <boost/filesystem.hpp>
#include "Sophus/se3.h"
#include "Sophus/so3.h"
#include "nav_msgs/Odometry.h"
Include dependency graph for utils.h:
This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes

struct  ALIGNED< T1, T2 >
struct  EigenAllocTraits< T >
struct  EigenAllocTraits< Vector3d >
class  ImageCoordinate< ImageParamsObj >
class  Maybe< T >
 XXX use pointer instead?? XXX not used?? More...
class  PanoramaParams
class  SingleVariableWrapper< T >
class  SpeedAsNaturalNumber
struct  SpeedSegment
class  TimeAsNaturalNumber

Namespaces

namespace  vslam
 

Namespace containing implementation of visual odometry.


Defines

#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
#define EIGEN_DONT_ALIGN
#define ROSCONSOLE_MIN_SEVERITY   ROSCONSOLE_SEVERITY_DEBUG

Typedefs

typedef ALIGNED< std::pair
< int, Eigen::Vector2d >
>::vector 
IdxPoint2dVec
typedef ALIGNED< std::pair
< int, Eigen::Vector3d >
>::vector 
IdxPoint3dVec
typedef vector< PanoImCoordImageCoordinateVec
typedef std::pair
< Eigen::Vector3d,
Eigen::Vector3d > 
ImageMatch
typedef ALIGNED< ImageMatch >
::vector 
ImageMatchVec
typedef ImageCoordinate
< PanoramaParams
PanoImCoord
typedef Eigen::Matrix< double,
3, Eigen::Dynamic > 
PointMatrix
typedef ALIGNED
< Eigen::Vector2d >::vector 
Points2dSTL
typedef ALIGNED
< Eigen::Vector3d >::vector 
Points3dSTL

Functions

ImageMatchVec buildImageMatchesFrom3dPoints (const SE3 &T_wc1, const SE3 &T_wc2, const Points3dSTL &points)
Vector2d c1PixelToc2Pixel (const SE3 &T_c1c2, const Vector2d &pixel1)
ImageMatch computeMatch (const Vector2d &px_c1, const Vector2d &px_c2)
void constructTrajectoryFromSpeedSegments (std::vector< SpeedSegment > speed_segments, ALIGNED< SE3 >::vector &transforms)
Vector3d coordFrameConventionLocalToROS (const Vector3d &vec)
SE3 coordFrameConventionLocalToROS (const SE3 &T)
template<class T >
cv::Point cvPointFromVector2 (Eigen::Matrix< T, 2, 1 > vec)
void generateLineTrajectory (const Vector3d &start, const Vector3d &end, int num_stops, ALIGNED< SE3 >::vector &transforms)
 XXX move to testutils.
void generateRandomScene (const Vector3d &upper_left_deep, const Vector3d &bottom_right_shallow, double target_num_vec, Points3dSTL &points)
 XXX move to testutils.
void generateRandomTrajectory (const Vector3d &upper_left_deep, const Vector3d &bottom_right_shallow, int num_stops, ALIGNED< SE3 >::vector &transforms)
 XXX move to testutils.
template<class T >
vector< T > getArrayIdices (const vector< T > &array, const vector< int > &idxs)
 select indexes from an array,
template<class T >
cv::Mat getArrayIdices (const cv::Mat &mat, const vector< int > &idxs)
 select rows from a matrix.
template<template< class, class > class C1, template< class, class > class C2, class T1 , class T2 , template< class > class A1, template< class > class A2>
C1< pair< T1, T2 >, A1< pair
< T1, T2 > > > 
getByIdx (const C1< pair< T1, T2 >, A1< pair< T1, T2 > > > &pts, const C2< T1, A2< T1 > > &idx)
 Select values with given sorted indexes from index-value pairs sorted by index.
template<class C1 , class C2 >
C1 getCommon (const C1 &idx1, const C2 &idx2)
 Get indexes that are common to both index sequrces.
template<class T1 , class T2 >
T1 getFirst (const pair< T1, T2 > &pair)
 get pair.first
geometry_msgs::Pose getPoseFromTcwInSE3 (const SE3 &T_cw)
template<class T1 , class T2 >
T2 getSecond (const pair< T1, T2 > &pair)
 get pair.second
Vector2d homogenousToPixel (const Vector3d &hom)
ImageMatchVec imageMatchCoords (const vector< cv::KeyPoint > &tkpts, const vector< cv::KeyPoint > &qkpts, const std::vector< cv::DMatch > &matches)
template<template< class, class > class C1, template< class, class > class C2, template< class > class Alloc1, template< class > class Alloc2, class TK , class TV >
list< pair< TK, TV >, Alloc2
< pair< TK, TV > > > 
indexBy (const C1< TK, Alloc1< TK > > &idx, const C2< TV, Alloc2< TV > > &points)
 Typical zip function from functional programmming languages.
template<class T >
inv_linspace_idx (T first, T last, int count, T val)
template<template< class, class > class C1, template< class, class > class C2, class T , class A >
list< T > inverse_idxs (const C1< T, A > &idx, const C2< T, A > &all_idx)
 returns indexes present in all_idx and not present in idx. Both containers have to be sorted.
bool isApprox (double num, double target_num, double error)
template<class T >
linspace_idx (T first, T last, int count, T idx)
template<class T >
list< T > makeSeq (T first, T last, T step)
 make sequence of numbers with numeric type given by T.
template<template< class, class, class, class > class M, class V , class K , class Cmp , template< class > class Alloc>
list< pair< K, V >, Alloc
< pair< K, V > > > 
mapToList (const M< K, V, Cmp, Alloc< pair< const K, V > > > &map)
 map key-value pairs from map to list of key-value pairs. Allocators remain the same.
template<template< class T, class=std::allocator< T > > class C, class T >
C< T > mkContainer (const T &val)
template<template< class, class > class C, class T >
C< T, std::allocator< T > > mkOneValueCont (const T &value)
template<template< class, class > class C, class T >
C< T, Eigen::aligned_allocator
< T > > 
mkOneValueContAA (const T &value)
void points3dSTLToPointMatrix (const Points3dSTL &pts, PointMatrix &ptmat)
template<class Iterator >
void printContainer (Iterator b, Iterator e, const char *separator)
Points3dSTL projectPoints (SE3 T_sd, const Points3dSTL &points)
Matrix3d sE3ToE (const SE3 &T)
 essential matrix from SE3 transform.
std::string seqencedName (int seq, std::string name)
 scheduled for removal XXX. Probably still used.
template<template< class, class > class CT, template< class, class > class CS, class T , template< class > class Alloc>
CT< T, Alloc< T > > switchCont (const CS< T, Alloc< T > > &container)
 swich storage container form container type CS to container type CT. Allocators remain the same.
template<class TR , template< class, class > class C, template< class > class A, class T >
C< TR, typename
EigenAllocTraits< TR >::alloc > 
transformF (const C< T, A< T > > &container, TR(*f)(const T &))
 Transforms values using given function.
template<class T >
bool truePred (const T &t)

Variables

const double PI = std::atan(1.0)*4
boost::mt19937 rng_mt19937
 pseudorandom number generator.

Detailed Description

Universal, globally useful definitions in vslam namespace.

This file is intended to be included by all other sources in vslam namespace.

Definition in file utils.h.


Define Documentation

Definition at line 14 of file utils.h.

Definition at line 13 of file utils.h.

#define ROSCONSOLE_MIN_SEVERITY   ROSCONSOLE_SEVERITY_DEBUG

Definition at line 10 of file utils.h.


Typedef Documentation

typedef ALIGNED<std::pair<int, Eigen::Vector2d> >::vector IdxPoint2dVec

Definition at line 55 of file utils.h.

typedef ALIGNED<std::pair<int, Eigen::Vector3d> >::vector IdxPoint3dVec

Definition at line 54 of file utils.h.

typedef std::pair<Eigen::Vector3d, Eigen::Vector3d> ImageMatch

Definition at line 56 of file utils.h.

typedef ALIGNED<ImageMatch>::vector ImageMatchVec

Definition at line 57 of file utils.h.

typedef Eigen::Matrix<double, 3, Eigen::Dynamic> PointMatrix

Definition at line 267 of file utils.h.

typedef ALIGNED<Eigen::Vector2d>::vector Points2dSTL

Definition at line 53 of file utils.h.

typedef ALIGNED<Eigen::Vector3d>::vector Points3dSTL

Definition at line 52 of file utils.h.


Function Documentation

ImageMatchVec buildImageMatchesFrom3dPoints ( const SE3 &  T_wc1,
const SE3 &  T_wc2,
const Points3dSTL points 
)

Definition at line 39 of file utils.cpp.

Vector2d c1PixelToc2Pixel ( const SE3 &  T_c1c2,
const Vector2d &  pixel1 
) [inline]

Definition at line 601 of file utils.h.

ImageMatch computeMatch ( const Vector2d &  px_c1,
const Vector2d &  px_c2 
) [inline]

Definition at line 251 of file utils.h.

void constructTrajectoryFromSpeedSegments ( std::vector< SpeedSegment speed_segments,
ALIGNED< SE3 >::vector &  transforms 
) [inline]

Definition at line 342 of file utils.h.

Vector3d coordFrameConventionLocalToROS ( const Vector3d &  vec) [inline]

Definition at line 370 of file utils.h.

SE3 coordFrameConventionLocalToROS ( const SE3 &  T) [inline]

Definition at line 375 of file utils.h.

template<class T >
cv::Point cvPointFromVector2 ( Eigen::Matrix< T, 2, 1 >  vec)

Definition at line 258 of file utils.h.

void generateLineTrajectory ( const Vector3d &  start,
const Vector3d &  end,
int  num_stops,
ALIGNED< SE3 >::vector &  transforms 
) [inline]

XXX move to testutils.

Definition at line 292 of file utils.h.

void generateRandomScene ( const Vector3d &  upper_left_deep,
const Vector3d &  bottom_right_shallow,
double  target_num_vec,
Points3dSTL points 
)

XXX move to testutils.

Definition at line 65 of file utils.cpp.

void generateRandomTrajectory ( const Vector3d &  upper_left_deep,
const Vector3d &  bottom_right_shallow,
int  num_stops,
ALIGNED< SE3 >::vector &  transforms 
)

XXX move to testutils.

Definition at line 90 of file utils.cpp.

template<class T >
vector<T> getArrayIdices ( const vector< T > &  array,
const vector< int > &  idxs 
)

select indexes from an array,

Definition at line 570 of file utils.h.

template<class T >
cv::Mat getArrayIdices ( const cv::Mat &  mat,
const vector< int > &  idxs 
)

select rows from a matrix.

Definition at line 581 of file utils.h.

template<template< class, class > class C1, template< class, class > class C2, class T1 , class T2 , template< class > class A1, template< class > class A2>
C1<pair<T1, T2>, A1<pair<T1, T2> > > getByIdx ( const C1< pair< T1, T2 >, A1< pair< T1, T2 > > > &  pts,
const C2< T1, A2< T1 > > &  idx 
)

Select values with given sorted indexes from index-value pairs sorted by index.

Parameters:
idxsorted container with indexes.
ptscontainer with index-value pairs sorted by index.
Returns:
selected index-value pairs.

Definition at line 467 of file utils.h.

template<class C1 , class C2 >
C1 getCommon ( const C1 &  idx1,
const C2 &  idx2 
)

Get indexes that are common to both index sequrces.

Thes function assumes norepeating indexes in ascenting order (in both idx1 and idx2). Both index containers idx1 and idx2 are assumed to be of the same type.

Definition at line 432 of file utils.h.

template<class T1 , class T2 >
T1 getFirst ( const pair< T1, T2 > &  pair)

get pair.first

Definition at line 400 of file utils.h.

geometry_msgs::Pose getPoseFromTcwInSE3 ( const SE3 &  T_cw) [inline]

Definition at line 546 of file utils.h.

template<class T1 , class T2 >
T2 getSecond ( const pair< T1, T2 > &  pair)

get pair.second

Definition at line 405 of file utils.h.

ImageMatchVec imageMatchCoords ( const vector< cv::KeyPoint > &  tkpts,
const vector< cv::KeyPoint > &  qkpts,
const std::vector< cv::DMatch > &  matches 
)

Definition at line 19 of file utils.cpp.

template<template< class, class > class C1, template< class, class > class C2, template< class > class Alloc1, template< class > class Alloc2, class TK , class TV >
list<pair<TK, TV>, Alloc2<pair<TK, TV> > > indexBy ( const C1< TK, Alloc1< TK > > &  idx,
const C2< TV, Alloc2< TV > > &  points 
)

Typical zip function from functional programmming languages.

Definition at line 450 of file utils.h.

template<class T >
T inv_linspace_idx ( first,
last,
int  count,
val 
)

Definition at line 91 of file utils.h.

template<template< class, class > class C1, template< class, class > class C2, class T , class A >
list<T> inverse_idxs ( const C1< T, A > &  idx,
const C2< T, A > &  all_idx 
)

returns indexes present in all_idx and not present in idx. Both containers have to be sorted.

Definition at line 516 of file utils.h.

bool isApprox ( double  num,
double  target_num,
double  error 
)

Definition at line 107 of file utils.cpp.

template<class T >
T linspace_idx ( first,
last,
int  count,
idx 
)

Definition at line 83 of file utils.h.

template<class T >
list<T> makeSeq ( first,
last,
step 
)

make sequence of numbers with numeric type given by T.

Definition at line 504 of file utils.h.

template<template< class, class, class, class > class M, class V , class K , class Cmp , template< class > class Alloc>
list<pair<K, V>, Alloc<pair<K, V> > > mapToList ( const M< K, V, Cmp, Alloc< pair< const K, V > > > &  map)

map key-value pairs from map to list of key-value pairs. Allocators remain the same.

Definition at line 482 of file utils.h.

template<template< class T, class=std::allocator< T > > class C, class T >
C<T> mkContainer ( const T &  val)

Definition at line 595 of file utils.h.

template<template< class, class > class C, class T >
C<T, std::allocator<T> > mkOneValueCont ( const T &  value)

Definition at line 530 of file utils.h.

template<template< class, class > class C, class T >
C<T, Eigen::aligned_allocator<T> > mkOneValueContAA ( const T &  value)

Definition at line 538 of file utils.h.

void points3dSTLToPointMatrix ( const Points3dSTL pts,
PointMatrix ptmat 
)

Definition at line 33 of file utils.cpp.

template<class Iterator >
void printContainer ( Iterator  b,
Iterator  e,
const char *  separator 
)

Definition at line 95 of file utils.h.

Points3dSTL projectPoints ( SE3  T_sd,
const Points3dSTL points 
)

Definition at line 48 of file utils.cpp.

Matrix3d sE3ToE ( const SE3 &  T) [inline]

essential matrix from SE3 transform.

Definition at line 497 of file utils.h.

std::string seqencedName ( int  seq,
std::string  name 
) [inline]

scheduled for removal XXX. Probably still used.

Definition at line 364 of file utils.h.

template<template< class, class > class CT, template< class, class > class CS, class T , template< class > class Alloc>
CT<T, Alloc<T> > switchCont ( const CS< T, Alloc< T > > &  container)

swich storage container form container type CS to container type CT. Allocators remain the same.

Definition at line 491 of file utils.h.

template<class TR , template< class, class > class C, template< class > class A, class T >
C<TR, typename EigenAllocTraits<TR>::alloc > transformF ( const C< T, A< T > > &  container,
TR(*)(const T &)  f 
)

Transforms values using given function.

Parameters:
containercontainer with values to be transformed.
ffunction used to transform each value in the container.
Returns:
transformed values y=f(x). Values are processed in order in which they appear in the input container.

Definition at line 419 of file utils.h.

template<class T >
bool truePred ( const T &  t)

Definition at line 512 of file utils.h.


Variable Documentation

const double PI = std::atan(1.0)*4

Definition at line 42 of file utils.h.

boost::mt19937 rng_mt19937

pseudorandom number generator.

Definition at line 17 of file utils.cpp.

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Defines


pose_estimation
Author(s): Jiri Divis/jiridivis@gmail.com
autogenerated on Wed Mar 27 2013 21:00:13