Namespaces | Classes | Typedefs | Enumerations | Functions
ccny_rgbd Namespace Reference

Namespaces

namespace  srv

Classes

struct  AddManualKeyframe
struct  AddManualKeyframeRequest_
struct  AddManualKeyframeResponse_
class  FeatureDetector
 Base class for sparse feature extractors. More...
class  FeatureHistory
 Auxiliary class for the frame-to-frame ICP class. More...
class  FeatureViewer
 Application to test and visualize the different type of feature detectors. More...
struct  GenerateGraph
struct  GenerateGraphRequest_
struct  GenerateGraphResponse_
class  GftDetector
 GoodFeaturesToTrack detector. More...
class  KeyframeAssociation
 Class representing an association between two keyframes, used for graph-based pose alignement. More...
class  KeyframeGraphDetector
 Detects graph correspondences based on visual feature matching between keyframes. More...
class  KeyframeGraphSolver
 Base class for graph-based global alignment classes. More...
class  KeyframeGraphSolverG2O
 Graph-based global alignement using g2o (generalized graph optimizaiton) More...
class  KeyframeMapper
 Builds a 3D map from a series of RGBD keyframes. More...
struct  Load
struct  LoadRequest_
struct  LoadResponse_
class  MotionEstimation
 Base class for visual odometry motion estimation methods. More...
class  MotionEstimationICP
 Frame-to-frame ICP motion estimation. More...
class  MotionEstimationICPProbModel
 Motion estimation based on aligning sparse features against a persistent, dynamic model. More...
class  OrbDetector
 ORB detector. More...
struct  PublishKeyframe
struct  PublishKeyframeRequest_
struct  PublishKeyframeResponse_
struct  PublishKeyframes
struct  PublishKeyframesRequest_
struct  PublishKeyframesResponse_
class  RGBDFrame
 Auxiliarry class that holds together rgb and depth images. 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  RGBDKeyframe
 Extension of an RGBDFrame, which has a pose, and a 3D point cloud. More...
struct  Save
struct  SaveRequest_
struct  SaveResponse_
struct  SolveGraph
struct  SolveGraphRequest_
struct  SolveGraphResponse_
class  StarDetector
 STAR detector. More...
class  SurfDetector
 SURF detector. More...
class  VisualOdometry
 Subscribes to incoming RGBD images and outputs the position of the moving (base) frame wrt some fixed frame. More...

Typedefs

