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

#include <keyframe_mapper.h>

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.
	KeyframeMapper (const ros::NodeHandle &nh, const ros::NodeHandle &nh_private)
	Constructor from ROS nodehandles.
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.
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	~KeyframeMapper ()
	Default destructor.
Protected Member Functions
virtual void	RGBDCallback (const ImageMsg::ConstPtr &rgb_msg, const ImageMsg::ConstPtr &depth_msg, const CameraInfoMsg::ConstPtr &info_msg)
	Main callback for RGB, Depth, and CameraInfo messages.
Protected Attributes
std::string	fixed_frame_
	the fixed frame (usually "odom")
KeyframeVector	keyframes_
	vector of RGBD Keyframes
ros::NodeHandle	nh_
	public nodehandle
ros::NodeHandle	nh_private_
	private nodepcdhandle
int	queue_size_
	Subscription queue size.
Private Member Functions
void	addKeyframe (const RGBDFrame &frame, const tf::Transform &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	buildOctomap (octomap::OcTree &tree)
	Builds an octomap octree from all keyframes.
void	buildPcdMap (PointCloudT &map_cloud)
	Builds an pcd map from all keyframes.
bool	processFrame (const RGBDFrame &frame, const tf::Transform &pose)
	< contains a vector of positions of the camera (not base) pose
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	savePcdMap (const std::string &path)
	Save the full map to disk as pcd.
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.
KeyframeAssociationVector	associations_
	keyframe associations that form the graph
boost::shared_ptr< ImageTransport >	depth_it_
	Image transport for depth message subscription.
ros::ServiceServer	generate_graph_service_
	ROS service to generate the graph correpondences.
KeyframeGraphDetector	graph_detector_
	builds graph from the keyframes
KeyframeGraphSolver *	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	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< ImageTransport >	rgb_it_
	Image transport for RGB message subscription.
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.
ImageSubFilter	sub_depth_
	Depth message subscriber.
CameraInfoSubFilter	sub_info_
	Camera info message subscriber.
ImageSubFilter	sub_rgb_
	RGB message subscriber.
boost::shared_ptr < RGBDSynchronizer3 >	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 68 of file keyframe_mapper.h.

Constructor & Destructor Documentation

ccny_rgbd::KeyframeMapper::KeyframeMapper	(	const ros::NodeHandle &	nh,
		const ros::NodeHandle &	nh_private
	)

Constructor from ROS nodehandles.

Parameters:

nh	the public nodehandle
nh_private	the private nodehandle

Definition at line 28 of file keyframe_mapper.cpp.

ccny_rgbd::KeyframeMapper::~KeyframeMapper ( ) [virtual]

Default destructor.

Definition at line 108 of file keyframe_mapper.cpp.

Member Function Documentation

void ccny_rgbd::KeyframeMapper::addKeyframe	(	const RGBDFrame &	frame,
		const tf::Transform &	pose
	)		`[private]`

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

Parameters:

frame	the incoming RGBD frame (image)
pose	the pose of the base frame when RGBD image was taken

Definition at line 166 of file keyframe_mapper.cpp.

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

ROS callback to manually request adding a keyframe.

Definition at line 442 of file keyframe_mapper.cpp.

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

Builds an octomap octree from all keyframes, with color.

Parameters:

tree	reference to the octomap octree

Definition at line 564 of file keyframe_mapper.cpp.

void ccny_rgbd::KeyframeMapper::buildOctomap ( octomap::OcTree & tree ) [private]

Builds an octomap octree from all keyframes.

Parameters:

tree	reference to the octomap octree

Definition at line 535 of file keyframe_mapper.cpp.

void ccny_rgbd::KeyframeMapper::buildPcdMap ( PointCloudT & map_cloud ) [private]

Builds an pcd map from all keyframes.

Parameters:

map_cloud the point cloud to be built

Definition at line 488 of file keyframe_mapper.cpp.

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

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

Definition at line 451 of file keyframe_mapper.cpp.

bool ccny_rgbd::KeyframeMapper::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 400 of file keyframe_mapper.cpp.

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

Convert a tf point to octomap point.

Parameters:

poseTf the tf point

Returns:: octomap point

Definition at line 348 of file keyframe_mapper.h.

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

Convert a tf pose to octomap pose.

Parameters:

poseTf the tf pose

Returns:: octomap pose

Definition at line 336 of file keyframe_mapper.h.

bool ccny_rgbd::KeyframeMapper::processFrame	(	const RGBDFrame &	frame,
		const tf::Transform &	pose
	)		`[private]`

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

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

Parameters:

frame	the incoming RGBD frame (image)
pose	the pose of the base frame when RGBD image was taken

Return values:

true	a keyframe was inserted
false	no keyframe was inserted

Definition at line 137 of file keyframe_mapper.cpp.

void ccny_rgbd::KeyframeMapper::publishKeyframeAssociations ( ) [private]

Publishes all the keyframe associations markers.

Definition at line 251 of file keyframe_mapper.cpp.

void ccny_rgbd::KeyframeMapper::publishKeyframeData ( int i ) [private]

Publishes the point cloud associated with a keyframe.

Parameters:

i	the keyframe index

Definition at line 234 of file keyframe_mapper.cpp.

void ccny_rgbd::KeyframeMapper::publishKeyframePose ( int i ) [private]

Publishes the pose marker associated with a keyframe.

Parameters:

i	the keyframe index

Definition at line 317 of file keyframe_mapper.cpp.

void ccny_rgbd::KeyframeMapper::publishKeyframePoses ( ) [private]

Publishes all the keyframe pose markers.

Definition at line 309 of file keyframe_mapper.cpp.

bool ccny_rgbd::KeyframeMapper::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 182 of file keyframe_mapper.cpp.

bool ccny_rgbd::KeyframeMapper::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 202 of file keyframe_mapper.cpp.

void ccny_rgbd::KeyframeMapper::publishPath ( ) [private]

Publishes all the path message.

Definition at line 610 of file keyframe_mapper.cpp.

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

Convert a tf quaternion to octomap quaternion.

Parameters:

poseTf the tf quaternion

Returns:: octomap quaternion

Definition at line 358 of file keyframe_mapper.h.

void ccny_rgbd::KeyframeMapper::RGBDCallback	(	const ImageMsg::ConstPtr &	rgb_msg,
		const ImageMsg::ConstPtr &	depth_msg,
		const CameraInfoMsg::ConstPtr &	info_msg
	)		`[protected, virtual]`

Main callback for RGB, Depth, and CameraInfo messages.

Parameters:

depth_msg	Depth message (16UC1, in mm)
rgb_msg	RGB message (8UC3)
info_msg	CameraInfo message, applies to both RGB and depth images

Definition at line 113 of file keyframe_mapper.cpp.

bool ccny_rgbd::KeyframeMapper::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 386 of file keyframe_mapper.cpp.

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

Save the full map to disk as octomap.

Parameters:

path	path to save the map to

Return values:

true	save was successful
false	save failed.

Definition at line 515 of file keyframe_mapper.cpp.

bool ccny_rgbd::KeyframeMapper::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 428 of file keyframe_mapper.cpp.

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

Save the full map to disk as pcd.

Parameters:

path	path to save the map to

Return values:

true	save was successful
false	save failed.

Definition at line 476 of file keyframe_mapper.cpp.

bool ccny_rgbd::KeyframeMapper::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 414 of file keyframe_mapper.cpp.

bool ccny_rgbd::KeyframeMapper::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 464 of file keyframe_mapper.cpp.

Member Data Documentation

ros::ServiceServer ccny_rgbd::KeyframeMapper::add_manual_keyframe_service_ [private]

ROS service to add a manual keyframe.

Definition at line 226 of file keyframe_mapper.h.

KeyframeAssociationVector ccny_rgbd::KeyframeMapper::associations_ [private]

keyframe associations that form the graph

Definition at line 261 of file keyframe_mapper.h.

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

Image transport for depth message subscription.

Definition at line 234 of file keyframe_mapper.h.

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

the fixed frame (usually "odom")

Definition at line 178 of file keyframe_mapper.h.

ros::ServiceServer ccny_rgbd::KeyframeMapper::generate_graph_service_ [private]

ROS service to generate the graph correpondences.

Definition at line 202 of file keyframe_mapper.h.

KeyframeGraphDetector ccny_rgbd::KeyframeMapper::graph_detector_ [private]

builds graph from the keyframes

Definition at line 258 of file keyframe_mapper.h.

KeyframeGraphSolver* ccny_rgbd::KeyframeMapper::graph_solver_ [private]

optimizes the graph for global alignement

Definition at line 259 of file keyframe_mapper.h.

KeyframeVector ccny_rgbd::KeyframeMapper::keyframes_ [protected]

vector of RGBD Keyframes

Definition at line 182 of file keyframe_mapper.h.

ros::Publisher ccny_rgbd::KeyframeMapper::keyframes_pub_ [private]

ROS publisher for the keyframe point clouds.

Definition at line 196 of file keyframe_mapper.h.

double ccny_rgbd::KeyframeMapper::kf_angle_eps_ [private]

angular distance threshold between keyframes

Definition at line 252 of file keyframe_mapper.h.

ros::Publisher ccny_rgbd::KeyframeMapper::kf_assoc_pub_ [private]

ROS publisher for the keyframe associations.

Definition at line 198 of file keyframe_mapper.h.

double ccny_rgbd::KeyframeMapper::kf_dist_eps_ [private]

linear distance threshold between keyframes

Definition at line 251 of file keyframe_mapper.h.

ros::ServiceServer ccny_rgbd::KeyframeMapper::load_kf_service_ [private]

ROS service to load all keyframes from disk.

Definition at line 223 of file keyframe_mapper.h.

bool ccny_rgbd::KeyframeMapper::manual_add_ [private]

flag indicating whetehr a manual add has been requested

Definition at line 256 of file keyframe_mapper.h.

ros::NodeHandle ccny_rgbd::KeyframeMapper::nh_ [protected]

public nodehandle

Definition at line 175 of file keyframe_mapper.h.

ros::NodeHandle ccny_rgbd::KeyframeMapper::nh_private_ [protected]

private nodepcdhandle

Definition at line 176 of file keyframe_mapper.h.

double ccny_rgbd::KeyframeMapper::octomap_res_ [private]

tree resolution for octomap (in meters)

Definition at line 250 of file keyframe_mapper.h.

bool ccny_rgbd::KeyframeMapper::octomap_with_color_ [private]

whetehr to save Octomaps with color info

Definition at line 253 of file keyframe_mapper.h.

PathMsg ccny_rgbd::KeyframeMapper::path_msg_ [private]

Definition at line 263 of file keyframe_mapper.h.

ros::Publisher ccny_rgbd::KeyframeMapper::path_pub_ [private]

ROS publisher for the keyframe path.

Definition at line 199 of file keyframe_mapper.h.

double ccny_rgbd::KeyframeMapper::pcd_map_res_ [private]

downsampling resolution of pcd map (in meters)

Definition at line 249 of file keyframe_mapper.h.

ros::Publisher ccny_rgbd::KeyframeMapper::poses_pub_ [private]

ROS publisher for the keyframe poses.

Definition at line 197 of file keyframe_mapper.h.

ros::ServiceServer ccny_rgbd::KeyframeMapper::pub_keyframe_service_ [private]

ROS service to publish a single keyframe.

Definition at line 208 of file keyframe_mapper.h.

ros::ServiceServer ccny_rgbd::KeyframeMapper::pub_keyframes_service_ [private]

ROS service to publish a subset of keyframes.

Definition at line 211 of file keyframe_mapper.h.

int ccny_rgbd::KeyframeMapper::queue_size_ [protected]

Subscription queue size.

Definition at line 180 of file keyframe_mapper.h.

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

Image transport for RGB message subscription.

Definition at line 231 of file keyframe_mapper.h.

ros::ServiceServer ccny_rgbd::KeyframeMapper::save_kf_service_ [private]

ROS service to save all keyframes to disk.

Definition at line 214 of file keyframe_mapper.h.

ros::ServiceServer ccny_rgbd::KeyframeMapper::save_octomap_service_ [private]

ROS service to save octomap to disk.

Definition at line 220 of file keyframe_mapper.h.

ros::ServiceServer ccny_rgbd::KeyframeMapper::save_pcd_map_service_ [private]

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

Definition at line 217 of file keyframe_mapper.h.

ros::ServiceServer ccny_rgbd::KeyframeMapper::solve_graph_service_ [private]

ROS service to optimize the graph.

Definition at line 205 of file keyframe_mapper.h.

ImageSubFilter ccny_rgbd::KeyframeMapper::sub_depth_ [private]

Depth message subscriber.

Definition at line 243 of file keyframe_mapper.h.

CameraInfoSubFilter ccny_rgbd::KeyframeMapper::sub_info_ [private]

Camera info message subscriber.

Definition at line 246 of file keyframe_mapper.h.

ImageSubFilter ccny_rgbd::KeyframeMapper::sub_rgb_ [private]

RGB message subscriber.

Definition at line 240 of file keyframe_mapper.h.

boost::shared_ptr<RGBDSynchronizer3> ccny_rgbd::KeyframeMapper::sync_ [private]

Callback syncronizer.

Definition at line 237 of file keyframe_mapper.h.

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

ROS transform listener.

Definition at line 228 of file keyframe_mapper.h.

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

Public Member Functions

Protected Member Functions

Protected Attributes

Private Member Functions

Static Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation