NASA Astrobee Robot Software
0.19.1
Flight software for the Astrobee robots operating inside the International Space Station.
|
#include <sparse_map.h>
Public Member Functions | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | SparseMap (const std::vector< std::string > &filenames, const std::string &detector, const camera::CameraParameters ¶ms) |
SparseMap (const std::string &protobuf_file, bool localization=false) | |
SparseMap (const std::vector< Eigen::Affine3d > &cid_to_cam_t, const std::vector< std::string > &filenames, const std::string &detector, const camera::CameraParameters ¶ms) | |
SparseMap (bool bundler_format, std::string const &filename, std::vector< std::string > const &files) | |
void | SetDefaultLocParams () |
void | SetLocParams (const LocalizationParameters &loc_params) |
const LocalizationParameters & | loc_params () const |
LocalizationParameters & | loc_params () |
void | SetDetectorParams (int min_features, int max_features, int retries, double min_thresh, double default_thresh, double max_thresh, double too_many_ratio, double too_few_ratio) |
void | DetectFeatures () |
void | Save (const std::string &protobuf_file) const |
bool | Localize (std::string const &img_file, camera::CameraModel *pose, std::vector< Eigen::Vector3d > *inlier_landmarks=NULL, std::vector< Eigen::Vector2d > *inlier_observations=NULL, std::vector< int > *cid_list=NULL) |
bool | Localize (const cv::Mat &image, camera::CameraModel *pose, std::vector< Eigen::Vector3d > *inlier_landmarks=NULL, std::vector< Eigen::Vector2d > *inlier_observations=NULL, std::vector< int > *cid_list=NULL) |
bool | Localize (cv::Mat const &test_descriptors, Eigen::Matrix2Xd const &test_keypoints, camera::CameraModel *pose, std::vector< Eigen::Vector3d > *inlier_landmarks, std::vector< Eigen::Vector2d > *inlier_observations, std::vector< int > *cid_list=NULL, const cv::Mat &image=cv::Mat()) |
size_t | GetNumFrames (void) const |
const std::string & | GetFrameFilename (int frame) const |
const Eigen::Affine3d & | GetFrameGlobalTransform (int frame) const |
void | SetFrameGlobalTransform (int frame, const Eigen::Affine3d &transform) |
const Eigen::Matrix2Xd & | GetFrameKeypoints (int frame) const |
cv::Mat | GetDescriptor (int frame, int fid) const |
const std::map< int, int > & | GetFrameFidToPidMap (int frame) const |
size_t | GetNumLandmarks (void) const |
Eigen::Vector3d | GetLandmarkPosition (int landmark) const |
const std::map< int, int > & | GetLandmarkCidToFidMap (int landmark) const |
camera::CameraParameters | GetCameraParameters (void) const |
void | SetCameraParameters (camera::CameraParameters camera_params) |
size_t | GetNumObservations (void) const |
Eigen::Affine3d | GetWorldTransform () const |
void | ApplyTransform (Eigen::Affine3d const &T) |
void | Load (const std::string &protobuf_file, bool localization=false) |
void | LoadKeypoints (const std::string &protobuf_file) |
void | InitializeCidFidToPid () |
void | DetectFeaturesFromFile (std::string const &filename, bool multithreaded, cv::Mat *descriptors, Eigen::Matrix2Xd *keypoints) |
void | DetectFeatures (cv::Mat const &image, bool multithreaded, cv::Mat *descriptors, Eigen::Matrix2Xd *keypoints) |
void | PruneMap (void) |
std::string | GetDetectorName () |
interest_point::FeatureDetector & | detector () |
int | GetHistogramEqualization () |
Public Attributes | |
std::vector< std::string > | cid_to_filename_ |
std::vector< Eigen::Matrix2Xd > | cid_to_keypoint_map_ |
std::vector< std::map< int, int > > | pid_to_cid_fid_ |
std::vector< Eigen::Vector3d > | pid_to_xyz_ |
std::vector< Eigen::Affine3d > | cid_to_cam_t_global_ |
std::vector< cv::Mat > | cid_to_descriptor_map_ |
std::vector< std::map< int, int > > | cid_fid_to_pid_ |
std::vector< std::map< int, int > > | cid_to_matching_cid_counts_ |
interest_point::FeatureDetector | detector_ |
camera::CameraParameters | camera_params_ |
sparse_mapping::VocabDB | vocab_db_ |
LocalizationParameters | loc_params_ |
std::string | protobuf_file_ |
std::map< int, int > | db_to_cid_map_ |
std::map< int, std::set< int > > | cid_to_cid_ |
Eigen::Affine3d | world_transform_ |
std::vector< Eigen::Matrix2Xd > | user_cid_to_keypoint_map_ |
std::vector< std::map< int, int > > | user_pid_to_cid_fid_ |
std::vector< Eigen::Vector3d > | user_pid_to_xyz_ |
cv::Ptr< cv::CLAHE > | clahe_ |
boost::optional< int > | best_previous_cid_ |
std::mutex | mutex_detector_ |
A class representing a sparse map, which consists of a collection of keyframes and detected features. To localize, an image's features are matched to the keyframes in the map. They keyframe features have known positions and the camera pose can be estimated with ransac.
sparse_mapping::SparseMap::SparseMap | ( | const std::vector< std::string > & | filenames, |
const std::string & | detector, | ||
const camera::CameraParameters & | params | ||
) |
Constructs a new sparse map from a list of image files and their associate keypoint and descriptor files. If use_cached_features is set to false, it reads the image files and performs feature detection instead. Does not perform bundle adjustment.
sparse_mapping::SparseMap::SparseMap | ( | const std::string & | protobuf_file, |
bool | localization = false |
||
) |
Constructs a new sparse map from a protobuf file, with specified vocabulary tree and optional parameters.
sparse_mapping::SparseMap::SparseMap | ( | const std::vector< Eigen::Affine3d > & | cid_to_cam_t, |
const std::vector< std::string > & | filenames, | ||
const std::string & | detector, | ||
const camera::CameraParameters & | params | ||
) |
Form a sparse map with given cameras/images, and no features
sparse_mapping::SparseMap::SparseMap | ( | bool | bundler_format, |
std::string const & | filename, | ||
std::vector< std::string > const & | files | ||
) |
|
inline |
Apply given transform to camera positions and 3D points
void sparse_mapping::SparseMap::DetectFeatures | ( | ) |
Detect features in given images
void sparse_mapping::SparseMap::DetectFeatures | ( | cv::Mat const & | image, |
bool | multithreaded, | ||
cv::Mat * | descriptors, | ||
Eigen::Matrix2Xd * | keypoints | ||
) |
void sparse_mapping::SparseMap::DetectFeaturesFromFile | ( | std::string const & | filename, |
bool | multithreaded, | ||
cv::Mat * | descriptors, | ||
Eigen::Matrix2Xd * | keypoints | ||
) |
|
inline |
|
inline |
Return the parameters of the camera used to construct the map.
|
inline |
Get the descriptor for a frame and feature.
|
inline |
|
inline |
Returns map of feature ids to landmark ids for the specified frame.
|
inline |
Get the filename of a keyframe in the map.
|
inline |
Get the global camera transform for a keyframe in the map.
|
inline |
Get the keypoint coordinates in the specified frame.
|
inline |
|
inline |
Return a map for a specified landmark, matching the ids of all the keyframes that landmark was seen in to the feature id within that frame.
|
inline |
Get the global position of the specified landmark.
|
inline |
Get the number of keyframes in the map.
|
inline |
Get the number of landmark points in the map.
|
inline |
Return the number of observations. Use this number to divide the final error to find the average pixel error.
|
inline |
Return the transform to real world coordinates.
void sparse_mapping::SparseMap::InitializeCidFidToPid | ( | ) |
void sparse_mapping::SparseMap::Load | ( | const std::string & | protobuf_file, |
bool | localization = false |
||
) |
void sparse_mapping::SparseMap::LoadKeypoints | ( | const std::string & | protobuf_file | ) |
|
inline |
|
inline |
bool sparse_mapping::SparseMap::Localize | ( | const cv::Mat & | image, |
camera::CameraModel * | pose, | ||
std::vector< Eigen::Vector3d > * | inlier_landmarks = NULL , |
||
std::vector< Eigen::Vector2d > * | inlier_observations = NULL , |
||
std::vector< int > * | cid_list = NULL |
||
) |
bool sparse_mapping::SparseMap::Localize | ( | cv::Mat const & | test_descriptors, |
Eigen::Matrix2Xd const & | test_keypoints, | ||
camera::CameraModel * | pose, | ||
std::vector< Eigen::Vector3d > * | inlier_landmarks, | ||
std::vector< Eigen::Vector2d > * | inlier_observations, | ||
std::vector< int > * | cid_list = NULL , |
||
const cv::Mat & | image = cv::Mat() |
||
) |
bool sparse_mapping::SparseMap::Localize | ( | std::string const & | img_file, |
camera::CameraModel * | pose, | ||
std::vector< Eigen::Vector3d > * | inlier_landmarks = NULL , |
||
std::vector< Eigen::Vector2d > * | inlier_observations = NULL , |
||
std::vector< int > * | cid_list = NULL |
||
) |
Estimate the camera pose for an image file.
void sparse_mapping::SparseMap::PruneMap | ( | void | ) |
void sparse_mapping::SparseMap::Save | ( | const std::string & | protobuf_file | ) | const |
Save the map to a protobuf file.
|
inline |
void sparse_mapping::SparseMap::SetDefaultLocParams | ( | ) |
void sparse_mapping::SparseMap::SetDetectorParams | ( | int | min_features, |
int | max_features, | ||
int | retries, | ||
double | min_thresh, | ||
double | default_thresh, | ||
double | max_thresh, | ||
double | too_many_ratio, | ||
double | too_few_ratio | ||
) |
|
inline |
void sparse_mapping::SparseMap::SetLocParams | ( | const LocalizationParameters & | loc_params | ) |
boost::optional<int> sparse_mapping::SparseMap::best_previous_cid_ |
camera::CameraParameters sparse_mapping::SparseMap::camera_params_ |
std::vector<std::map<int, int> > sparse_mapping::SparseMap::cid_fid_to_pid_ |
std::vector<Eigen::Affine3d > sparse_mapping::SparseMap::cid_to_cam_t_global_ |
std::map< int, std::set<int> > sparse_mapping::SparseMap::cid_to_cid_ |
std::vector<cv::Mat> sparse_mapping::SparseMap::cid_to_descriptor_map_ |
std::vector<std::string> sparse_mapping::SparseMap::cid_to_filename_ |
std::vector<Eigen::Matrix2Xd > sparse_mapping::SparseMap::cid_to_keypoint_map_ |
std::vector<std::map<int, int> > sparse_mapping::SparseMap::cid_to_matching_cid_counts_ |
cv::Ptr<cv::CLAHE> sparse_mapping::SparseMap::clahe_ |
std::map<int, int> sparse_mapping::SparseMap::db_to_cid_map_ |
interest_point::FeatureDetector sparse_mapping::SparseMap::detector_ |
LocalizationParameters sparse_mapping::SparseMap::loc_params_ |
std::mutex sparse_mapping::SparseMap::mutex_detector_ |
std::vector<std::map<int, int> > sparse_mapping::SparseMap::pid_to_cid_fid_ |
std::vector<Eigen::Vector3d> sparse_mapping::SparseMap::pid_to_xyz_ |
std::string sparse_mapping::SparseMap::protobuf_file_ |
std::vector<Eigen::Matrix2Xd> sparse_mapping::SparseMap::user_cid_to_keypoint_map_ |
std::vector<std::map<int, int> > sparse_mapping::SparseMap::user_pid_to_cid_fid_ |
std::vector<Eigen::Vector3d> sparse_mapping::SparseMap::user_pid_to_xyz_ |
|
mutable |
Eigen::Affine3d sparse_mapping::SparseMap::world_transform_ |