NASA Astrobee Robot Software  0.19.1
Flight software for the Astrobee robots operating inside the International Space Station.
sparse_mapping::SparseMap Struct Reference

#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 &params)
 
 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 &params)
 
 SparseMap (bool bundler_format, std::string const &filename, std::vector< std::string > const &files)
 
void SetDefaultLocParams ()
 
void SetLocParams (const LocalizationParameters &loc_params)
 
const LocalizationParametersloc_params () const
 
LocalizationParametersloc_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::FeatureDetectordetector ()
 
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_
 

Detailed Description

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.

Constructor & Destructor Documentation

◆ SparseMap() [1/4]

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.

◆ SparseMap() [2/4]

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.

◆ SparseMap() [3/4]

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

◆ SparseMap() [4/4]

sparse_mapping::SparseMap::SparseMap ( bool  bundler_format,
std::string const &  filename,
std::vector< std::string > const &  files 
)

Member Function Documentation

◆ ApplyTransform()

void sparse_mapping::SparseMap::ApplyTransform ( Eigen::Affine3d const &  T)
inline

Apply given transform to camera positions and 3D points

◆ DetectFeatures() [1/2]

void sparse_mapping::SparseMap::DetectFeatures ( )

Detect features in given images

◆ DetectFeatures() [2/2]

void sparse_mapping::SparseMap::DetectFeatures ( cv::Mat const &  image,
bool  multithreaded,
cv::Mat *  descriptors,
Eigen::Matrix2Xd *  keypoints 
)

◆ DetectFeaturesFromFile()

void sparse_mapping::SparseMap::DetectFeaturesFromFile ( std::string const &  filename,
bool  multithreaded,
cv::Mat *  descriptors,
Eigen::Matrix2Xd *  keypoints 
)

◆ detector()

interest_point::FeatureDetector& sparse_mapping::SparseMap::detector ( )
inline

◆ GetCameraParameters()

camera::CameraParameters sparse_mapping::SparseMap::GetCameraParameters ( void  ) const
inline

Return the parameters of the camera used to construct the map.

◆ GetDescriptor()

cv::Mat sparse_mapping::SparseMap::GetDescriptor ( int  frame,
int  fid 
) const
inline

Get the descriptor for a frame and feature.

◆ GetDetectorName()

std::string sparse_mapping::SparseMap::GetDetectorName ( )
inline

◆ GetFrameFidToPidMap()

const std::map<int, int>& sparse_mapping::SparseMap::GetFrameFidToPidMap ( int  frame) const
inline

Returns map of feature ids to landmark ids for the specified frame.

◆ GetFrameFilename()

const std::string& sparse_mapping::SparseMap::GetFrameFilename ( int  frame) const
inline

Get the filename of a keyframe in the map.

◆ GetFrameGlobalTransform()

const Eigen::Affine3d& sparse_mapping::SparseMap::GetFrameGlobalTransform ( int  frame) const
inline

Get the global camera transform for a keyframe in the map.

◆ GetFrameKeypoints()

const Eigen::Matrix2Xd& sparse_mapping::SparseMap::GetFrameKeypoints ( int  frame) const
inline

Get the keypoint coordinates in the specified frame.

◆ GetHistogramEqualization()

int sparse_mapping::SparseMap::GetHistogramEqualization ( )
inline

◆ GetLandmarkCidToFidMap()

const std::map<int, int>& sparse_mapping::SparseMap::GetLandmarkCidToFidMap ( int  landmark) const
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.

◆ GetLandmarkPosition()

Eigen::Vector3d sparse_mapping::SparseMap::GetLandmarkPosition ( int  landmark) const
inline

Get the global position of the specified landmark.

◆ GetNumFrames()

size_t sparse_mapping::SparseMap::GetNumFrames ( void  ) const
inline

Get the number of keyframes in the map.

◆ GetNumLandmarks()

size_t sparse_mapping::SparseMap::GetNumLandmarks ( void  ) const
inline

Get the number of landmark points in the map.

◆ GetNumObservations()

size_t sparse_mapping::SparseMap::GetNumObservations ( void  ) const
inline

Return the number of observations. Use this number to divide the final error to find the average pixel error.

◆ GetWorldTransform()

Eigen::Affine3d sparse_mapping::SparseMap::GetWorldTransform ( ) const
inline

Return the transform to real world coordinates.

