libstereo-odometry (C++ Library for robust Stereo odometry)
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Classes | Public Member Functions | Private Types | Private Member Functions | Private Attributes | List of all members
rso::CStereoOdometryEstimator Class Reference

#include <libstereo-odometry.h>

Classes

struct  TDetectParams
 
struct  TGeneralParams
 
struct  TGUIParams
 
struct  TImagePairData
 
struct  TInfoForTheGUI
 
struct  TInterFrameMatchingParams
 
struct  TLeastSquaresParams
 
struct  TLeftRightMatchParams
 
struct  TRectifyParams
 
struct  TStereoOdometryRequest
 
struct  TStereoOdometryResult
 
struct  TTrackedPixels
 
struct  TTrackingData
 

Public Member Functions

 CStereoOdometryEstimator ()
 
 ~CStereoOdometryEstimator ()
 
void enable_time_profiler (bool enable=true)
 
mrpt::utils::CTimeLogger & get_time_profiler ()
 
void setVerbosityLevel (int level)
 
int getFASTThreshold ()
 
void setFASTThreshold (int value)
 
void resetFASTThreshold ()
 
bool isFASTThMin ()
 
bool isFASTThMax ()
 
int getORBThreshold ()
 
void setORBThreshold (int value)
 
void resetORBThreshold ()
 
bool isORBThMin ()
 
bool isORBThMax ()
 
bool keyPressedOnGUI ()
 Return true if any key was pressed on the GUI window. More...
 
int getKeyPressedOnGUI ()
 
void loadParamsFromConfigFile (const mrpt::utils::CConfigFile &iniFile, const std::vector< std::string > &sections)
 
void loadParamsFromConfigFileName (const std::string &fileName, const std::vector< std::string > &sections)
 
void setThisFrameAsKF ()
 
void resetIds ()
 
void setIds (const vector< size_t > &ids)
 
CImage & getRefCurrentImageLeft ()
 
CImage & getRefCurrentImageRight ()
 
CImage getCopyCurrentImageLeft ()
 
CImage getCopyCurrentImageRight ()
 
vector< size_t > & getRefCurrentIDs (const size_t octave)
 
void getValues (vector< cv::KeyPoint > &leftKP, vector< cv::KeyPoint > &rightKP, cv::Mat &leftDesc, cv::Mat &rightDesc, vector< cv::DMatch > &matches, vector< size_t > &matches_id)
 
void setMaxMatchID (const size_t id)
 
Main API methods
void processNewImagePair (TStereoOdometryRequest &request_data, TStereoOdometryResult &result)
 
bool getChangeInPose (const vector_index_pairs_t &tracked_pairs, const vector< cv::DMatch > &pre_matches, const vector< cv::DMatch > &cur_matches, const vector< cv::KeyPoint > &pre_left_feats, const vector< cv::KeyPoint > &pre_right_feats, const vector< cv::KeyPoint > &cur_left_feats, const vector< cv::KeyPoint > &cur_right_feats, const mrpt::utils::TStereoCamera &stereo_camera, TStereoOdometryResult &result, const vector< double > &ini_estimation=vector< double >(6, 0))
 
void getProjectedCoords (const vector< cv::DMatch > &pre_matches, const vector< cv::KeyPoint > &pre_left_feats, const vector< cv::KeyPoint > &pre_right_feats, const vector< pair< int, float > > &other_matches_tracked, const mrpt::utils::TStereoCamera &stereo_camera, const CPose3D &change_pose, vector< pair< TPixelCoordf, TPixelCoordf > > &pro_pre_feats)
 
bool saveStateToFile (const string &filename)
 
bool loadStateFromFile (const string &filename)
 
void dumpToConsole ()
 

Public Attributes

Public data fields
TRectifyParams params_rectify
 
TDetectParams params_detect
 
TLeftRightMatchParams params_lr_match
 
TInterFrameMatchingParams params_if_match
 
TLeastSquaresParams params_least_squares
 
TGUIParams params_gui
 
TGeneralParams params_general
 

Private Types

typedef stlplus::smart_ptr
< TImagePairData > 		TImagePairDataPtr
 

Private Member Functions

