Classes | Typedefs | Functions | Variables
ccny_rgbd Namespace Reference

Classes

class  FeatureViewer
 Application to test and visualize the different type of feature detectors. More...
class  KeyframeMapper
 Builds a 3D map from a series of RGBD keyframes. More...
class  Navigator
 Builds a 3D map from a series of RGBD keyframes. More...
class  RGBDImageProc
 Processes the raw output of OpenNI sensors to create a stream of RGB-D images. More...
class  RGBDImageProcNodelet
 Nodelet driver for the RGBDImageProc class. More...
class  RGBNavigator
 Builds a 3D map from a series of RGBD keyframes. More...
class  VisualOdometry
 Subscribes to incoming RGBD images and outputs the position of the moving (base) frame wrt some fixed frame. More...

Typedefs

typedef Eigen::Affine3f AffineTransform
typedef
Eigen::aligned_allocator
< AffineTransform
AffineTransformAllocator
typedef std::vector
< AffineTransform,
AffineTransformAllocator
AffineTransformVector
typedef std::vector< bool > BoolVector
typedef sensor_msgs::CameraInfo CameraInfoMsg
typedef
message_filters::Subscriber
< CameraInfoMsg
CameraInfoSubFilter
typedef
dynamic_reconfigure::Server
< FeatureDetectorConfig > 
FeatureDetectorConfigServer
typedef std::vector< float > FloatVector
typedef
dynamic_reconfigure::Server
< GftDetectorConfig > 
GftDetectorConfigServer
typedef boost::shared_ptr
< GftDetectorConfigServer
GftDetectorConfigServerPtr
typedef sensor_msgs::Image ImageMsg
typedef image_transport::Publisher ImagePublisher
typedef
image_transport::SubscriberFilter 
ImageSubFilter
typedef
image_transport::ImageTransport 
ImageTransport
typedef std::vector< int > IntVector
typedef pcl::KdTreeFLANN
< PointFeature
KdTree
typedef std::vector< cv::KeyPoint > KeypointVector
typedef Eigen::Matrix3f Matrix3f
typedef std::vector
< Eigen::Matrix3f
Matrix3fVector
typedef nav_msgs::Odometry OdomMsg
typedef
dynamic_reconfigure::Server
< OrbDetectorConfig > 
OrbDetectorConfigServer
typedef boost::shared_ptr
< OrbDetectorConfigServer
OrbDetectorConfigServerPtr
typedef nav_msgs::Path PathMsg
typedef
image_geometry::PinholeCameraModel 
PinholeCameraModel
typedef std::vector< cv::Point2f > Point2fVector
typedef std::vector< cv::Point3f > Point3fVector
typedef pcl::PointCloud
< PointFeature
PointCloudFeature
typedef pcl::PointCloud< PointTPointCloudT
typedef pcl::PointXYZ PointFeature
typedef pcl::PointXYZRGB PointT
typedef Eigen::Affine3f Pose
typedef
message_filters::Synchronizer
< RGBDSyncPolicy3
RGBDSynchronizer3
typedef
message_filters::Synchronizer
< RGBDSyncPolicy4
RGBDSynchronizer4
typedef
message_filters::sync_policies::ApproximateTime
< ImageMsg, ImageMsg,
CameraInfoMsg
RGBDSyncPolicy3
typedef
message_filters::sync_policies::ApproximateTime
< ImageMsg, ImageMsg,
CameraInfoMsg, CameraInfoMsg
RGBDSyncPolicy4
typedef
message_filters::Synchronizer
< RGBSyncPolicy
RGBSynchronizer
typedef
message_filters::sync_policies::ApproximateTime
< ImageMsg, CameraInfoMsg
RGBSyncPolicy
typedef
dynamic_reconfigure::Server
< StarDetectorConfig > 
StarDetectorConfigServer
typedef boost::shared_ptr
< StarDetectorConfigServer
StarDetectorConfigServerPtr
typedef
dynamic_reconfigure::Server
< SurfDetectorConfig > 
SurfDetectorConfigServer
typedef boost::shared_ptr
< SurfDetectorConfigServer
SurfDetectorConfigServerPtr
typedef
pcl::registration::TransformationEstimationSVD
< PointFeature, PointFeature
TransformationEstimationSVD
typedef Eigen::Vector3f Vector3f
typedef std::vector
< Eigen::Vector3f
Vector3fVector

