#include <reconstruction.h>
Classes | |
struct | point2d |
struct | point3d |
struct | track |
Public Member Functions | |
std::vector< point3d > | getPoints () |
Reconstruction () | |
void | setCalibration (FLOAT f, FLOAT cu, FLOAT cv) |
void | update (std::vector< Matcher::p_match > p_matched, Matrix Tr, int32_t point_type=1, int32_t min_track_length=2, double max_dist=30, double min_angle=2) |
~Reconstruction () | |
Private Types | |
enum | result { UPDATED, FAILED, CONVERGED } |
Private Member Functions | |
void | computeObservations (const std::vector< point2d > &p) |
bool | computePredictionsAndJacobian (const std::vector< Matrix >::iterator &P_begin, const std::vector< Matrix >::iterator &P_end, point3d &p) |
bool | initPoint (const track &t, point3d &p) |
double | pointDistance (const track &t, point3d &p) |
int32_t | pointType (const track &t, point3d &p) |
double | rayAngle (const track &t, point3d &p) |
bool | refinePoint (const track &t, point3d &p) |
void | testJacobian () |
result | updatePoint (const track &t, point3d &p, const FLOAT &step_size, const FLOAT &eps) |
Private Attributes | |
FLOAT * | J |
Matrix | K |
FLOAT * | p_observe |
FLOAT * | p_predict |
std::vector< Matrix > | P_total |
std::vector< point3d > | points |
Matrix | Tr_cam_road |
std::vector< Matrix > | Tr_inv_total |
std::vector< Matrix > | Tr_total |
std::vector< track > | tracks |
Definition at line 35 of file reconstruction.h.
enum Reconstruction::result [private] |
Definition at line 86 of file reconstruction.h.
Definition at line 27 of file reconstruction.cpp.
Definition at line 31 of file reconstruction.cpp.
void Reconstruction::computeObservations | ( | const std::vector< point2d > & | p | ) | [private] |
Definition at line 300 of file reconstruction.cpp.
bool Reconstruction::computePredictionsAndJacobian | ( | const std::vector< Matrix >::iterator & | P_begin, |
const std::vector< Matrix >::iterator & | P_end, | ||
point3d & | p | ||
) | [private] |
Definition at line 307 of file reconstruction.cpp.
std::vector<point3d> Reconstruction::getPoints | ( | ) | [inline] |
Definition at line 69 of file reconstruction.h.
bool Reconstruction::initPoint | ( | const track & | t, |
point3d & | p | ||
) | [private] |
Definition at line 144 of file reconstruction.cpp.
double Reconstruction::pointDistance | ( | const track & | t, |
point3d & | p | ||
) | [private] |
Definition at line 200 of file reconstruction.cpp.
int32_t Reconstruction::pointType | ( | const track & | t, |
point3d & | p | ||
) | [private] |
Definition at line 226 of file reconstruction.cpp.
double Reconstruction::rayAngle | ( | const track & | t, |
point3d & | p | ||
) | [private] |
Definition at line 208 of file reconstruction.cpp.
bool Reconstruction::refinePoint | ( | const track & | t, |
point3d & | p | ||
) | [private] |
Definition at line 175 of file reconstruction.cpp.
void Reconstruction::setCalibration | ( | FLOAT | f, |
FLOAT | cu, | ||
FLOAT | cv | ||
) |
Definition at line 34 of file reconstruction.cpp.
void Reconstruction::testJacobian | ( | ) | [private] |
Definition at line 342 of file reconstruction.cpp.
void Reconstruction::update | ( | std::vector< Matcher::p_match > | p_matched, |
Matrix | Tr, | ||
int32_t | point_type = 1 , |
||
int32_t | min_track_length = 2 , |
||
double | max_dist = 30 , |
||
double | min_angle = 2 |
||
) |
Definition at line 50 of file reconstruction.cpp.
Reconstruction::result Reconstruction::updatePoint | ( | const track & | t, |
point3d & | p, | ||
const FLOAT & | step_size, | ||
const FLOAT & | eps | ||
) | [private] |
Definition at line 254 of file reconstruction.cpp.
FLOAT* Reconstruction::J [private] |
Definition at line 107 of file reconstruction.h.
Matrix Reconstruction::K [private] |
Definition at line 99 of file reconstruction.h.
FLOAT* Reconstruction::p_observe [private] |
Definition at line 108 of file reconstruction.h.
FLOAT * Reconstruction::p_predict [private] |
Definition at line 108 of file reconstruction.h.
std::vector<Matrix> Reconstruction::P_total [private] |
Definition at line 104 of file reconstruction.h.
std::vector<point3d> Reconstruction::points [private] |
Definition at line 105 of file reconstruction.h.
Matrix Reconstruction::Tr_cam_road [private] |
Definition at line 99 of file reconstruction.h.
std::vector<Matrix> Reconstruction::Tr_inv_total [private] |
Definition at line 103 of file reconstruction.h.
std::vector<Matrix> Reconstruction::Tr_total [private] |
Definition at line 102 of file reconstruction.h.
std::vector<track> Reconstruction::tracks [private] |
Definition at line 101 of file reconstruction.h.