void m_update_indexes (TImagePairData::img_data_t &data, size_t octave, const bool order)
 
void m_featlist_to_kpslist (CStereoOdometryEstimator::TImagePairData::img_data_t &img_data)
 
void m_adaptive_non_max_sup (const size_t &num_out_points, const vector< cv::KeyPoint > &keypoints, const cv::Mat &descriptors, vector< cv::KeyPoint > &out_kp_rad, cv::Mat &out_kp_desc, const double &min_radius_th=0)
 
void m_non_max_sup (const size_t &num_out_points, const vector< cv::KeyPoint > &keypoints, const cv::Mat &descriptors, vector< cv::KeyPoint > &out_kp_rad, cv::Mat &out_kp_desc, const size_t &imgH, const size_t &imgW, vector< bool > &survivors)
 
void m_non_max_sup (const vector< cv::KeyPoint > &keypoints, vector< bool > &survivors, const size_t &imgH, const size_t &imgW, const size_t &num_out_points) const
 
bool m_evalRGN (const TKeyPointList &list1_l, const TKeyPointList &list1_r, const TKeyPointList &list2_l, const TKeyPointList &list2_r, const vector< bool > &mask, const vector< TPoint3D > &lmks, const vector< double > &deltaPose, const mrpt::utils::TStereoCamera &stereoCam, Eigen::MatrixXd &out_newPose, Eigen::MatrixXd &out_gradient, vector< double > &out_residual, double &out_cost, VOErrorCode &out_error_code)
 
void m_pinhole_stereo_projection (const vector< TPoint3D > &lmks, const TStereoCamera &cam, const vector< double > &delta_pose, vector< pair< TPixelCoordf, TPixelCoordf > > &out_pixels, vector< Eigen::MatrixXd > &out_jacobian)
 
bool m_dump_keypoints_to_stream (std::ofstream &stream, const vector< cv::KeyPoint > &keypoints, const cv::Mat &descriptors)
 
bool m_dump_matches_to_stream (std::ofstream &stream, const vector< cv::DMatch > &matches, const vector< size_t > &matches_ids)
 
bool m_load_keypoints_from_stream (std::ifstream &stream, vector< cv::KeyPoint > &keypoints, cv::Mat &descriptors)
 
bool m_load_matches_from_stream (std::ifstream &stream, vector< cv::DMatch > &matches, vector< size_t > &matches_ids)
 
bool m_jacobian_is_good (Eigen::MatrixXd &jacobian)
 
void m_filter_by_fundmatrix (const vector< cv::Point2f > &prevPts, const vector< cv::Point2f > &nextPts, vector< uchar > &status) const
 
void stage1_prepare_rectify (TStereoOdometryRequest &in_imgs, TImagePairData &out_imgpair)
 
void stage2_detect_features (TImagePairData::img_data_t &img_data, mrpt::utils::CImage &gui_image, bool update_dyn_thresholds=false)
 
void stage3_match_left_right (CStereoOdometryEstimator::TImagePairData &imgpair, const TStereoCamera &stereoCamera)
 
void stage4_track (TTrackingData &out_tracked_feats, TImagePairData &prev_imgpair, TImagePairData &cur_imgpair)
 
void stage5_optimize (TTrackingData &out_tracked_feats, const mrpt::utils::TStereoCamera &stereoCam, TStereoOdometryResult &result, const vector< double > &initial_estimation=vector< double >(6, 0))
 

Private Attributes

mrpt::utils::CTimeLogger m_profiler
 
size_t m_lastID
 Identificator of the last tracked feature. More...
 
size_t m_num_tracked_pairs_from_last_kf
 
size_t m_num_tracked_pairs_from_last_frame
 
size_t m_last_kf_max_id
 Maximum ID of a match belonging to certain frame defined as 'KF'. More...
 
bool m_reset
 
vector< vector< size_t > > m_kf_ids
 
size_t m_last_match_ID
 Identificator of the last match ID. More...
 
size_t m_kf_max_match_ID
 
int m_current_fast_th
 
int m_current_orb_th
 
bool m_error_in_tracking
 
VOErrorCode m_error
 
