All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends
Public Member Functions | Protected Member Functions | Protected Attributes | Private Member Functions | Static Private Member Functions | Private Attributes
ccny_rgbd::RGBNavigator Class Reference

Builds a 3D map from a series of RGBD keyframes. More...

#include <rgb_navigator.h>

List of all members.

Public Member Functions

bool addManualKeyframeSrvCallback (AddManualKeyframe::Request &request, AddManualKeyframe::Response &response)
 ROS callback to manually request adding a keyframe.
bool generateGraphSrvCallback (GenerateGraph::Request &request, GenerateGraph::Response &response)
 ROS callback to generate the graph of keyframe correspondences for global alignment.
void initParams ()
 Initializes all the parameters from the ROS param server.
bool loadKeyframesSrvCallback (Load::Request &request, Load::Response &response)
 ROS callback load keyframes from disk.
bool publishKeyframeSrvCallback (PublishKeyframe::Request &request, PublishKeyframe::Response &response)
 ROS callback to publish a single keyframe as point clouds.
bool publishKeyframesSrvCallback (PublishKeyframes::Request &request, PublishKeyframes::Response &response)
 ROS callback to publish keyframes as point clouds.
 RGBNavigator (const ros::NodeHandle &nh, const ros::NodeHandle &nh_private)
 Constructor from ROS nodehandles.
bool saveKeyframesSrvCallback (Save::Request &request, Save::Response &response)
 ROS callback save all the keyframes to disk.
bool saveOctomapSrvCallback (Save::Request &request, Save::Response &response)
 ROS callback to create an Octomap and save it to file.
bool savePcdMapSrvCallback (Save::Request &request, Save::Response &response)
 ROS callback to create an aggregate 3D map and save it to pcd file.
bool solveGraphSrvCallback (SolveGraph::Request &request, SolveGraph::Response &response)
 ROS callback to perform global alignment.
virtual ~RGBNavigator ()
 Default destructor.

Protected Member Functions

virtual void RGBCallback (const ImageMsg::ConstPtr &rgb_msg, const CameraInfoMsg::ConstPtr &info_msg)
 Main callback for RGB, Depth, and CameraInfo messages.

Protected Attributes

std::vector< int > aruco_kfs
std::vector< std::vector
< double > > 
aruco_pos
std::string fixed_frame_
 the fixed frame (usually "odom")
rgbdtools::KeyframeVector keyframes_
 vector of RGBD Keyframes
double max_range_
 Maximum threshold for range (in the z-coordinate of the camera frame)
double max_stdev_
 Maximum threshold for range (z-coordinate) standard deviation.
ros::NodeHandle nh_
 public nodehandle
ros::NodeHandle nh_private_
 private nodepcdhandle
int queue_size_
 Subscription queue size.

Private Member Functions

void addKeyframe (const rgbdtools::RGBDFrame &frame, const AffineTransform &pose)
 creates a keyframe from an RGBD frame and inserts it in the keyframe vector.
void buildColorOctomap (octomap::ColorOcTree &tree)
 Builds an octomap octree from all keyframes, with color.
void buildFullCloud (PointCloudT &map_cloud)
void buildPcdMap (PointCloudT &map_cloud)
 Builds an pcd map from all keyframes.
void filter_aruco ()
bool loadPath (const std::string &filepath)
bool processFrame (const rgbdtools::RGBDFrame &frame, const AffineTransform &pose)
 processes an incoming RGBD frame with a given pose, and determines whether a keyframe should be inserted
void publishArucoPos ()
void publishKeyframeAssociations ()
 Publishes all the keyframe associations markers.
void publishKeyframeData (int i)
 Publishes the point cloud associated with a keyframe.
void publishKeyframePose (int i)
 Publishes the pose marker associated with a keyframe.
void publishKeyframePoses ()
 Publishes all the keyframe pose markers.
void publishPath ()
 Publishes all the path message.
bool saveOctomap (const std::string &path)
 Save the full map to disk as octomap.
bool savePath (const std::string &filepath)
bool savePathTUMFormat (const std::string &filepath)
bool savePcdMap (const std::string &path)
 Save the full map to disk as pcd.
void updatePathFromKeyframePoses ()

Static Private Member Functions

static octomap::point3d pointTfToOctomap (const tf::Point &ptTf)
 Convert a tf point to octomap point.
static octomap::pose6d poseTfToOctomap (const tf::Pose &poseTf)
 Convert a tf pose to octomap pose.
static octomath::Quaternion quaternionTfToOctomap (const tf::Quaternion &qTf)
 Convert a tf quaternion to octomap quaternion.

Private Attributes

ros::ServiceServer add_manual_keyframe_service_
 ROS service to add a manual keyframe.
PathMsg aruco_path
 < contains a vector of positions of the camera (not base) pose
rgbdtools::KeyframeAssociationVector associations_
 keyframe associations that form the graph
boost::shared_ptr< ImageTransportdepth_it_
 Image transport for depth message subscription.
ros::ServiceServer generate_graph_service_
 ROS service to generate the graph correpondences.
rgbdtools::KeyframeGraphDetector graph_detector_
 builds graph from the keyframes
rgbdtools::KeyframeGraphSolverG2O graph_solver_
 optimizes the graph for global alignement
ros::Publisher keyframes_pub_
 ROS publisher for the keyframe point clouds.
double kf_angle_eps_
 angular distance threshold between keyframes
ros::Publisher kf_assoc_pub_
 ROS publisher for the keyframe associations.
double kf_dist_eps_
 linear distance threshold between keyframes
ros::ServiceServer load_kf_service_
 ROS service to load all keyframes from disk.
bool manual_add_
 flag indicating whetehr a manual add has been requested
double max_map_z_
 maximum z (in fixed frame) when exporting maps.
double min_map_z_
 min z (in fixed frame) when exporting maps.
double octomap_res_
 tree resolution for octomap (in meters)
bool octomap_with_color_
 whetehr to save Octomaps with color info
PathMsg path_msg_
ros::Publisher path_pub_
 ROS publisher for the keyframe path.
double pcd_map_res_
 downsampling resolution of pcd map (in meters)
ros::Publisher poses_pub_
 ROS publisher for the keyframe poses.
ros::ServiceServer pub_keyframe_service_
 ROS service to publish a single keyframe.
ros::ServiceServer pub_keyframes_service_
 ROS service to publish a subset of keyframes.
boost::shared_ptr< ImageTransportrgb_it_
 Image transport for RGB message subscription.
int rgbd_frame_index_
ros::ServiceServer save_kf_service_
 ROS service to save all keyframes to disk.
ros::ServiceServer save_octomap_service_
 ROS service to save octomap to disk.
ros::ServiceServer save_pcd_map_service_
 ROS service to save the entire map as pcd to disk.
ros::ServiceServer solve_graph_service_
 ROS service to optimize the graph.
rgbdtools::KeyframeGraphSolverISAM solver_
ImageSubFilter sub_depth_
 Depth message subscriber.
CameraInfoSubFilter sub_info_
 Camera info message subscriber.
ImageSubFilter sub_rgb_
 RGB message subscriber.
boost::shared_ptr
< RGBSynchronizer
sync_
 Callback syncronizer.
tf::TransformListener tf_listener_
 ROS transform listener.

Detailed Description

Builds a 3D map from a series of RGBD keyframes.

The KeyframeMapper app subscribes to a stream of RGBD images, as well a transform between a fixed and a moving frame (generated, for example, by the VisualOdometry app). The mapper stores a sequence of keyframes containing all the necessary data to build a textured 3D point cloud map.

The class provides an interface to perform graph-based global alignement (in post-processing).

Additionally, the class provides an interface to save and load keyframes to file, so that post-processing can be done with offline data.

Definition at line 69 of file rgb_navigator.h.


Constructor & Destructor Documentation

ccny_rgbd::RGBNavigator::RGBNavigator ( const ros::NodeHandle nh,
const ros::NodeHandle nh_private 
)