Functions

void convertCameraInfoToMats (const CameraInfoMsg::ConstPtr camera_info_msg, cv::Mat &intr, cv::Mat &dist)
 Create OpenCV matrices from a CameraInfoMsg.
void convertMatToCameraInfo (const cv::Mat &intr, CameraInfoMsg &camera_info_msg)
 Create CameraInfoMsg from OpenCV matrices (assumes no distortion)
void createRGBDFrameFromROSMessages (const ImageMsg::ConstPtr &rgb_msg, const ImageMsg::ConstPtr &depth_msg, const CameraInfoMsg::ConstPtr &info_msg, rgbdtools::RGBDFrame &frame)
AffineTransform eigenAffineFromTf (const tf::Transform &tf)
Eigen::Matrix4f eigenFromTf (const tf::Transform &tf)
 Converts an tf::Transform transform to an Eigen transform.
int getmaxheight (PathMsg path_msg)
int getminheight (PathMsg path_msg)
double getMsDuration (const ros::WallTime &start)
 Returns the duration, in ms, from a given time.
void getTfDifference (const tf::Transform &motion, double &dist, double &angle)
 Given a transform, calculates the linear and angular distance between it and identity.
void getTfDifference (const tf::Transform &a, const tf::Transform b, double &dist, double &angle)
 Given two transformss, calculates the linear and angular distance between them, or the linear and angular components of a.inv() * b.
void openCVRtToTf (const cv::Mat &R, const cv::Mat &t, tf::Transform &transform)
 Creates a tf transform from a 3x3 OpenCV rotation matrix and a 3x1 OpenCV translation vector.
void pathEigenAffineToROS (const AffineTransformVector &path, PathMsg &path_msg)
 Copies over the poses from a Eigen vector to a ROS message. Assumes the message is already correctly resized, and preserves the headers of each pose in the message.
void pathROSToEigenAffine (const PathMsg &path_msg, AffineTransformVector &path)
 copies over the poses from a ROS message Eigen vector. The eigen vector will be cleared and resized appropriately.
 PLUGINLIB_DECLARE_CLASS (ccny_rgbd, RGBDImageProcNodelet, RGBDImageProcNodelet, nodelet::Nodelet)
void removeInvalidDistributions (const Vector3fVector &means, const Matrix3fVector &covariances, const BoolVector &valid, Vector3fVector &means_f, Matrix3fVector &covariances_f)
void removeInvalidMeans (const Vector3fVector &means, const BoolVector &valid, Vector3fVector &means_f)
tf::Transform tfFromEigen (Eigen::Matrix4f trans)
 Converts an Eigen transform to a tf::Transform.
tf::Transform tfFromEigenAffine (const AffineTransform &trans)
bool tfGreaterThan (const tf::Transform &a, double dist, double angle)
 Given a transfom (possibly computed as a difference between two transforms) checks if either its angular or linar component exceeds a threshold.
void tfToEigenRt (const tf::Transform &tf, Matrix3f &R, Vector3f &t)
 Decomposes a tf into an Eigen 3x3 rotation matrix and Eigen 3x1 rotation vector.
void tfToOpenCVRt (const tf::Transform &transform, cv::Mat &R, cv::Mat &t)
 Decomposes a tf::Transform into a 3x3 OpenCV rotation matrix and a 3x1 OpenCV translation vector.
void tfToXYZRPY (const tf::Transform &t, double &x, double &y, double &z, double &roll, double &pitch, double &yaw)
 Decomposes a tf::Transform into x, y, z, roll, pitch, yaw.