typedef
::ccny_rgbd::AddManualKeyframeRequest_
< std::allocator< void > > 
AddManualKeyframeRequest
typedef boost::shared_ptr
< ::ccny_rgbd::AddManualKeyframeRequest
const > 
AddManualKeyframeRequestConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::AddManualKeyframeRequest
AddManualKeyframeRequestPtr
typedef
::ccny_rgbd::AddManualKeyframeResponse_
< std::allocator< void > > 
AddManualKeyframeResponse
typedef boost::shared_ptr
< ::ccny_rgbd::AddManualKeyframeResponse
const > 
AddManualKeyframeResponseConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::AddManualKeyframeResponse
AddManualKeyframeResponsePtr
typedef std::vector< bool > BoolVector
typedef sensor_msgs::CameraInfo CameraInfoMsg
typedef
message_filters::Subscriber
< CameraInfoMsg
CameraInfoSubFilter
typedef
dynamic_reconfigure::Server
< FeatureDetectorConfig > 
FeatureDetectorConfigServer
typedef boost::shared_ptr
< FeatureDetector
FeatureDetectorPtr
typedef std::vector< float > FloatVector
typedef
::ccny_rgbd::GenerateGraphRequest_
< std::allocator< void > > 
GenerateGraphRequest
typedef boost::shared_ptr
< ::ccny_rgbd::GenerateGraphRequest
const > 
GenerateGraphRequestConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::GenerateGraphRequest
GenerateGraphRequestPtr
typedef
::ccny_rgbd::GenerateGraphResponse_
< std::allocator< void > > 
GenerateGraphResponse
typedef boost::shared_ptr
< ::ccny_rgbd::GenerateGraphResponse
const > 
GenerateGraphResponseConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::GenerateGraphResponse
GenerateGraphResponsePtr
typedef
dynamic_reconfigure::Server
< GftDetectorConfig > 
GftDetectorConfigServer
typedef boost::shared_ptr
< GftDetectorConfigServer
GftDetectorConfigServerPtr
typedef boost::shared_ptr
< GftDetector
GftDetectorPtr
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
Eigen::aligned_allocator
< RGBDKeyframe
KeyframeAllocator
typedef
Eigen::aligned_allocator
< KeyframeAssociation
KeyframeAssociationAllocator
typedef std::vector
< KeyframeAssociation,
KeyframeAssociationAllocator
KeyframeAssociationVector
typedef std::vector
< RGBDKeyframe,
KeyframeAllocator
KeyframeVector
typedef std::vector< cv::KeyPoint > KeypointVector
typedef
::ccny_rgbd::LoadRequest_
< std::allocator< void > > 
LoadRequest
typedef boost::shared_ptr
< ::ccny_rgbd::LoadRequest
const > 
LoadRequestConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::LoadRequest
LoadRequestPtr
typedef
::ccny_rgbd::LoadResponse_
< std::allocator< void > > 
LoadResponse
typedef boost::shared_ptr
< ::ccny_rgbd::LoadResponse
const > 
LoadResponseConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::LoadResponse
LoadResponsePtr
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 boost::shared_ptr
< OrbDetector
OrbDetectorPtr
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
::ccny_rgbd::PublishKeyframeRequest_
< std::allocator< void > > 
PublishKeyframeRequest
typedef boost::shared_ptr
< ::ccny_rgbd::PublishKeyframeRequest
const > 
PublishKeyframeRequestConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::PublishKeyframeRequest
PublishKeyframeRequestPtr
typedef
::ccny_rgbd::PublishKeyframeResponse_
< std::allocator< void > > 
PublishKeyframeResponse
typedef boost::shared_ptr
< ::ccny_rgbd::PublishKeyframeResponse
const > 
PublishKeyframeResponseConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::PublishKeyframeResponse
PublishKeyframeResponsePtr
typedef
::ccny_rgbd::PublishKeyframesRequest_
< std::allocator< void > > 
PublishKeyframesRequest
typedef boost::shared_ptr
< ::ccny_rgbd::PublishKeyframesRequest
const > 
PublishKeyframesRequestConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::PublishKeyframesRequest
PublishKeyframesRequestPtr
typedef
::ccny_rgbd::PublishKeyframesResponse_
< std::allocator< void > > 
PublishKeyframesResponse
typedef boost::shared_ptr
< ::ccny_rgbd::PublishKeyframesResponse
const > 
PublishKeyframesResponseConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::PublishKeyframesResponse
PublishKeyframesResponsePtr
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
::ccny_rgbd::SaveRequest_
< std::allocator< void > > 
SaveRequest
typedef boost::shared_ptr
< ::ccny_rgbd::SaveRequest
const > 
SaveRequestConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::SaveRequest
SaveRequestPtr
typedef
::ccny_rgbd::SaveResponse_
< std::allocator< void > > 
SaveResponse
typedef boost::shared_ptr
< ::ccny_rgbd::SaveResponse
const > 
SaveResponseConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::SaveResponse
SaveResponsePtr
typedef
::ccny_rgbd::SolveGraphRequest_
< std::allocator< void > > 
SolveGraphRequest
typedef boost::shared_ptr
< ::ccny_rgbd::SolveGraphRequest
const > 
SolveGraphRequestConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::SolveGraphRequest
SolveGraphRequestPtr
typedef
::ccny_rgbd::SolveGraphResponse_
< std::allocator< void > > 
SolveGraphResponse
typedef boost::shared_ptr
< ::ccny_rgbd::SolveGraphResponse
const > 
SolveGraphResponseConstPtr
typedef boost::shared_ptr
< ::ccny_rgbd::SolveGraphResponse
SolveGraphResponsePtr
typedef
dynamic_reconfigure::Server
< StarDetectorConfig > 
StarDetectorConfigServer
typedef boost::shared_ptr
< StarDetectorConfigServer
StarDetectorConfigServerPtr
typedef boost::shared_ptr
< StarDetector
StarDetectorPtr
typedef
dynamic_reconfigure::Server
< SurfDetectorConfig > 
SurfDetectorConfigServer
typedef boost::shared_ptr
< SurfDetectorConfigServer
SurfDetectorConfigServerPtr
typedef boost::shared_ptr
< SurfDetector
SurfDetectorPtr
typedef
pcl::registration::TransformationEstimationSVD
< PointFeature, PointFeature
TransformationEstimationSVD
typedef Eigen::Vector3f Vector3f
typedef std::vector
< Eigen::Vector3f > 
Vector3fVector