Constructor from ROS nodehandles.

Parameters:
nhthe public nodehandle
nh_privatethe private nodehandle

Definition at line 28 of file rgb_navigator.cpp.

Default destructor.

Definition at line 89 of file rgb_navigator.cpp.


Member Function Documentation

void ccny_rgbd::RGBNavigator::addKeyframe ( const rgbdtools::RGBDFrame frame,
const AffineTransform pose 
) [private]

creates a keyframe from an RGBD frame and inserts it in the keyframe vector.

Parameters:
framethe incoming RGBD frame (image)
posethe pose of the base frame when RGBD image was taken

Definition at line 406 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::addManualKeyframeSrvCallback ( AddManualKeyframe::Request &  request,
AddManualKeyframe::Response &  response 
)

ROS callback to manually request adding a keyframe.

Definition at line 869 of file rgb_navigator.cpp.

void ccny_rgbd::RGBNavigator::buildColorOctomap ( octomap::ColorOcTree &  tree) [private]

Builds an octomap octree from all keyframes, with color.

Parameters:
treereference to the octomap octree

Definition at line 1116 of file rgb_navigator.cpp.

void ccny_rgbd::RGBNavigator::buildFullCloud ( PointCloudT map_cloud) [private]

Definition at line 1063 of file rgb_navigator.cpp.

void ccny_rgbd::RGBNavigator::buildPcdMap ( PointCloudT map_cloud) [private]

Builds an pcd map from all keyframes.

Parameters:
map_cloudthe point cloud to be built

Definition at line 1034 of file rgb_navigator.cpp.

Definition at line 150 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::generateGraphSrvCallback ( GenerateGraph::Request &  request,
GenerateGraph::Response &  response 
)

ROS callback to generate the graph of keyframe correspondences for global alignment.

Definition at line 879 of file rgb_navigator.cpp.

Initializes all the parameters from the ROS param server.

Definition at line 95 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::loadKeyframesSrvCallback ( Load::Request &  request,
Load::Response &  response 
)

ROS callback load keyframes from disk.

The argument should be a string with the directory pointing to the keyframes.

Definition at line 698 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::loadPath ( const std::string &  filepath) [private]

Definition at line 1266 of file rgb_navigator.cpp.

static octomap::point3d ccny_rgbd::RGBNavigator::pointTfToOctomap ( const tf::Point &  ptTf) [inline, static, private]

Convert a tf point to octomap point.

Parameters:
poseTfthe tf point
Returns:
octomap point

Definition at line 358 of file rgb_navigator.h.

static octomap::pose6d ccny_rgbd::RGBNavigator::poseTfToOctomap ( const tf::Pose poseTf) [inline, static, private]

Convert a tf pose to octomap pose.

Parameters:
poseTfthe tf pose
Returns:
octomap pose

Definition at line 346 of file rgb_navigator.h.

bool ccny_rgbd::RGBNavigator::processFrame ( const rgbdtools::RGBDFrame frame,
const AffineTransform pose 
) [private]

processes an incoming RGBD frame with a given pose, and determines whether a keyframe should be inserted

Parameters:
framethe incoming RGBD frame (image)
posethe pose of the base frame when RGBD image was taken
Return values:
truea keyframe was inserted
falseno keyframe was inserted

Definition at line 352 of file rgb_navigator.cpp.

Definition at line 565 of file rgb_navigator.cpp.

Publishes all the keyframe associations markers.

Definition at line 496 of file rgb_navigator.cpp.

Publishes the point cloud associated with a keyframe.

Parameters:
ithe keyframe index

Definition at line 475 of file rgb_navigator.cpp.

Publishes the pose marker associated with a keyframe.

Parameters:
ithe keyframe index

Definition at line 594 of file rgb_navigator.cpp.

Publishes all the keyframe pose markers.

Definition at line 557 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::publishKeyframeSrvCallback ( PublishKeyframe::Request &  request,
PublishKeyframe::Response &  response 
)