int m_verbose_level
 0: None (only critical msgs), 1: verbose, 2:even more verbose More...
 
unsigned int m_it_counter
 Iteration counter. More...
 
vector< double > m_last_computed_pose
 
TImagePairDataPtr m_current_imgpair
 
TImagePairDataPtr m_prev_imgpair
 
mrpt::vision::CStereoRectifyMap m_stereo_rectifier
 
std::vector< int > m_threshold
 

GUI stuff

mrpt::system::TThreadHandle m_thread_gui
 
bool m_threads_must_close
 set to true at destruction to signal all threads that they must end. More...
 
int m_win_keyhit
 
TInfoForTheGUI * m_next_gui_info
 
TInfoForTheGUI m_gui_info_cache [2]
 We only need two instances of this struct at once: one for writing the new data, and the "final" one sent to the GUI thread. More...
 
int m_gui_info_cache_next_index
 Will alternate between 0 and 1. More...
 
TInfoForTheGUI * m_gui_info
 
mrpt::synch::CCriticalSection m_gui_info_cs
 
void thread_gui ()
 

Detailed Description

The main class of libstereo-odometry: it implements the Stereo Odometry problem and holds all the related options and parameters.

Member Typedef Documentation

typedef stlplus::smart_ptr<TImagePairData> rso::CStereoOdometryEstimator::TImagePairDataPtr
private

Constructor & Destructor Documentation

rso::CStereoOdometryEstimator::CStereoOdometryEstimator ( )

Default constructor

rso::CStereoOdometryEstimator::~CStereoOdometryEstimator ( )

Destructor: close windows & clean up

Member Function Documentation

void rso::CStereoOdometryEstimator::dumpToConsole ( )
inline
void rso::CStereoOdometryEstimator::enable_time_profiler ( bool  enable = true)
inline

Enable or disables time profiling of all operations (default=enabled), which will be reported upon destruction

mrpt::utils::CTimeLogger& rso::CStereoOdometryEstimator::get_time_profiler ( )
inline

Access to the time profiler

bool rso::CStereoOdometryEstimator::getChangeInPose ( const vector_index_pairs_t &  tracked_pairs,
const vector< cv::DMatch > &  pre_matches,
const vector< cv::DMatch > &  cur_matches,
const vector< cv::KeyPoint > &  pre_left_feats,
const vector< cv::KeyPoint > &  pre_right_feats,
const vector< cv::KeyPoint > &  cur_left_feats,
const vector< cv::KeyPoint > &  cur_right_feats,
const mrpt::utils::TStereoCamera &  stereo_camera,
TStereoOdometryResult &  result,
const vector< double > &  ini_estimation = vector< double >(6, 0) 
)

this custom method gets info from two different frames and computes the change in pose between them

CImage rso::CStereoOdometryEstimator::getCopyCurrentImageLeft ( )
inline
CImage rso::CStereoOdometryEstimator::getCopyCurrentImageRight ( )
inline
int rso::CStereoOdometryEstimator::getFASTThreshold ( )
inline

Sets and gets FAST detector threshold (within ORB)

int rso::CStereoOdometryEstimator::getKeyPressedOnGUI ( )

Return the key code of the last key pressed on the GUI window: ==0: means no key pressed on window; >0: is the key code, -1: the window was closed by the user.

See Also
getKeyPressedOnGUI(), keyPressedOnGUI()
int rso::CStereoOdometryEstimator::getORBThreshold ( )
inline

Sets and gets ORB matching threshold

void rso::CStereoOdometryEstimator::getProjectedCoords ( const vector< cv::DMatch > &  pre_matches,
const vector< cv::KeyPoint > &  pre_left_feats,
const vector< cv::KeyPoint > &  pre_right_feats,
const vector< pair< int, float > > &  other_matches_tracked,
const mrpt::utils::TStereoCamera &  stereo_camera,
const CPose3D &  change_pose,
vector< pair< TPixelCoordf, TPixelCoordf > > &  pro_pre_feats 
)
vector<size_t>& rso::CStereoOdometryEstimator::getRefCurrentIDs ( const size_t  octave)
inline
CImage& rso::CStereoOdometryEstimator::getRefCurrentImageLeft ( )
inline
CImage& rso::CStereoOdometryEstimator::getRefCurrentImageRight ( )
inline
void rso::CStereoOdometryEstimator::getValues ( vector< cv::KeyPoint > &  leftKP,
vector< cv::KeyPoint > &  rightKP,
cv::Mat &  leftDesc,
cv::Mat &  rightDesc,
vector< cv::DMatch > &  matches,
vector< size_t > &  matches_id 
)
inline

Returns copies to the inner structures

bool rso::CStereoOdometryEstimator::isFASTThMax ( )
inline
bool rso::CStereoOdometryEstimator::isFASTThMin ( )
inline
bool rso::CStereoOdometryEstimator::isORBThMax ( )
inline
bool rso::CStereoOdometryEstimator::isORBThMin ( )
inline
bool rso::CStereoOdometryEstimator::keyPressedOnGUI ( )

Return true if any key was pressed on the GUI window.

See Also
getKeyPressedOnGUI()
void rso::CStereoOdometryEstimator::loadParamsFromConfigFile ( const mrpt::utils::CConfigFile &  iniFile,
const std::vector< std::string > &  sections 
)
inline

Loads configuration from an INI file Sections must be (in this order) related to: RECTIFY, DETECT, MATCH, IF-MATCH, LEAST_SQUARES, GUI, GENERAL

void rso::CStereoOdometryEstimator::loadParamsFromConfigFileName ( const std::string &  fileName,
const std::vector< std::string > &  sections 
)
inline

Loads configuration from an INI file from its name

bool rso::CStereoOdometryEstimator::loadStateFromFile ( const string &  filename)
void rso::CStereoOdometryEstimator::m_adaptive_non_max_sup ( const size_t &  num_out_points,
const vector< cv::KeyPoint > &  keypoints,
const cv::Mat &  descriptors,
vector< cv::KeyPoint > &  out_kp_rad,
cv::Mat &  out_kp_desc,
const double &  min_radius_th = 0 
)
private

Performs adaptive non maximal suppression for the detected features

bool rso::CStereoOdometryEstimator::m_dump_keypoints_to_stream ( std::ofstream &  stream,
const vector< cv::KeyPoint > &  keypoints,
const cv::Mat &  descriptors 
)
private

Saves a vector of keypoints to a strem

bool rso::CStereoOdometryEstimator::m_dump_matches_to_stream ( std::ofstream &  stream,
const vector< cv::DMatch > &  matches,
const vector< size_t > &  matches_ids 
)
private

Saves the matche sto a strem

bool rso::CStereoOdometryEstimator::m_evalRGN ( const TKeyPointList &  list1_l,
const TKeyPointList &  list1_r,
const TKeyPointList &  list2_l,
const TKeyPointList &  list2_r,
const vector< bool > &  mask,
const vector< TPoint3D > &  lmks,
const vector< double > &  deltaPose,
const mrpt::utils::TStereoCamera &  stereoCam,
Eigen::MatrixXd &  out_newPose,
Eigen::MatrixXd &  out_gradient,
vector< double > &  out_residual,
double &  out_cost,
VOErrorCode &  out_error_code 
)
private

Performs one iteration of a robust Gauss-Newton minimization for the visual odometry

void rso::CStereoOdometryEstimator::m_featlist_to_kpslist ( CStereoOdometryEstimator::TImagePairData::img_data_t &  img_data)
private

Updates the row index matrix

void rso::CStereoOdometryEstimator::m_filter_by_fundmatrix ( const vector< cv::Point2f > &  prevPts,
const vector< cv::Point2f > &  nextPts,
vector< uchar > &  status 
) const
private
bool rso::CStereoOdometryEstimator::m_jacobian_is_good ( Eigen::MatrixXd &  jacobian)
inlineprivate
bool rso::CStereoOdometryEstimator::m_load_keypoints_from_stream ( std::ifstream &  stream,
vector< cv::KeyPoint > &  keypoints,
cv::Mat &  descriptors 
)
private
bool rso::CStereoOdometryEstimator::m_load_matches_from_stream ( std::ifstream &  stream,
vector< cv::DMatch > &  matches,
vector< size_t > &  matches_ids 
)
private
void rso::CStereoOdometryEstimator::m_non_max_sup ( const size_t &  num_out_points,
const vector< cv::KeyPoint > &  keypoints,
const cv::Mat &  descriptors,
vector< cv::KeyPoint > &  out_kp_rad,
cv::Mat &  out_kp_desc,
const size_t &  imgH,
const size_t &  imgW,
vector< bool > &  survivors 
)
private