Variables

cv::Size CamSize
cv::Mat dist
bool first = true
cv::Mat intr
aruco::MarkerDetector MDetector
aruco::CameraParameters param
rgbdtools::KeyframeGraphSolverISAM solver_
int triple_add = 0

Typedef Documentation

typedef Eigen::Affine3f ccny_rgbd::AffineTransform

Definition at line 55 of file types.h.

typedef Eigen::aligned_allocator<AffineTransform> ccny_rgbd::AffineTransformAllocator

Definition at line 68 of file types.h.

Definition at line 69 of file types.h.

typedef std::vector<bool> ccny_rgbd::BoolVector

Definition at line 61 of file types.h.

typedef sensor_msgs::CameraInfo ccny_rgbd::CameraInfoMsg

Definition at line 85 of file types.h.

Definition at line 95 of file types.h.

typedef dynamic_reconfigure::Server<FeatureDetectorConfig> ccny_rgbd::FeatureDetectorConfigServer

Definition at line 107 of file types.h.

typedef std::vector<float> ccny_rgbd::FloatVector

Definition at line 60 of file types.h.

typedef dynamic_reconfigure::Server<GftDetectorConfig> ccny_rgbd::GftDetectorConfigServer

Definition at line 109 of file types.h.

Definition at line 110 of file types.h.

typedef sensor_msgs::Image ccny_rgbd::ImageMsg

Definition at line 84 of file types.h.

typedef image_transport::Publisher ccny_rgbd::ImagePublisher

Definition at line 93 of file types.h.

typedef image_transport::SubscriberFilter ccny_rgbd::ImageSubFilter

Definition at line 94 of file types.h.

typedef image_transport::ImageTransport ccny_rgbd::ImageTransport

Definition at line 92 of file types.h.

typedef std::vector<int> ccny_rgbd::IntVector

Definition at line 59 of file types.h.

typedef pcl::KdTreeFLANN<PointFeature> ccny_rgbd::KdTree

Definition at line 79 of file types.h.

typedef std::vector<cv::KeyPoint> ccny_rgbd::KeypointVector

Definition at line 66 of file types.h.

Definition at line 52 of file types.h.

Definition at line 64 of file types.h.

typedef nav_msgs::Odometry ccny_rgbd::OdomMsg

Definition at line 86 of file types.h.

typedef dynamic_reconfigure::Server<OrbDetectorConfig> ccny_rgbd::OrbDetectorConfigServer

Definition at line 118 of file types.h.

Definition at line 119 of file types.h.

typedef nav_msgs::Path ccny_rgbd::PathMsg

Definition at line 87 of file types.h.

typedef image_geometry::PinholeCameraModel ccny_rgbd::PinholeCameraModel

Definition at line 91 of file types.h.

typedef std::vector<cv::Point2f> ccny_rgbd::Point2fVector

Definition at line 62 of file types.h.

typedef std::vector<cv::Point3f> ccny_rgbd::Point3fVector

Definition at line 63 of file types.h.

typedef pcl::PointCloud<PointFeature> ccny_rgbd::PointCloudFeature

Definition at line 77 of file types.h.

typedef pcl::PointCloud<PointT> ccny_rgbd::PointCloudT

Definition at line 74 of file types.h.

typedef pcl::PointXYZ ccny_rgbd::PointFeature

Definition at line 76 of file types.h.

typedef pcl::PointXYZRGB ccny_rgbd::PointT

Definition at line 73 of file types.h.

typedef Eigen::Affine3f ccny_rgbd::Pose

Definition at line 54 of file types.h.

Definition at line 102 of file types.h.

Definition at line 103 of file types.h.

Definition at line 98 of file types.h.

Definition at line 99 of file types.h.

Definition at line 101 of file types.h.

Definition at line 97 of file types.h.

