Holds the data for one graph node and provides functionality to compute relative transformations to other Nodes.
More...
Public Member Functions |
void | addPointCloud (pointcloud_type::Ptr pc_col) |
void | clearFeatureInformation () |
void | clearPointCloud () |
| erase the points from the cloud to save memory
|
Node * | copy_filtered (const Eigen::Vector3f ¢er, float radius) const |
| Center needs to be in the coordinate frame of the node (i.e. the camera is 0,0,0)
|
unsigned int | featureMatching (const Node *other, std::vector< cv::DMatch > *matches) const |
tf::StampedTransform | getBase2PointsTransform () const |
| Transform, e.g., from kinematics.
|
const sensor_msgs::CameraInfo & | getCamInfo () const |
tf::StampedTransform | getGroundTruthTransform () const |
| Transform, e.g., from MoCap.
|
long | getMemoryFootprint (bool print) const |
tf::StampedTransform | getOdomTransform () const |
| Transform, e.g., from Joint/Wheel odometry.
|
bool | getRelativeTransformationTo (const Node *target_node, std::vector< cv::DMatch > *initial_matches, Eigen::Matrix4f &resulting_transformation, float &rmse, std::vector< cv::DMatch > &matches) const |
| Compute the relative transformation between the nodes.
|
void | knnSearch (cv::Mat &query, cv::Mat &indices, cv::Mat &dists, int knn, const cv::flann::SearchParams ¶ms) const |
MatchingResult | matchNodePair (const Node *older_node) |
| Compare the features of two nodes and compute the transformation.
|
| Node (const cv::Mat &visual, const cv::Mat &depth, const cv::Mat &detection_mask, const sensor_msgs::CameraInfoConstPtr &cam_info, myHeader depth_header, cv::Ptr< cv::FeatureDetector > detector, cv::Ptr< cv::DescriptorExtractor > extractor) |
| Node (const cv::Mat visual, cv::Ptr< cv::FeatureDetector > detector, cv::Ptr< cv::DescriptorExtractor > extractor, pointcloud_type::Ptr point_cloud, const cv::Mat detection_mask=cv::Mat()) |
| Node () |
void | reducePointCloud (double voxelfilter_size) |
| reduce the points from the cloud using a voxelgrid_filter to save memory
|
void | setBase2PointsTransform (tf::StampedTransform &b2p) |
| Transform, e.g., from kinematics.
|
void | setGroundTruthTransform (tf::StampedTransform gt) |
| Transform, e.g., from MoCap.
|
void | setOdomTransform (tf::StampedTransform odom) |
| Transform, e.g., from Joint/Wheel odometry.
|
| ~Node () |
| Delete the flannIndex if built.
|
Public Attributes |
std::vector< std::pair< float,
float > > | feature_depth_stats_ |
| Contains the minimum and maximum depth in the feature's range (not used yet)
|
cv::Mat | feature_descriptors_ |
| descriptor definitions
|
std::vector< cv::KeyPoint > | feature_locations_2d_ |
| Where in the image are the descriptors.
|
std_vector_of_eigen_vector4f | feature_locations_3d_ |
| backprojected 3d descriptor locations relative to cam position in homogeneous coordinates (last dimension is 1.0)
|
std::vector< unsigned char > | feature_matching_stats_ |
| Contains how often a feature has been an (inlier) match.
|
bool | has_odometry_edge_ |
myHeader | header_ |
int | id_ |
| pointcloud_type centrally defines what the pc is templated on
|
bool | matchable_ |
bool | odometry_set_ |
pointcloud_type::Ptr | pc_col |
int | seq_id_ |
std::vector< float > | siftgpu_descriptors |
ros::Time | timestamp_ |
bool | valid_tf_estimate_ |
int | vertex_id_ |
Protected Member Functions |
void | computeInliersAndError (const std::vector< cv::DMatch > &initial_matches, const Eigen::Matrix4f &transformation, const std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > &origins, const std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > &targets, size_t min_inliers, std::vector< cv::DMatch > &new_inliers, double &mean_error, double squaredMaxInlierDistInM=0.0009) const |
const cv::flann::Index * | getFlannIndex () const |
void | mat2dist (const Eigen::Matrix4f &t, double &dist) |
| Get the norm of the translational part of an affine matrix (Helper for isBigTrafo)
|
void | projectTo3D (std::vector< cv::KeyPoint > &feature_locations_2d, std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > &feature_locations_3d, const pointcloud_type::ConstPtr point_cloud) |
| return the 3D projection of valid keypoints using information from the point cloud and remove invalid keypoints (NaN depth)
|
void | projectTo3D (std::vector< cv::KeyPoint > &feature_locations_2d, std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > &feature_locations_3d, const cv::Mat &depth, const sensor_msgs::CameraInfoConstPtr &cam_info) |
| return the 3D projection of valid keypoints using information from the depth image and remove invalid keypoints (NaN depth)
|
void | retrieveBase2CamTransformation () |
| Retrieves and stores the transformation from base to point cloud at capturing time.
|
Protected Attributes |
tf::StampedTransform | base2points_ |
| contains the transformation from the base (defined on param server) to the point_cloud
|
sensor_msgs::CameraInfo | cam_info_ |
cv::flann::Index * | flannIndex |
tf::StampedTransform | ground_truth_transform_ |
| contains the transformation from the mocap system
|
int | initial_node_matches_ |
tf::StampedTransform | odom_transform_ |
| contains the transformation from the wheel encoders/joint states
|
Static Protected Attributes |
static QMutex | gicp_mutex |
static QMutex | siftgpu_mutex |
Holds the data for one graph node and provides functionality to compute relative transformations to other Nodes.
Definition at line 59 of file node.h.