Performs non maximal suppression for the detected features

void rso::CStereoOdometryEstimator::m_non_max_sup ( const vector< cv::KeyPoint > &  keypoints,
vector< bool > &  survivors,
const size_t &  imgH,
const size_t &  imgW,
const size_t &  num_out_points 
) const
private

Performs non maximal suppression for the detected features without taking into account the descriptors (auxiliary method)

void rso::CStereoOdometryEstimator::m_pinhole_stereo_projection ( const vector< TPoint3D > &  lmks,
const TStereoCamera &  cam,
const vector< double > &  delta_pose,
vector< pair< TPixelCoordf, TPixelCoordf > > &  out_pixels,
vector< Eigen::MatrixXd > &  out_jacobian 
)
private
void rso::CStereoOdometryEstimator::m_update_indexes ( TImagePairData::img_data_t &  data,
size_t  octave,
const bool  order 
)
private

Updates the row index matrix

void rso::CStereoOdometryEstimator::processNewImagePair ( TStereoOdometryRequest &  request_data,
TStereoOdometryResult &  result 
)

The main entry point: process a new pair of images and returns the estimated camera pose increment.

void rso::CStereoOdometryEstimator::resetFASTThreshold ( )
inline
void rso::CStereoOdometryEstimator::resetIds ( )
inline
void rso::CStereoOdometryEstimator::resetORBThreshold ( )
inline
bool rso::CStereoOdometryEstimator::saveStateToFile ( const string &  filename)
void rso::CStereoOdometryEstimator::setFASTThreshold ( int  value)
inline
void rso::CStereoOdometryEstimator::setIds ( const vector< size_t > &  ids)
inline
void rso::CStereoOdometryEstimator::setMaxMatchID ( const size_t  id)
inline
void rso::CStereoOdometryEstimator::setORBThreshold ( int  value)
inline
void rso::CStereoOdometryEstimator::setThisFrameAsKF ( )
inline

Sets/resets the match IDs generator

void rso::CStereoOdometryEstimator::setVerbosityLevel ( int  level)
inline

Changes the verbosity level: 0=None (only critical msgs), 1=verbose, 2=so verbose you'll have to say "Stop!"

void rso::CStereoOdometryEstimator::stage1_prepare_rectify ( TStereoOdometryRequest &  in_imgs,
TImagePairData &  out_imgpair 
)
private

Stage1 operations:

  • Convert to grayscale (if input is RGB)
  • Rectify stereo images (if input not already rectified)
  • Build pyramid
void rso::CStereoOdometryEstimator::stage2_detect_features ( TImagePairData::img_data_t &  img_data,
mrpt::utils::CImage &  gui_image,
bool  update_dyn_thresholds = false 
)
private

Stage2 operations:

  • Detect features on each image and on each scale.
void rso::CStereoOdometryEstimator::stage3_match_left_right ( CStereoOdometryEstimator::TImagePairData &  imgpair,
const TStereoCamera &  stereoCamera 
)
private

Stage3 operations:

  • Match features between L/R images.
void rso::CStereoOdometryEstimator::stage4_track ( TTrackingData &  out_tracked_feats,
TImagePairData &  prev_imgpair,
TImagePairData &  cur_imgpair 
)
private

Stage4 operations:

  • Track features in both L/R images between two consecutive time steps.
  • Robustness checks for tracking.
void rso::CStereoOdometryEstimator::stage5_optimize ( TTrackingData &  out_tracked_feats,
const mrpt::utils::TStereoCamera &  stereoCam,
TStereoOdometryResult &  result,
const vector< double > &  initial_estimation = vector< double >(6, 0) 
)
private

Stage5 operations:

  • Estimate the optimal change in pose between time steps.