typedef dynamic_reconfigure::Server<StarDetectorConfig> ccny_rgbd::StarDetectorConfigServer

Definition at line 112 of file types.h.

Definition at line 113 of file types.h.

typedef dynamic_reconfigure::Server<SurfDetectorConfig> ccny_rgbd::SurfDetectorConfigServer

Definition at line 115 of file types.h.

Definition at line 116 of file types.h.

Definition at line 80 of file types.h.

Definition at line 53 of file types.h.

Definition at line 65 of file types.h.


Function Documentation

void ccny_rgbd::convertCameraInfoToMats ( const CameraInfoMsg::ConstPtr  camera_info_msg,
cv::Mat &  intr,
cv::Mat &  dist 
)

Create OpenCV matrices from a CameraInfoMsg.

Parameters:
camera_info_msginput CameraInfoMsg
introutput OpenCV intrinsic matrix
distoutput OpenCV distortion vector

Definition at line 266 of file util.cpp.

void ccny_rgbd::convertMatToCameraInfo ( const cv::Mat &  intr,
CameraInfoMsg &  camera_info_msg 
)

Create CameraInfoMsg from OpenCV matrices (assumes no distortion)

Parameters:
intrinput OpenCV intrinsic matrix
camera_info_msgoutput CameraInfoMsg

Definition at line 289 of file util.cpp.

void ccny_rgbd::createRGBDFrameFromROSMessages ( const ImageMsg::ConstPtr &  rgb_msg,
const ImageMsg::ConstPtr &  depth_msg,
const CameraInfoMsg::ConstPtr &  info_msg,
rgbdtools::RGBDFrame frame 
)
Todo:
assert that distortion (dist) is 0
Todo:
assert that distortion (dist) is 0

Definition at line 315 of file util.cpp.

AffineTransform ccny_rgbd::eigenAffineFromTf ( const tf::Transform &  tf)

Definition at line 118 of file util.cpp.

Eigen::Matrix4f ccny_rgbd::eigenFromTf ( const tf::Transform &  tf)

Converts an tf::Transform transform to an Eigen transform.

Parameters:
tfthe tf transform
Returns:
the Eigen version of the transform

Definition at line 81 of file util.cpp.

int ccny_rgbd::getmaxheight ( PathMsg  path_msg)

Definition at line 27 of file util.cpp.

int ccny_rgbd::getminheight ( PathMsg  path_msg)

Definition at line 40 of file util.cpp.

double ccny_rgbd::getMsDuration ( const ros::WallTime start)

Returns the duration, in ms, from a given time.

Parameters:
startthe start time
Returns:
duration (in ms) from start until now

Definition at line 166 of file util.cpp.

void ccny_rgbd::getTfDifference ( const tf::Transform &  motion,
double &  dist,
double &  angle 
)

Given a transform, calculates the linear and angular distance between it and identity.

Parameters:
motionthe incremental motion
distreference to linear distance
anglereference to angular distance

Definition at line 53 of file util.cpp.

void ccny_rgbd::getTfDifference ( const tf::Transform &  a,
const tf::Transform  b,
double &  dist,
double &  angle 
)

Given two transformss, calculates the linear and angular distance between them, or the linear and angular components of a.inv() * b.

Parameters:
athe first transform
bthe second transform
distreference to the linear distance
anglereference to the angular distance

Definition at line 60 of file util.cpp.

void ccny_rgbd::openCVRtToTf ( const cv::Mat &  R,
const cv::Mat &  t,
tf::Transform &  transform 
)

Creates a tf transform from a 3x3 OpenCV rotation matrix and a 3x1 OpenCV translation vector.

Parameters:
Rthe 3x3 OpenCV matrix
tthe 3x1 OpenCV vector
transformreference to the output transform

Definition at line 247 of file util.cpp.

void ccny_rgbd::pathEigenAffineToROS ( const AffineTransformVector path,
PathMsg &  path_msg 
)