Enumerations

enum  DepthFitMode { DEPTH_FIT_LINEAR, DEPTH_FIT_LINEAR_ZERO, DEPTH_FIT_QUADRATIC, DEPTH_FIT_QUADRATIC_ZERO }
 Polynomial fit modes for depth unwarping. More...

Functions

void buildPointCloud (const cv::Mat &depth_img_rect, const cv::Mat &intr_rect_ir, PointCloudT &cloud)
 Constructs a point cloud, a depth image and instrinsic matrix.
void buildPointCloud (const cv::Mat &depth_img_rect_reg, const cv::Mat &rgb_img_rect, const cv::Mat &intr_rect_rgb, PointCloudT &cloud)
 Constructs a point cloud with color.
void buildRegisteredDepthImage (const cv::Mat &intr_rect_ir, const cv::Mat &intr_rect_rgb, const cv::Mat &ir2rgb, const cv::Mat &depth_img_rect, cv::Mat &depth_img_rect_reg)
 reprojects a depth image to another depth image, registered in the rgb camera's frame.
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 depthImageFloatTo16bit (const cv::Mat &depth_image_in, cv::Mat &depth_image_out)
 converts a 32FC1 depth image (in meters) to a 16UC1 depth image (in mm).
Eigen::Matrix4f eigenFromTf (const tf::Transform &tf)
 Converts an tf::Transform transform to an Eigen transform.
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.
bool loadKeyframes (KeyframeVector &keyframes, const std::string &path)
 Loads a vector of RGBD keyframes to disk.
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.
 PLUGINLIB_DECLARE_CLASS (ccny_rgbd, RGBDImageProcNodelet, RGBDImageProcNodelet, nodelet::Nodelet)
void pointCloudFromMeans (const Vector3fVector &means, PointCloudFeature &cloud)
 Creates a pcl point cloud form a vector of eigen matrix means.
void removeInvalidDistributions (const Vector3fVector &means, const Matrix3fVector &covariances, const BoolVector &valid, Vector3fVector &means_f, Matrix3fVector &covariances_f)
 Filters out a vector of means and a vector of covariances given a mask of valid entries.
void removeInvalidMeans (const Vector3fVector &means, const BoolVector &valid, Vector3fVector &means_f)
 Filters out a vector of means given a mask of valid entries.
bool saveKeyframes (const KeyframeVector &keyframes, const std::string &path)
 Saves a vector of RGBD keyframes to disk.
tf::Transform tfFromEigen (Eigen::Matrix4f trans)
 Converts an Eigen transform to a tf::Transform.
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.
void transformDistributions (Vector3fVector &means, Matrix3fVector &covariances, const tf::Transform &transform)
 Transforms a vector of means and covariances.
void transformMeans (Vector3fVector &means, const tf::Transform &transform)
 Transforms a vector of means.
void unwarpDepthImage (cv::Mat &depth_img_in, const cv::Mat &coeff0, const cv::Mat &coeff1, const cv::Mat &coeff2, int fit_mode=DEPTH_FIT_QUADRATIC)
 Given a depth image, uwarps it according to a polynomial model.

Typedef Documentation

Definition at line 41 of file AddManualKeyframe.h.

Definition at line 44 of file AddManualKeyframe.h.

Definition at line 43 of file AddManualKeyframe.h.

Definition at line 64 of file AddManualKeyframe.h.

Definition at line 67 of file AddManualKeyframe.h.

Definition at line 66 of file AddManualKeyframe.h.

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

Definition at line 59 of file types.h.

typedef sensor_msgs::CameraInfo ccny_rgbd::CameraInfoMsg

Definition at line 80 of file types.h.

Definition at line 90 of file types.h.

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

Definition at line 99 of file types.h.

typedef boost::shared_ptr<FeatureDetector> ccny_rgbd::FeatureDetectorPtr

Definition at line 119 of file feature_detector.h.

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

Definition at line 58 of file types.h.

Definition at line 41 of file GenerateGraph.h.

Definition at line 44 of file GenerateGraph.h.

Definition at line 43 of file GenerateGraph.h.

Definition at line 64 of file GenerateGraph.h.

Definition at line 67 of file GenerateGraph.h.

