Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes | Static Protected Attributes
Node Class Reference

Holds the data for one graph node and provides functionality to compute relative transformations to other Nodes. More...

#include <node.h>

List of all members.

Public Member Functions

void addPointCloud (pointcloud_type::Ptr pc_col)
void clearFeatureInformation ()
void clearPointCloud ()
 erase the points from the cloud to save memory
Nodecopy_filtered (const Eigen::Vector3f &center, 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 &params) 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

Detailed Description

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.


Constructor & Destructor Documentation

Node::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 
)

Visual must be CV_8UC1, depth CV_32FC1, detection_mask must be CV_8UC1 with non-zero at potential keypoint locations

Construct node without precomputed point cloud. Computes the point cloud on demand, possibly subsampled

Definition at line 100 of file node.cpp.

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

Visual must be CV_8UC1 detection_mask must be CV_8UC1 with non-zero at potential keypoint locations

Definition at line 250 of file node.cpp.

Node::Node ( ) [inline]

Definition at line 80 of file node.h.

Delete the flannIndex if built.

Definition at line 369 of file node.cpp.


Member Function Documentation

void Node::addPointCloud ( pointcloud_type::Ptr  pc_col)

Definition at line 1442 of file node.cpp.

Definition at line 1429 of file node.cpp.

erase the points from the cloud to save memory

Definition at line 1489 of file node.cpp.

void Node::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 [protected]

Definition at line 966 of file node.cpp.

Node * Node::copy_filtered ( const Eigen::Vector3f &  center,
float  radius 
) const

Center needs to be in the coordinate frame of the node (i.e. the camera is 0,0,0)

Definition at line 1632 of file node.cpp.

unsigned int Node::featureMatching ( const Node other,
std::vector< cv::DMatch > *  matches 
) const

Fills "matches" and returns ratio of "good" features in the sense of distinction via the "nn_distance_ratio" setting (see parameter server)

Definition at line 533 of file node.cpp.

Transform, e.g., from kinematics.

Definition at line 389 of file node.cpp.

const sensor_msgs::CameraInfo& Node::getCamInfo ( ) const [inline]

Definition at line 197 of file node.h.

const cv::flann::Index * Node::getFlannIndex ( ) const [protected]

Definition at line 491 of file node.cpp.

Transform, e.g., from MoCap.

Definition at line 386 of file node.cpp.

long Node::getMemoryFootprint ( bool  print) const

Definition at line 1459 of file node.cpp.

Transform, e.g., from Joint/Wheel odometry.

Definition at line 383 of file node.cpp.

bool Node::getRelativeTransformationTo ( const Node earlier_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.

Find transformation with largest support, RANSAC style. Return false if no transformation can be found

Iterations with more than half of the initial_matches inlying, count tenfold

Iterations with more than 3/4 of the initial_matches inlying, count twentyfold

Can this get better anyhow?

Definition at line 1072 of file node.cpp.

void Node::knnSearch ( cv::Mat &  query,
cv::Mat &  indices,
cv::Mat &  dists,
int  knn,
const cv::flann::SearchParams &  params 
) const

Definition at line 1571 of file node.cpp.

void Node::mat2dist ( const Eigen::Matrix4f &  t,
double &  dist 
) [inline, protected]

Get the norm of the translational part of an affine matrix (Helper for isBigTrafo)

Definition at line 236 of file node.h.

MatchingResult Node::matchNodePair ( const Node older_node)

Compare the features of two nodes and compute the transformation.

Definition at line 1303 of file node.cpp.

void Node::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 
) [protected]

return the 3D projection of valid keypoints using information from the point cloud and remove invalid keypoints (NaN depth)

Definition at line 853 of file node.cpp.

void Node::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 
) [protected]

return the 3D projection of valid keypoints using information from the depth image and remove invalid keypoints (NaN depth)

Definition at line 898 of file node.cpp.

void Node::reducePointCloud ( double  voxelfilter_size)

reduce the points from the cloud using a voxelgrid_filter to save memory

Definition at line 1446 of file node.cpp.

Retrieves and stores the transformation from base to point cloud at capturing time.

Transform, e.g., from kinematics.

Definition at line 380 of file node.cpp.

Transform, e.g., from MoCap.

Definition at line 377 of file node.cpp.

Transform, e.g., from Joint/Wheel odometry.

Definition at line 373 of file node.cpp.


Member Data Documentation

contains the transformation from the base (defined on param server) to the point_cloud

Definition at line 204 of file node.h.

sensor_msgs::CameraInfo Node::cam_info_ [protected]

Definition at line 208 of file node.h.

std::vector<std::pair<float, float> > Node::feature_depth_stats_

Contains the minimum and maximum depth in the feature's range (not used yet)

Definition at line 176 of file node.h.

descriptor definitions

Definition at line 167 of file node.h.

std::vector<cv::KeyPoint> Node::feature_locations_2d_

Where in the image are the descriptors.

Definition at line 174 of file node.h.

backprojected 3d descriptor locations relative to cam position in homogeneous coordinates (last dimension is 1.0)

Definition at line 170 of file node.h.

std::vector<unsigned char> Node::feature_matching_stats_

Contains how often a feature has been an (inlier) match.

Definition at line 178 of file node.h.

cv::flann::Index* Node::flannIndex [mutable, protected]

Definition at line 203 of file node.h.

QMutex Node::gicp_mutex [static, protected]

Definition at line 201 of file node.h.

contains the transformation from the mocap system

Definition at line 205 of file node.h.

Definition at line 160 of file node.h.

Definition at line 196 of file node.h.

int Node::id_

pointcloud_type centrally defines what the pc is templated on

Definition at line 154 of file node.h.

int Node::initial_node_matches_ [protected]

Definition at line 207 of file node.h.

Definition at line 158 of file node.h.

contains the transformation from the wheel encoders/joint states

Definition at line 206 of file node.h.

Definition at line 161 of file node.h.

pointcloud_type::Ptr Node::pc_col

Definition at line 162 of file node.h.

Definition at line 155 of file node.h.

std::vector<float> Node::siftgpu_descriptors

Definition at line 171 of file node.h.

QMutex Node::siftgpu_mutex [static, protected]

Definition at line 202 of file node.h.

Definition at line 159 of file node.h.

Definition at line 157 of file node.h.

Definition at line 156 of file node.h.


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


rgbdslam_v2
Author(s): Felix Endres, Juergen Hess, Nikolas Engelhard
autogenerated on Thu Jun 6 2019 21:49:45