Copies over the poses from a Eigen vector to a ROS message. Assumes the message is already correctly resized, and preserves the headers of each pose in the message.

Definition at line 352 of file util.cpp.

void ccny_rgbd::pathROSToEigenAffine ( const PathMsg &  path_msg,
AffineTransformVector path 
)

copies over the poses from a ROS message Eigen vector. The eigen vector will be cleared and resized appropriately.

Definition at line 365 of file util.cpp.

ccny_rgbd::PLUGINLIB_DECLARE_CLASS ( ccny_rgbd  ,
RGBDImageProcNodelet  ,
RGBDImageProcNodelet  ,
nodelet::Nodelet   
)
void ccny_rgbd::removeInvalidDistributions ( const Vector3fVector means,
const Matrix3fVector covariances,
const BoolVector valid,
Vector3fVector means_f,
Matrix3fVector covariances_f 
)

Definition at line 187 of file util.cpp.

void ccny_rgbd::removeInvalidMeans ( const Vector3fVector means,
const BoolVector valid,
Vector3fVector means_f 
)

Definition at line 171 of file util.cpp.

tf::Transform ccny_rgbd::tfFromEigen ( Eigen::Matrix4f  trans)

Converts an Eigen transform to a tf::Transform.

Parameters:
transEigen transform
Returns:
tf version of the eigen transform

Definition at line 66 of file util.cpp.

tf::Transform ccny_rgbd::tfFromEigenAffine ( const AffineTransform trans)

Definition at line 103 of file util.cpp.

bool ccny_rgbd::tfGreaterThan ( const tf::Transform &  a,
double  dist,
double  angle 
)

Given a transfom (possibly computed as a difference between two transforms) checks if either its angular or linar component exceeds a threshold.

Parameters:
ainput transform
distlinear distance trheshold
angleangular distance threshold

Definition at line 152 of file util.cpp.

void ccny_rgbd::tfToEigenRt ( const tf::Transform &  tf,
Matrix3f R,
Vector3f t 
)

Decomposes a tf into an Eigen 3x3 rotation matrix and Eigen 3x1 rotation vector.

Parameters:
tfthe transform
Rreference to the 3x3 Eigen matrix
treference to the 3x1 Eigen vector

Definition at line 208 of file util.cpp.

void ccny_rgbd::tfToOpenCVRt ( const tf::Transform &  transform,
cv::Mat &  R,
cv::Mat &  t 
)

Decomposes a tf::Transform into a 3x3 OpenCV rotation matrix and a 3x1 OpenCV translation vector.

Parameters:
transformthe transform
Rreference to the 3x3 OpenCV matrix
treference to the 3x1 OpenCV vector

Definition at line 227 of file util.cpp.

void ccny_rgbd::tfToXYZRPY ( const tf::Transform &  t,
double &  x,
double &  y,
double &  z,
double &  roll,
double &  pitch,
double &  yaw 
)

Decomposes a tf::Transform into x, y, z, roll, pitch, yaw.

Parameters:
tthe input transform
xthe output x component
ythe output y component
zthe output z component
rollthe output roll component
pitchthe output pitch component
yawthe output yaw component

Definition at line 139 of file util.cpp.


Variable Documentation

Definition at line 283 of file navigator.cpp.

cv::Mat ccny_rgbd::dist

Definition at line 282 of file navigator.cpp.

bool ccny_rgbd::first = true

Definition at line 280 of file navigator.cpp.

cv::Mat ccny_rgbd::intr

Definition at line 282 of file navigator.cpp.

aruco::MarkerDetector ccny_rgbd::MDetector

Definition at line 93 of file keyframe_mapper.cpp.

aruco::CameraParameters ccny_rgbd::param

Definition at line 281 of file navigator.cpp.

Definition at line 96 of file navigator.cpp.

Definition at line 200 of file keyframe_mapper.cpp.

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


ccny_rgbd
Author(s): Ivan Dryanovski
autogenerated on Tue Aug 27 2013 10:34:02