Definition at line 66 of file GenerateGraph.h.

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

Definition at line 101 of file types.h.

Definition at line 102 of file types.h.

typedef boost::shared_ptr<GftDetector> ccny_rgbd::GftDetectorPtr

Definition at line 74 of file gft_detector.h.

typedef sensor_msgs::Image ccny_rgbd::ImageMsg

Definition at line 79 of file types.h.

Definition at line 88 of file types.h.

Definition at line 89 of file types.h.

Definition at line 87 of file types.h.

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

Definition at line 57 of file types.h.

Definition at line 74 of file types.h.

typedef Eigen::aligned_allocator<RGBDKeyframe> ccny_rgbd::KeyframeAllocator

Definition at line 100 of file rgbd_keyframe.h.

Definition at line 70 of file keyframe_association.h.

Definition at line 71 of file keyframe_association.h.

Definition at line 101 of file rgbd_keyframe.h.

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

Definition at line 64 of file types.h.

typedef ::ccny_rgbd::LoadRequest_<std::allocator<void> > ccny_rgbd::LoadRequest

Definition at line 46 of file Load.h.

typedef boost::shared_ptr< ::ccny_rgbd::LoadRequest const> ccny_rgbd::LoadRequestConstPtr

Definition at line 49 of file Load.h.

typedef boost::shared_ptr< ::ccny_rgbd::LoadRequest> ccny_rgbd::LoadRequestPtr

Definition at line 48 of file Load.h.

typedef ::ccny_rgbd::LoadResponse_<std::allocator<void> > ccny_rgbd::LoadResponse

Definition at line 69 of file Load.h.

typedef boost::shared_ptr< ::ccny_rgbd::LoadResponse const> ccny_rgbd::LoadResponseConstPtr

Definition at line 72 of file Load.h.

typedef boost::shared_ptr< ::ccny_rgbd::LoadResponse> ccny_rgbd::LoadResponsePtr

Definition at line 71 of file Load.h.

typedef Eigen::Matrix3f ccny_rgbd::Matrix3f

Definition at line 52 of file types.h.

typedef std::vector<Eigen::Matrix3f> ccny_rgbd::Matrix3fVector

Definition at line 62 of file types.h.

typedef nav_msgs::Odometry ccny_rgbd::OdomMsg

Definition at line 81 of file types.h.

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

Definition at line 110 of file types.h.

Definition at line 111 of file types.h.

typedef boost::shared_ptr<OrbDetector> ccny_rgbd::OrbDetectorPtr

Definition at line 72 of file orb_detector.h.

typedef nav_msgs::Path ccny_rgbd::PathMsg

Definition at line 82 of file types.h.

Definition at line 86 of file types.h.

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

Definition at line 60 of file types.h.

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

Definition at line 61 of file types.h.

Definition at line 72 of file types.h.

Definition at line 69 of file types.h.

Definition at line 71 of file types.h.

Definition at line 68 of file types.h.

Definition at line 46 of file PublishKeyframe.h.

Definition at line 49 of file PublishKeyframe.h.

Definition at line 48 of file PublishKeyframe.h.

Definition at line 69 of file PublishKeyframe.h.

Definition at line 72 of file PublishKeyframe.h.

Definition at line 71 of file PublishKeyframe.h.

Definition at line 46 of file PublishKeyframes.h.

Definition at line 49 of file PublishKeyframes.h.

Definition at line 48 of file PublishKeyframes.h.

Definition at line 69 of file PublishKeyframes.h.

Definition at line 72 of file PublishKeyframes.h.

Definition at line 71 of file PublishKeyframes.h.

Definition at line 94 of file types.h.

Definition at line 95 of file types.h.

Definition at line 92 of file types.h.

Definition at line 93 of file types.h.

typedef ::ccny_rgbd::SaveRequest_<std::allocator<void> > ccny_rgbd::SaveRequest

Definition at line 46 of file Save.h.

typedef boost::shared_ptr< ::ccny_rgbd::SaveRequest const> ccny_rgbd::SaveRequestConstPtr

Definition at line 49 of file Save.h.

typedef boost::shared_ptr< ::ccny_rgbd::SaveRequest> ccny_rgbd::SaveRequestPtr

Definition at line 48 of file Save.h.

typedef ::ccny_rgbd::SaveResponse_<std::allocator<void> > ccny_rgbd::SaveResponse

Definition at line 69 of file Save.h.