ROS callback to publish a single keyframe as point clouds.

The argument should be an integer with the idnex of the keyframe

Definition at line 422 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::publishKeyframesSrvCallback ( PublishKeyframes::Request &  request,
PublishKeyframes::Response &  response 
)

ROS callback to publish keyframes as point clouds.

The argument should be a regular expression string matching the index of the desired keyframes to be published.

Note: passing a string may require escaping the quotation marks, otherwise ROS sometimes confuses it for an integer

Examples:

  • /publish_keyframes ".*" --> publishes all keyframes
  • /publish_keyframes "[1-9]" --> publishes keyframes 1 to 9

The keyframe point clouds are published one by one.

Definition at line 442 of file rgb_navigator.cpp.

Publishes all the path message.

Definition at line 1173 of file rgb_navigator.cpp.

static octomath::Quaternion ccny_rgbd::RGBNavigator::quaternionTfToOctomap ( const tf::Quaternion &  qTf) [inline, static, private]

Convert a tf quaternion to octomap quaternion.

Parameters:
poseTfthe tf quaternion
Returns:
octomap quaternion

Definition at line 368 of file rgb_navigator.h.

void ccny_rgbd::RGBNavigator::RGBCallback ( const ImageMsg::ConstPtr &  rgb_msg,
const CameraInfoMsg::ConstPtr &  info_msg 
) [protected, virtual]

Main callback for RGB, Depth, and CameraInfo messages.

Parameters:
depth_msgDepth message (16UC1, in mm)
rgb_msgRGB message (8UC3)
info_msgCameraInfo message, applies to both RGB and depth images

Definition at line 281 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::saveKeyframesSrvCallback ( Save::Request &  request,
Save::Response &  response 
)

ROS callback save all the keyframes to disk.

The argument should be a string with the directory where to save the keyframes.

Definition at line 676 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::saveOctomap ( const std::string &  path) [private]

Save the full map to disk as octomap.

Parameters:
pathpath to save the map to
Return values:
truesave was successful
falsesave failed.

Definition at line 1085 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::saveOctomapSrvCallback ( Save::Request &  request,
Save::Response &  response 
)

ROS callback to create an Octomap and save it to file.

The resolution of the map can be controlled via the octomap_res_ parameter.

The argument should be the path to the .bt file

Definition at line 756 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::savePath ( const std::string &  filepath) [private]

Definition at line 1206 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::savePathTUMFormat ( const std::string &  filepath) [private]

Definition at line 1237 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::savePcdMap ( const std::string &  path) [private]

Save the full map to disk as pcd.

Parameters:
pathpath to save the map to
Return values:
truesave was successful
falsesave failed.

Definition at line 1021 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::savePcdMapSrvCallback ( Save::Request &  request,
Save::Response &  response 
)

ROS callback to create an aggregate 3D map and save it to pcd file.

The resolution of the map can be controlled via the pcd_map_res_ parameter.

The argument should be the path to the .pcd file

Definition at line 742 of file rgb_navigator.cpp.

bool ccny_rgbd::RGBNavigator::solveGraphSrvCallback ( SolveGraph::Request &  request,
SolveGraph::Response &  response 
)

ROS callback to perform global alignment.

Note: The generate_graph service should be called prior to invoking this service.

Definition at line 893 of file rgb_navigator.cpp.

In the event that the keyframe poses change (from pose-graph solving) this function will propagete teh changes in the path message

Definition at line 928 of file rgb_navigator.cpp.


Member Data Documentation

ROS service to add a manual keyframe.

Definition at line 234 of file rgb_navigator.h.

std::vector<int> ccny_rgbd::RGBNavigator::aruco_kfs [protected]

Definition at line 180 of file rgb_navigator.h.

< contains a vector of positions of the camera (not base) pose

Definition at line 276 of file rgb_navigator.h.

std::vector<std::vector<double> > ccny_rgbd::RGBNavigator::aruco_pos [protected]

Definition at line 179 of file rgb_navigator.h.

keyframe associations that form the graph

