#include <lc.h>
Classes | |
struct | Params |
Public Member Functions | |
void | finalize () |
Finalizes the loop closure class. | |
void | getCandidates (vector< pair< int, float > > &candidates) |
Get the best n_candidates to close loop with the last image. | |
void | getCandidates (int image_id, vector< pair< int, float > > &candidates) |
Get the best n_candidates to close loop with the image specified by id. | |
bool | getHash (int img_id, vector< float > &hash) |
Returns the hash for a some specific image id. | |
bool | getLoopClosure (int &lc_img_id) |
Try to find a loop closure between last node and all other nodes. | |
bool | getLoopClosure (int &lc_img_id, tf::Transform &trans) |
Try to find a loop closure between last node and all other nodes. | |
bool | getLoopClosure (int img_id_a, int img_id_b, tf::Transform &trans, bool logging=false) |
Compute the loop closure (if any) between A -> B. | |
bool | getLoopClosure (int img_id_a, int img_id_b, tf::Transform &trans, int &matches, int &inliers, bool logging=false) |
Compute the loop closure (if any) between A -> B. | |
bool | hashMatching (int img_id_a, int img_id_b, float &matching) |
Computes the hash matching between 2 images. | |
void | init () |
Initializes the loop closure class. | |
LoopClosure () | |
LoopClosure class constructor. | |
Params | params () const |
void | setCameraModel (image_geometry::StereoCameraModel stereo_camera_model, cv::Mat camera_matrix) |
Sets the camera model. | |
int | setNode (cv::Mat img) |
int | setNode (cv::Mat img_l, cv::Mat img_r) |
void | setParams (const Params ¶ms) |
Sets the class parameters. | |
Private Member Functions | |
void | buildLikelihoodVector (vector< pair< int, float > > hash_matchings, vector< pair< int, float > > &likelihood) |
Compute the likelihood vectors given a certain hash matching set. | |
bool | compute (Image query, Image candidate, int &matches, int &inliers, tf::Transform &trans) |
Compute the loop closure (if any). | |
void | getBestMatchings (int image_id, int best_n, vector< pair< int, float > > &best_matchings) |
Retrieve the best n matchings give a certain image id. | |
Image | getImage (string img_file) |
Get the image information from file. | |
void | getMatchingsLikelihood (vector< pair< int, float > > matchings, vector< float > &matchings_likelihood, vector< pair< int, float > > cur_likelihood, vector< pair< int, float > > prev_likelihood) |
Given a vector of matches and the current and previous likelihood vectors, returns the likelihood for these matches. | |
void | groupSimilarImages (vector< pair< int, float > > matchings, vector< vector< int > > &groups) |
Group the matchings by images with similar id. | |
Private Attributes | |
cv::Mat | camera_matrix_ |
> Incremental index for the stored images | |
Hash | hash_ |
> Query image object | |
vector< pair< int, vector < float > > > | hash_table_ |
> Used to save the camera matrix | |
int | img_id_ |
> Hash object | |
vector< int > | lc_candidate_positions_ |
> Hash table | |
vector< pair< int, int > > | lc_found_ |
> Stores the previous likelihood vector | |
Params | params_ |
vector< pair< int, float > > | prev_likelihood_ |
> Loop closure candidate positions | |
Image | query_ |
> Stores parameters |
LoopClosure class constructor.
void haloc::LoopClosure::buildLikelihoodVector | ( | vector< pair< int, float > > | hash_matchings, |
vector< pair< int, float > > & | likelihood | ||
) | [private] |
bool haloc::LoopClosure::compute | ( | Image | query, |
Image | candidate, | ||
int & | matches, | ||
int & | inliers, | ||
tf::Transform & | trans | ||
) | [private] |
Compute the loop closure (if any).
Query | image object. |
Candidate | image object. |
Return | the number of matches found. |
Return | the number of inliers found. |
Return | the transform between nodes if loop closure is valid. |
void haloc::LoopClosure::finalize | ( | ) |
void haloc::LoopClosure::getBestMatchings | ( | int | image_id, |
int | best_n, | ||
vector< pair< int, float > > & | best_matchings | ||
) | [private] |
void haloc::LoopClosure::getCandidates | ( | vector< pair< int, float > > & | candidates | ) |
void haloc::LoopClosure::getCandidates | ( | int | image_id, |
vector< pair< int, float > > & | candidates | ||
) |
bool haloc::LoopClosure::getHash | ( | int | img_id, |
vector< float > & | hash | ||
) |
haloc::Image haloc::LoopClosure::getImage | ( | string | img_file | ) | [private] |
bool haloc::LoopClosure::getLoopClosure | ( | int & | lc_img_id | ) |
bool haloc::LoopClosure::getLoopClosure | ( | int & | lc_img_id, |
tf::Transform & | trans | ||
) |
bool haloc::LoopClosure::getLoopClosure | ( | int | img_id_a, |
int | img_id_b, | ||
tf::Transform & | trans, | ||
bool | logging = false |
||
) |
bool haloc::LoopClosure::getLoopClosure | ( | int | img_id_a, |
int | img_id_b, | ||
tf::Transform & | trans, | ||
int & | matches, | ||
int & | inliers, | ||
bool | logging = false |
||
) |
Compute the loop closure (if any) between A -> B.
reference | image id. |
current | image id. |
Return | the transform between nodes if loop closure is valid. |
Show | output log or not. |
The | number of matches |
The | number of inliers |
void haloc::LoopClosure::getMatchingsLikelihood | ( | vector< pair< int, float > > | matchings, |
vector< float > & | matchings_likelihood, | ||
vector< pair< int, float > > | cur_likelihood, | ||
vector< pair< int, float > > | prev_likelihood | ||
) | [private] |
Given a vector of matches and the current and previous likelihood vectors, returns the likelihood for these matches.
Hash | matching vector in the format <image_id, distance>. |
Stores | the likelihood for the given matching vectors |
Current | vector of likelihood in the format <image_id, probability>. |
Previous | vector of likelihood in the format <image_id, probability>. |
void haloc::LoopClosure::groupSimilarImages | ( | vector< pair< int, float > > | matchings, |
vector< vector< int > > & | groups | ||
) | [private] |
bool haloc::LoopClosure::hashMatching | ( | int | img_id_a, |
int | img_id_b, | ||
float & | matching | ||
) |
void haloc::LoopClosure::init | ( | ) |
Params haloc::LoopClosure::params | ( | ) | const [inline] |
void haloc::LoopClosure::setCameraModel | ( | image_geometry::StereoCameraModel | stereo_camera_model, |
cv::Mat | camera_matrix | ||
) |
int haloc::LoopClosure::setNode | ( | cv::Mat | img | ) |
int haloc::LoopClosure::setNode | ( | cv::Mat | img_l, |
cv::Mat | img_r | ||
) |
void haloc::LoopClosure::setParams | ( | const Params & | params | ) |
cv::Mat haloc::LoopClosure::camera_matrix_ [private] |
Hash haloc::LoopClosure::hash_ [private] |
vector< pair<int, vector<float> > > haloc::LoopClosure::hash_table_ [private] |
int haloc::LoopClosure::img_id_ [private] |
vector<int> haloc::LoopClosure::lc_candidate_positions_ [private] |
vector< pair<int, int > > haloc::LoopClosure::lc_found_ [private] |
Params haloc::LoopClosure::params_ [private] |
vector< pair<int,float> > haloc::LoopClosure::prev_likelihood_ [private] |
Image haloc::LoopClosure::query_ [private] |