typedef boost::shared_ptr< ::ccny_rgbd::SaveResponse const> ccny_rgbd::SaveResponseConstPtr

Definition at line 72 of file Save.h.

typedef boost::shared_ptr< ::ccny_rgbd::SaveResponse> ccny_rgbd::SaveResponsePtr

Definition at line 71 of file Save.h.

Definition at line 41 of file SolveGraph.h.

Definition at line 44 of file SolveGraph.h.

Definition at line 43 of file SolveGraph.h.

Definition at line 64 of file SolveGraph.h.

Definition at line 67 of file SolveGraph.h.

Definition at line 66 of file SolveGraph.h.

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

Definition at line 104 of file types.h.

Definition at line 105 of file types.h.

typedef boost::shared_ptr<StarDetector> ccny_rgbd::StarDetectorPtr

Definition at line 72 of file star_detector.h.

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

Definition at line 107 of file types.h.

Definition at line 108 of file types.h.

typedef boost::shared_ptr<SurfDetector> ccny_rgbd::SurfDetectorPtr

Definition at line 70 of file surf_detector.h.

Definition at line 75 of file types.h.

typedef Eigen::Vector3f ccny_rgbd::Vector3f

Definition at line 53 of file types.h.

typedef std::vector<Eigen::Vector3f> ccny_rgbd::Vector3fVector

Definition at line 63 of file types.h.


Enumeration Type Documentation

Polynomial fit modes for depth unwarping.

The modes include:

  • DEPTH_FIT_LINEAR (c0 + c1*d)
  • DEPTH_FIT_LINEAR_ZERO (c1*d)
  • DEPTH_FIT_QUADRATIC (c0 + c1*d + c2*d^2)
  • DEPTH_FIT_QUADRATIC_ZERO (c1*d + c2*d^2)

The recommended mode is DEPTH_FIT_QUADRATIC

Enumerator:
DEPTH_FIT_LINEAR 
DEPTH_FIT_LINEAR_ZERO 
DEPTH_FIT_QUADRATIC 
DEPTH_FIT_QUADRATIC_ZERO 

Definition at line 41 of file proc_util.h.


Function Documentation

void ccny_rgbd::buildPointCloud ( const cv::Mat &  depth_img_rect,
const cv::Mat &  intr_rect_ir,
PointCloudT &  cloud 
)

Constructs a point cloud, a depth image and instrinsic matrix.

Parameters:
depth_img_rectrectified depth image (16UC1, in mm)
intr_rect_irintinsic matrix of the rectified depth image
cloudreference to teh output point cloud

Definition at line 308 of file rgbd_util.cpp.

void ccny_rgbd::buildPointCloud ( const cv::Mat &  depth_img_rect_reg,
const cv::Mat &  rgb_img_rect,
const cv::Mat &  intr_rect_rgb,
PointCloudT &  cloud 
)

Constructs a point cloud with color.

Prior to calling this functions, both images need to be rectified, and the depth image has to be registered into the frame of the RGB image.

Parameters:
depth_img_rect_regrectified and registered depth image (16UC1, in mm)
rgb_img_rectrectified rgb image (8UC3)
intr_rect_rgbintrinsic matrix
cloudreference to the output point cloud

Definition at line 348 of file rgbd_util.cpp.

void ccny_rgbd::buildRegisteredDepthImage ( const cv::Mat &  intr_rect_ir,
const cv::Mat &  intr_rect_rgb,
const cv::Mat &  ir2rgb,
const cv::Mat &  depth_img_rect,
cv::Mat &  depth_img_rect_reg 
)

reprojects a depth image to another depth image, registered in the rgb camera's frame.

Both images need to be rectified first. ir2rgb is a matrix such that for any point P_IR in the depth camera frame P_RGB = ir2rgb * P_IR

Parameters:
intr_rect_irintrinsic matrix of the rectified depth image
intr_rect_rgbintrinsic matrix of the rectified RGB image
ir2rgbextrinsic matrix between the IR(depth) and RGB cameras
depth_img_rectthe input image: rectified depth image
depth_img_rect_regthe output image: rectified and registered into the RGB frame

Definition at line 395 of file rgbd_util.cpp.

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 201 of file rgbd_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 224 of file rgbd_util.cpp.

void ccny_rgbd::depthImageFloatTo16bit ( const cv::Mat &  depth_image_in,
cv::Mat &  depth_image_out 
)