Definition at line 273 of file rgb_navigator.h.

boost::shared_ptr<ImageTransport> ccny_rgbd::RGBNavigator::depth_it_ [private]

Image transport for depth message subscription.

Definition at line 242 of file rgb_navigator.h.

std::string ccny_rgbd::RGBNavigator::fixed_frame_ [protected]

the fixed frame (usually "odom")

Definition at line 184 of file rgb_navigator.h.

ROS service to generate the graph correpondences.

Definition at line 210 of file rgb_navigator.h.

builds graph from the keyframes

Definition at line 270 of file rgb_navigator.h.

optimizes the graph for global alignement

Definition at line 271 of file rgb_navigator.h.

vector of RGBD Keyframes

Definition at line 191 of file rgb_navigator.h.

ROS publisher for the keyframe point clouds.

Definition at line 204 of file rgb_navigator.h.

angular distance threshold between keyframes

Definition at line 260 of file rgb_navigator.h.

ROS publisher for the keyframe associations.

Definition at line 206 of file rgb_navigator.h.

linear distance threshold between keyframes

Definition at line 259 of file rgb_navigator.h.

ROS service to load all keyframes from disk.

Definition at line 231 of file rgb_navigator.h.

flag indicating whetehr a manual add has been requested

Definition at line 266 of file rgb_navigator.h.

maximum z (in fixed frame) when exporting maps.

Definition at line 262 of file rgb_navigator.h.

Maximum threshold for range (in the z-coordinate of the camera frame)

Definition at line 188 of file rgb_navigator.h.

Maximum threshold for range (z-coordinate) standard deviation.

Definition at line 189 of file rgb_navigator.h.

min z (in fixed frame) when exporting maps.

Definition at line 263 of file rgb_navigator.h.

public nodehandle

Definition at line 181 of file rgb_navigator.h.

private nodepcdhandle

Definition at line 182 of file rgb_navigator.h.

tree resolution for octomap (in meters)

Definition at line 258 of file rgb_navigator.h.

whetehr to save Octomaps with color info

Definition at line 261 of file rgb_navigator.h.

Definition at line 275 of file rgb_navigator.h.

ROS publisher for the keyframe path.

Definition at line 207 of file rgb_navigator.h.

downsampling resolution of pcd map (in meters)

Definition at line 257 of file rgb_navigator.h.

ROS publisher for the keyframe poses.

Definition at line 205 of file rgb_navigator.h.

ROS service to publish a single keyframe.

Definition at line 216 of file rgb_navigator.h.

ROS service to publish a subset of keyframes.

Definition at line 219 of file rgb_navigator.h.

Subscription queue size.

Definition at line 186 of file rgb_navigator.h.

boost::shared_ptr<ImageTransport> ccny_rgbd::RGBNavigator::rgb_it_ [private]

Image transport for RGB message subscription.

Definition at line 239 of file rgb_navigator.h.

Definition at line 268 of file rgb_navigator.h.

ROS service to save all keyframes to disk.

Definition at line 222 of file rgb_navigator.h.

ROS service to save octomap to disk.

Definition at line 228 of file rgb_navigator.h.

ROS service to save the entire map as pcd to disk.

Definition at line 225 of file rgb_navigator.h.

ROS service to optimize the graph.

Definition at line 213 of file rgb_navigator.h.

Definition at line 272 of file rgb_navigator.h.

Depth message subscriber.

Definition at line 251 of file rgb_navigator.h.

Camera info message subscriber.

Definition at line 254 of file rgb_navigator.h.

RGB message subscriber.

Definition at line 248 of file rgb_navigator.h.

boost::shared_ptr<RGBSynchronizer> ccny_rgbd::RGBNavigator::sync_ [private]

Callback syncronizer.

Definition at line 245 of file rgb_navigator.h.

tf::TransformListener ccny_rgbd::RGBNavigator::tf_listener_ [private]

ROS transform listener.

Definition at line 236 of file rgb_navigator.h.


The documentation for this class was generated from the following files:
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


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