#include <viso_stereo.h>
Classes | |
struct | parameters |
Public Member Functions | |
std::vector< Matcher::p_match > | getMatches () |
bool | process (uint8_t *I1, uint8_t *I2, int32_t *dims, bool replace=false) |
VisualOdometryStereo (parameters param) | |
~VisualOdometryStereo () | |
Private Types | |
enum | result { UPDATED, FAILED, CONVERGED } |
Private Member Functions | |
void | computeObservations (std::vector< Matcher::p_match > &p_matched, std::vector< int32_t > &active) |
void | computeResidualsAndJacobian (std::vector< double > &tr, std::vector< int32_t > &active) |
void | computeResidualsAndJacobianP3p (TooN::Matrix< 3, 4 > &tr, std::vector< int32_t > &active) |
void | creatAdjMatrix (std::vector< Matcher::p_match > &p_matched, bool **&conn) |
std::vector< double > | estimateMotion (std::vector< Matcher::p_match > p_matched) |
std::vector< double > | estimateMotionLinear (std::vector< Matcher::p_match > p_matched) |
std::vector< double > | estimateMotionMaxClique (std::vector< Matcher::p_match > p_matched) |
std::vector< int32_t > | getInlier (std::vector< Matcher::p_match > &p_matched, std::vector< double > &tr) |
std::vector< int32_t > | getInlier (std::vector< Matcher::p_match > &p_matched, TooN::Matrix< 3, 4 > &tr) |
bool | justice2Matches (Matcher::p_match &m1, Matcher::p_match &m2) |
result | updateParameters (std::vector< Matcher::p_match > &p_matched, std::vector< int32_t > &active, std::vector< double > &tr, double step_size, double eps) |
result | updateParametersLinear (std::vector< Matcher::p_match > &p_matched, std::vector< int32_t > &active, std::vector< double > &tr, double step_size, double eps) |
result | updateParametersP3p (std::vector< Matcher::p_match > &p_matched, std::vector< int32_t > &active, TooN::Matrix< 3, 4 > &tr, double step_size, double eps) |
Private Attributes | |
std::vector< Matcher::p_match > | p_matched1 |
double * | p_residual |
parameters | param |
double | rx_old |
double | ry_old |
double | rz_old |
double | tx_old |
double | ty_old |
double | tz_old |
double * | X |
double * | XC |
double * | Y |
double * | YC |
double * | Z |
double * | ZC |
Definition at line 28 of file viso_stereo.h.
enum VisualOdometryStereo::result [private] |
Definition at line 75 of file viso_stereo.h.
Definition at line 83 of file viso_stereo.cpp.
Definition at line 93 of file viso_stereo.cpp.
void VisualOdometryStereo::computeObservations | ( | std::vector< Matcher::p_match > & | p_matched, |
std::vector< int32_t > & | active | ||
) | [private] |
Definition at line 1038 of file viso_stereo.cpp.
void VisualOdometryStereo::computeResidualsAndJacobian | ( | std::vector< double > & | tr, |
std::vector< int32_t > & | active | ||
) | [private] |
Definition at line 1103 of file viso_stereo.cpp.
void VisualOdometryStereo::computeResidualsAndJacobianP3p | ( | TooN::Matrix< 3, 4 > & | tr, |
std::vector< int32_t > & | active | ||
) | [private] |
Definition at line 1050 of file viso_stereo.cpp.
void VisualOdometryStereo::creatAdjMatrix | ( | std::vector< Matcher::p_match > & | p_matched, |
bool **& | conn | ||
) | [private] |
Definition at line 149 of file viso_stereo.cpp.
vector< double > VisualOdometryStereo::estimateMotion | ( | std::vector< Matcher::p_match > | p_matched | ) | [private, virtual] |
Implements VisualOdometry.
Definition at line 673 of file viso_stereo.cpp.
vector< double > VisualOdometryStereo::estimateMotionLinear | ( | std::vector< Matcher::p_match > | p_matched | ) | [private, virtual] |
Reimplemented from VisualOdometry.
Definition at line 537 of file viso_stereo.cpp.
vector< double > VisualOdometryStereo::estimateMotionMaxClique | ( | std::vector< Matcher::p_match > | p_matched | ) | [private, virtual] |
Reimplemented from VisualOdometry.
Definition at line 167 of file viso_stereo.cpp.
std::vector<int32_t> VisualOdometryStereo::getInlier | ( | std::vector< Matcher::p_match > & | p_matched, |
std::vector< double > & | tr | ||
) | [private] |
std::vector<int32_t> VisualOdometryStereo::getInlier | ( | std::vector< Matcher::p_match > & | p_matched, |
TooN::Matrix< 3, 4 > & | tr | ||
) | [private] |
std::vector<Matcher::p_match> VisualOdometryStereo::getMatches | ( | ) | [inline] |
Definition at line 67 of file viso_stereo.h.
bool VisualOdometryStereo::justice2Matches | ( | Matcher::p_match & | m1, |
Matcher::p_match & | m2 | ||
) | [private] |
Definition at line 119 of file viso_stereo.cpp.
bool VisualOdometryStereo::process | ( | uint8_t * | I1, |
uint8_t * | I2, | ||
int32_t * | dims, | ||
bool | replace = false |
||
) |
Definition at line 95 of file viso_stereo.cpp.
VisualOdometryStereo::result VisualOdometryStereo::updateParameters | ( | std::vector< Matcher::p_match > & | p_matched, |
std::vector< int32_t > & | active, | ||
std::vector< double > & | tr, | ||
double | step_size, | ||
double | eps | ||
) | [private] |
Definition at line 993 of file viso_stereo.cpp.
VisualOdometryStereo::result VisualOdometryStereo::updateParametersLinear | ( | std::vector< Matcher::p_match > & | p_matched, |
std::vector< int32_t > & | active, | ||
std::vector< double > & | tr, | ||
double | step_size, | ||
double | eps | ||
) | [private] |
Definition at line 895 of file viso_stereo.cpp.
VisualOdometryStereo::result VisualOdometryStereo::updateParametersP3p | ( | std::vector< Matcher::p_match > & | p_matched, |
std::vector< int32_t > & | active, | ||
TooN::Matrix< 3, 4 > & | tr, | ||
double | step_size, | ||
double | eps | ||
) | [private] |
Definition at line 862 of file viso_stereo.cpp.
std::vector<Matcher::p_match> VisualOdometryStereo::p_matched1 [private] |
Definition at line 91 of file viso_stereo.h.
double* VisualOdometryStereo::p_residual [private] |
Definition at line 89 of file viso_stereo.h.
parameters VisualOdometryStereo::param [private] |
Reimplemented from VisualOdometry.
Definition at line 93 of file viso_stereo.h.
double VisualOdometryStereo::rx_old [private] |
Definition at line 90 of file viso_stereo.h.
double VisualOdometryStereo::ry_old [private] |
Definition at line 90 of file viso_stereo.h.
double VisualOdometryStereo::rz_old [private] |
Definition at line 90 of file viso_stereo.h.
double VisualOdometryStereo::tx_old [private] |
Definition at line 90 of file viso_stereo.h.
double VisualOdometryStereo::ty_old [private] |
Definition at line 90 of file viso_stereo.h.
double VisualOdometryStereo::tz_old [private] |
Definition at line 90 of file viso_stereo.h.
double* VisualOdometryStereo::X [private] |
Definition at line 88 of file viso_stereo.h.
double * VisualOdometryStereo::XC [private] |
Definition at line 88 of file viso_stereo.h.
double * VisualOdometryStereo::Y [private] |
Definition at line 88 of file viso_stereo.h.
double * VisualOdometryStereo::YC [private] |
Definition at line 88 of file viso_stereo.h.
double * VisualOdometryStereo::Z [private] |
Definition at line 88 of file viso_stereo.h.
double * VisualOdometryStereo::ZC [private] |
Definition at line 88 of file viso_stereo.h.