converts a 32FC1 depth image (in meters) to a 16UC1 depth image (in mm).

Parameters:
depth_image_inthe input 32FC1 image
depth_image_outthe output 16UC1 image

Definition at line 475 of file rgbd_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 53 of file rgbd_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 101 of file rgbd_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 28 of file rgbd_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 35 of file rgbd_util.cpp.

bool ccny_rgbd::loadKeyframes ( KeyframeVector &  keyframes,
const std::string &  path 
)

Loads a vector of RGBD keyframes to disk.

Parameters:
keyframesReference to the keyframe being saved
pathThe path to the folder where everything will be stored
Return values:
trueSuccessfully saved the data
falseSaving failed - for example, cannot create directory

Definition at line 126 of file rgbd_keyframe.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 182 of file rgbd_util.cpp.

ccny_rgbd::PLUGINLIB_DECLARE_CLASS ( ccny_rgbd  ,
RGBDImageProcNodelet  ,
RGBDImageProcNodelet  ,
nodelet::Nodelet   
)
void ccny_rgbd::pointCloudFromMeans ( const Vector3fVector &  means,
PointCloudFeature &  cloud 
)

Creates a pcl point cloud form a vector of eigen matrix means.

Parameters:
meansvector of 3x1 matrices of positions (3D means)
cloudreference to the output cloud

Definition at line 286 of file rgbd_util.cpp.

void ccny_rgbd::removeInvalidDistributions ( const Vector3fVector &  means,
const Matrix3fVector &  covariances,
const BoolVector &  valid,
Vector3fVector &  means_f,
Matrix3fVector &  covariances_f 
)

Filters out a vector of means and a vector of covariances given a mask of valid entries.

Parameters:
meansinput vector of 3x1 matrices
covariancesinput vector of 3x3 matrices
validvector mask of valid flags
means_foutput vector of 3x1 matrices
covariances_foutput vector of 3x1 matrices

Definition at line 122 of file rgbd_util.cpp.

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

Filters out a vector of means given a mask of valid entries.

Parameters:
meansinput vector of 3x1 matrices
validvector mask of valid flags
means_foutput vector of 3x1 matrices

Definition at line 106 of file rgbd_util.cpp.

bool ccny_rgbd::saveKeyframes ( const KeyframeVector &  keyframes,
const std::string &  path 
)

Saves a vector of RGBD keyframes to disk.

Parameters:
keyframesReference to the keyframe being saved
pathThe path to the folder where everything will be stored
Return values:
trueSuccessfully saved the data
falseSaving failed - for example, cannot create directory

Definition at line 108 of file rgbd_keyframe.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 41 of file rgbd_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 87 of file rgbd_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 143 of file rgbd_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 162 of file rgbd_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 74 of file rgbd_util.cpp.

void ccny_rgbd::transformDistributions ( Vector3fVector &  means,
Matrix3fVector &  covariances,
const tf::Transform transform 
)

Transforms a vector of means and covariances.

Parameters:
meansvector of 3x1 matrices of positions (3D means)
covariancesvector of 3x3 covariance matrices
transformthe tranformation to be applied to all the means and covariances

Definition at line 266 of file rgbd_util.cpp.

void ccny_rgbd::transformMeans ( Vector3fVector &  means,
const tf::Transform transform 
)

Transforms a vector of means.

Parameters:
meansvector of 3x1 matrices of positions (3D means)
transformthe tranformation to be applied to all the means

Definition at line 250 of file rgbd_util.cpp.

void ccny_rgbd::unwarpDepthImage ( cv::Mat &  depth_img_in,
const cv::Mat &  coeff0,
const cv::Mat &  coeff1,
const cv::Mat &  coeff2,
int  fit_mode = DEPTH_FIT_QUADRATIC 
)

Given a depth image, uwarps it according to a polynomial model.

The size of the c matrices should be equal to the image size.

Parameters:
depth_img_indepth image to be unwarped (16UC1, in mm)
coeff0matrix of c0 coefficients
coeff1matrix of c1 coefficients
coeff2matrix of c2 coefficients
fit_modethe polynomial fitting mode, see DepthFitMode. d = c0 + c1*d + c2*d^2

Definition at line 28 of file proc_util.cpp.

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends


ccny_rgbd
Author(s): Ivan Dryanovski
autogenerated on Fri Apr 12 2013 20:38:48