void rso::CStereoOdometryEstimator::thread_gui ( )
private

The thread for updating the GUI

Member Data Documentation

int rso::CStereoOdometryEstimator::m_current_fast_th
private
TImagePairDataPtr rso::CStereoOdometryEstimator::m_current_imgpair
private

At any time step, the current (latest) and previous pair of stereo images, already processed as pyramids, detectors.

int rso::CStereoOdometryEstimator::m_current_orb_th
private
VOErrorCode rso::CStereoOdometryEstimator::m_error
private
bool rso::CStereoOdometryEstimator::m_error_in_tracking
private
TInfoForTheGUI* rso::CStereoOdometryEstimator::m_gui_info
private

This variable is set by processNewImagePair()

TInfoForTheGUI rso::CStereoOdometryEstimator::m_gui_info_cache[2]
private

We only need two instances of this struct at once: one for writing the new data, and the "final" one sent to the GUI thread.

int rso::CStereoOdometryEstimator::m_gui_info_cache_next_index
private

Will alternate between 0 and 1.

mrpt::synch::CCriticalSection rso::CStereoOdometryEstimator::m_gui_info_cs
private
unsigned int rso::CStereoOdometryEstimator::m_it_counter
private

Iteration counter.

vector< vector<size_t> > rso::CStereoOdometryEstimator::m_kf_ids
private
size_t rso::CStereoOdometryEstimator::m_kf_max_match_ID
private
vector<double> rso::CStereoOdometryEstimator::m_last_computed_pose
private
size_t rso::CStereoOdometryEstimator::m_last_kf_max_id
private

Maximum ID of a match belonging to certain frame defined as 'KF'.

size_t rso::CStereoOdometryEstimator::m_last_match_ID
private

Identificator of the last match ID.

size_t rso::CStereoOdometryEstimator::m_lastID
private

Identificator of the last tracked feature.

TInfoForTheGUI* rso::CStereoOdometryEstimator::m_next_gui_info
private

Each stage may add or modify this struct, which upon complete processing of one iteration, if sent to the GUI thread via m_gui_info

size_t rso::CStereoOdometryEstimator::m_num_tracked_pairs_from_last_frame
private
size_t rso::CStereoOdometryEstimator::m_num_tracked_pairs_from_last_kf
private
TImagePairDataPtr rso::CStereoOdometryEstimator::m_prev_imgpair
private
mrpt::utils::CTimeLogger rso::CStereoOdometryEstimator::m_profiler
mutableprivate

Profiler for all SRBA operations Enabled by default, can be disabled with enable_time_profiler(false)

bool rso::CStereoOdometryEstimator::m_reset
private
mrpt::vision::CStereoRectifyMap rso::CStereoOdometryEstimator::m_stereo_rectifier
private
mrpt::system::TThreadHandle rso::CStereoOdometryEstimator::m_thread_gui
private
bool rso::CStereoOdometryEstimator::m_threads_must_close
private

set to true at destruction to signal all threads that they must end.

std::vector<int> rso::CStereoOdometryEstimator::m_threshold
private
int rso::CStereoOdometryEstimator::m_verbose_level
private

0: None (only critical msgs), 1: verbose, 2:even more verbose

int rso::CStereoOdometryEstimator::m_win_keyhit
private

==0: means no key pressed on window; >0: is the key code, -1: the window was closed by the user.

See Also
getKeyPressedOnGUI(), keyPressedOnGUI()
TDetectParams rso::CStereoOdometryEstimator::params_detect
TGeneralParams rso::CStereoOdometryEstimator::params_general
TGUIParams rso::CStereoOdometryEstimator::params_gui
TInterFrameMatchingParams rso::CStereoOdometryEstimator::params_if_match
TLeastSquaresParams rso::CStereoOdometryEstimator::params_least_squares
TLeftRightMatchParams rso::CStereoOdometryEstimator::params_lr_match
TRectifyParams rso::CStereoOdometryEstimator::params_rectify

Different parameters for the SRBA methods

The documentation for this class was generated from the following file:
Generated on Tue Jan 19 2016 10:40:30 for libstereo-odometry (C++ Library for robust Stereo odometry) by   doxygen 1.8.6