◆ InitializeCidFidToPid()

void sparse_mapping::SparseMap::InitializeCidFidToPid ( )

◆ Load()

void sparse_mapping::SparseMap::Load ( const std::string &  protobuf_file,
bool  localization = false 
)

◆ LoadKeypoints()

void sparse_mapping::SparseMap::LoadKeypoints ( const std::string &  protobuf_file)

◆ loc_params() [1/2]

LocalizationParameters& sparse_mapping::SparseMap::loc_params ( )
inline

◆ loc_params() [2/2]

const LocalizationParameters& sparse_mapping::SparseMap::loc_params ( ) const
inline

◆ Localize() [1/3]

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 
)

◆ Localize() [2/3]

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() 
)

◆ Localize() [3/3]

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.

◆ PruneMap()

void sparse_mapping::SparseMap::PruneMap ( void  )

◆ Save()

void sparse_mapping::SparseMap::Save ( const std::string &  protobuf_file) const

Save the map to a protobuf file.

◆ SetCameraParameters()

void sparse_mapping::SparseMap::SetCameraParameters ( camera::CameraParameters  camera_params)
inline

◆ SetDefaultLocParams()

void sparse_mapping::SparseMap::SetDefaultLocParams ( )

◆ SetDetectorParams()

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 
)

◆ SetFrameGlobalTransform()

void sparse_mapping::SparseMap::SetFrameGlobalTransform ( int  frame,
const Eigen::Affine3d &  transform 
)
inline

◆ SetLocParams()

void sparse_mapping::SparseMap::SetLocParams ( const LocalizationParameters loc_params)

Member Data Documentation

◆ best_previous_cid_

boost::optional<int> sparse_mapping::SparseMap::best_previous_cid_

◆ camera_params_

camera::CameraParameters sparse_mapping::SparseMap::camera_params_

◆ cid_fid_to_pid_

std::vector<std::map<int, int> > sparse_mapping::SparseMap::cid_fid_to_pid_

◆ cid_to_cam_t_global_

std::vector<Eigen::Affine3d > sparse_mapping::SparseMap::cid_to_cam_t_global_

◆ cid_to_cid_

std::map< int, std::set<int> > sparse_mapping::SparseMap::cid_to_cid_

◆ cid_to_descriptor_map_

std::vector<cv::Mat> sparse_mapping::SparseMap::cid_to_descriptor_map_

◆ cid_to_filename_

std::vector<std::string> sparse_mapping::SparseMap::cid_to_filename_

◆ cid_to_keypoint_map_

std::vector<Eigen::Matrix2Xd > sparse_mapping::SparseMap::cid_to_keypoint_map_

◆ cid_to_matching_cid_counts_

std::vector<std::map<int, int> > sparse_mapping::SparseMap::cid_to_matching_cid_counts_

◆ clahe_

cv::Ptr<cv::CLAHE> sparse_mapping::SparseMap::clahe_

◆ db_to_cid_map_

std::map<int, int> sparse_mapping::SparseMap::db_to_cid_map_

◆ detector_

interest_point::FeatureDetector sparse_mapping::SparseMap::detector_

◆ loc_params_

LocalizationParameters sparse_mapping::SparseMap::loc_params_

◆ mutex_detector_

std::mutex sparse_mapping::SparseMap::mutex_detector_

◆ pid_to_cid_fid_

std::vector<std::map<int, int> > sparse_mapping::SparseMap::pid_to_cid_fid_

◆ pid_to_xyz_

std::vector<Eigen::Vector3d> sparse_mapping::SparseMap::pid_to_xyz_

◆ protobuf_file_

std::string sparse_mapping::SparseMap::protobuf_file_

◆ user_cid_to_keypoint_map_

std::vector<Eigen::Matrix2Xd> sparse_mapping::SparseMap::user_cid_to_keypoint_map_

◆ user_pid_to_cid_fid_

std::vector<std::map<int, int> > sparse_mapping::SparseMap::user_pid_to_cid_fid_

◆ user_pid_to_xyz_

std::vector<Eigen::Vector3d> sparse_mapping::SparseMap::user_pid_to_xyz_

◆ vocab_db_

sparse_mapping::VocabDB sparse_mapping::SparseMap::vocab_db_
mutable

◆ world_transform_

Eigen::Affine3d sparse_mapping::SparseMap::world_transform_

The documentation for this struct was generated from the following files: