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.
void	initParams ()
	Initializes all the parameters from the ROS param server.
	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")
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	buildOctomap (octomap::OcTree &tree)
	Builds an octomap octree from all keyframes.
void	buildPcdMap (PointCloudT &map_cloud)
	Builds an pcd map from all keyframes.
bool	loadPath (const std::string &filepath)
bool	processFrame (const rgbdtools::RGBDFrame &frame, const AffineTransform &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	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.
rgbdtools::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.
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	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.
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 < 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 69 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 89 of file keyframe_mapper.cpp.

Member Function Documentation

void ccny_rgbd::KeyframeMapper::addKeyframe	(	const rgbdtools::RGBDFrame &	frame,
		const AffineTransform &	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 256 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 613 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 884 of file keyframe_mapper.cpp.

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

Definition at line 806 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 847 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 777 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 622 of file keyframe_mapper.cpp.

void ccny_rgbd::KeyframeMapper::initParams ( )

Initializes all the parameters from the ROS param server.

Definition at line 94 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 503 of file keyframe_mapper.cpp.

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

Definition at line 1007 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 360 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 348 of file keyframe_mapper.h.

bool ccny_rgbd::KeyframeMapper::processFrame	(	const rgbdtools::RGBDFrame &	frame,
		const AffineTransform &	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 202 of file keyframe_mapper.cpp.

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

Publishes all the keyframe associations markers.

Definition at line 342 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 325 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 411 of file keyframe_mapper.cpp.

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

Publishes all the keyframe pose markers.

Definition at line 403 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 272 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 292 of file keyframe_mapper.cpp.

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

Publishes all the path message.

Definition at line 941 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 370 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 148 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 481 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 827 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 599 of file keyframe_mapper.cpp.

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

Definition at line 947 of file keyframe_mapper.cpp.

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

Definition at line 978 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 764 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 585 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 636 of file keyframe_mapper.cpp.

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

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 671 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 234 of file keyframe_mapper.h.

rgbdtools::KeyframeAssociationVector ccny_rgbd::KeyframeMapper::associations_ [private]

keyframe associations that form the graph

Definition at line 272 of file keyframe_mapper.h.

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

Image transport for depth message subscription.

Definition at line 242 of file keyframe_mapper.h.

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

the fixed frame (usually "odom")

Definition at line 183 of file keyframe_mapper.h.

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

ROS service to generate the graph correpondences.

Definition at line 210 of file keyframe_mapper.h.

rgbdtools::KeyframeGraphDetector ccny_rgbd::KeyframeMapper::graph_detector_ [private]

builds graph from the keyframes

Definition at line 269 of file keyframe_mapper.h.

rgbdtools::KeyframeGraphSolverG2O ccny_rgbd::KeyframeMapper::graph_solver_ [private]

optimizes the graph for global alignement

Definition at line 270 of file keyframe_mapper.h.

rgbdtools::KeyframeVector ccny_rgbd::KeyframeMapper::keyframes_ [protected]

vector of RGBD Keyframes

Definition at line 190 of file keyframe_mapper.h.

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

ROS publisher for the keyframe point clouds.

Definition at line 204 of file keyframe_mapper.h.

double ccny_rgbd::KeyframeMapper::kf_angle_eps_ [private]

angular distance threshold between keyframes

Definition at line 260 of file keyframe_mapper.h.

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

ROS publisher for the keyframe associations.

Definition at line 206 of file keyframe_mapper.h.

double ccny_rgbd::KeyframeMapper::kf_dist_eps_ [private]

linear distance threshold between keyframes

Definition at line 259 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 231 of file keyframe_mapper.h.

bool ccny_rgbd::KeyframeMapper::manual_add_ [private]

flag indicating whetehr a manual add has been requested

Definition at line 265 of file keyframe_mapper.h.

double ccny_rgbd::KeyframeMapper::max_map_z_ [private]

maximum z (in fixed frame) when exporting maps.

Definition at line 262 of file keyframe_mapper.h.

double ccny_rgbd::KeyframeMapper::max_range_ [protected]

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

Definition at line 187 of file keyframe_mapper.h.

double ccny_rgbd::KeyframeMapper::max_stdev_ [protected]

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

Definition at line 188 of file keyframe_mapper.h.

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

public nodehandle

Definition at line 180 of file keyframe_mapper.h.

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

private nodepcdhandle

Definition at line 181 of file keyframe_mapper.h.

double ccny_rgbd::KeyframeMapper::octomap_res_ [private]

tree resolution for octomap (in meters)

Definition at line 258 of file keyframe_mapper.h.

bool ccny_rgbd::KeyframeMapper::octomap_with_color_ [private]

whetehr to save Octomaps with color info

Definition at line 261 of file keyframe_mapper.h.

PathMsg ccny_rgbd::KeyframeMapper::path_msg_ [private]

Definition at line 274 of file keyframe_mapper.h.

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

ROS publisher for the keyframe path.

Definition at line 207 of file keyframe_mapper.h.

double ccny_rgbd::KeyframeMapper::pcd_map_res_ [private]

downsampling resolution of pcd map (in meters)

Definition at line 257 of file keyframe_mapper.h.

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

ROS publisher for the keyframe poses.

Definition at line 205 of file keyframe_mapper.h.

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

ROS service to publish a single keyframe.

Definition at line 216 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 219 of file keyframe_mapper.h.

int ccny_rgbd::KeyframeMapper::queue_size_ [protected]

Subscription queue size.

Definition at line 185 of file keyframe_mapper.h.

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

Image transport for RGB message subscription.

Definition at line 239 of file keyframe_mapper.h.

int ccny_rgbd::KeyframeMapper::rgbd_frame_index_ [private]

Definition at line 267 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 222 of file keyframe_mapper.h.

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

ROS service to save octomap to disk.

Definition at line 228 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 225 of file keyframe_mapper.h.

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

ROS service to optimize the graph.

Definition at line 213 of file keyframe_mapper.h.

rgbdtools::KeyframeGraphSolverISAM ccny_rgbd::KeyframeMapper::solver_ [private]

Definition at line 271 of file keyframe_mapper.h.

ImageSubFilter ccny_rgbd::KeyframeMapper::sub_depth_ [private]

Depth message subscriber.

Definition at line 251 of file keyframe_mapper.h.

CameraInfoSubFilter ccny_rgbd::KeyframeMapper::sub_info_ [private]

Camera info message subscriber.

Definition at line 254 of file keyframe_mapper.h.

ImageSubFilter ccny_rgbd::KeyframeMapper::sub_rgb_ [private]

RGB message subscriber.

Definition at line 248 of file keyframe_mapper.h.

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

Callback syncronizer.

Definition at line 245 of file keyframe_mapper.h.

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

ROS transform listener.

Definition at line 236 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