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

#include <navigator.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.
bool	loadKeyframesSrvCallback (Load::Request &request, Load::Response &response)
	ROS callback load keyframes from disk.
	Navigator (const ros::NodeHandle &nh, const ros::NodeHandle &nh_private)
	Constructor from ROS nodehandles.
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	~Navigator ()
	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::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
ros::Publisher	aruco_pub_
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	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< 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 navigator.h.

Constructor & Destructor Documentation

ccny_rgbd::Navigator::Navigator	(	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 navigator.cpp.

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

Default destructor.

Definition at line 91 of file navigator.cpp.

Member Function Documentation

void ccny_rgbd::Navigator::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 453 of file navigator.cpp.

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

ROS callback to manually request adding a keyframe.

Definition at line 954 of file navigator.cpp.

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

Builds an octomap octree from all keyframes, with color.

Parameters:

tree	reference to the octomap octree

Definition at line 1201 of file navigator.cpp.

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

Definition at line 1148 of file navigator.cpp.

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

Builds an pcd map from all keyframes.

Parameters:

map_cloud the point cloud to be built

Definition at line 1119 of file navigator.cpp.

void ccny_rgbd::Navigator::filter_aruco ( ) [private]

Definition at line 153 of file navigator.cpp.

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

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

Definition at line 964 of file navigator.cpp.

void ccny_rgbd::Navigator::initParams ( )

Initializes all the parameters from the ROS param server.

Definition at line 97 of file navigator.cpp.

bool ccny_rgbd::Navigator::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 783 of file navigator.cpp.

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

Definition at line 1323 of file navigator.cpp.

static octomap::point3d ccny_rgbd::Navigator::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 navigator.h.

static octomap::pose6d ccny_rgbd::Navigator::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 navigator.h.

bool ccny_rgbd::Navigator::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:

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 399 of file navigator.cpp.

void ccny_rgbd::Navigator::publishArucoPos ( ) [private]

Definition at line 612 of file navigator.cpp.

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

Publishes all the keyframe associations markers.

Definition at line 543 of file navigator.cpp.

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

Publishes the point cloud associated with a keyframe.

Parameters:

i	the keyframe index

Definition at line 522 of file navigator.cpp.

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

Publishes the pose marker associated with a keyframe.

Parameters:

i	the keyframe index

Definition at line 673 of file navigator.cpp.

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

Publishes all the keyframe pose markers.

Definition at line 604 of file navigator.cpp.

bool ccny_rgbd::Navigator::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 469 of file navigator.cpp.

bool ccny_rgbd::Navigator::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 489 of file navigator.cpp.

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

Publishes all the path message.

Definition at line 640 of file navigator.cpp.

static octomath::Quaternion ccny_rgbd::Navigator::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 navigator.h.

void ccny_rgbd::Navigator::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 285 of file navigator.cpp.

bool ccny_rgbd::Navigator::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 761 of file navigator.cpp.

bool ccny_rgbd::Navigator::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 1170 of file navigator.cpp.

bool ccny_rgbd::Navigator::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 841 of file navigator.cpp.

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

Definition at line 1263 of file navigator.cpp.

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

Definition at line 1294 of file navigator.cpp.

bool ccny_rgbd::Navigator::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 1106 of file navigator.cpp.

bool ccny_rgbd::Navigator::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 827 of file navigator.cpp.

bool ccny_rgbd::Navigator::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 978 of file navigator.cpp.

void ccny_rgbd::Navigator::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 1013 of file navigator.cpp.

Member Data Documentation

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

ROS service to add a manual keyframe.

Definition at line 236 of file navigator.h.

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

Definition at line 180 of file navigator.h.

PathMsg ccny_rgbd::Navigator::aruco_path [private]

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

Definition at line 278 of file navigator.h.

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

Definition at line 179 of file navigator.h.

ros::Publisher ccny_rgbd::Navigator::aruco_pub_ [private]

Definition at line 209 of file navigator.h.

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

keyframe associations that form the graph

Definition at line 275 of file navigator.h.

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

Image transport for depth message subscription.

Definition at line 244 of file navigator.h.

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

the fixed frame (usually "odom")

Definition at line 184 of file navigator.h.

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

ROS service to generate the graph correpondences.

Definition at line 212 of file navigator.h.

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

builds graph from the keyframes

Definition at line 272 of file navigator.h.

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

optimizes the graph for global alignement

Definition at line 273 of file navigator.h.

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

vector of RGBD Keyframes

Definition at line 191 of file navigator.h.

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

ROS publisher for the keyframe point clouds.

Definition at line 205 of file navigator.h.

double ccny_rgbd::Navigator::kf_angle_eps_ [private]

angular distance threshold between keyframes

Definition at line 262 of file navigator.h.

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

ROS publisher for the keyframe associations.

Definition at line 207 of file navigator.h.

double ccny_rgbd::Navigator::kf_dist_eps_ [private]

linear distance threshold between keyframes

Definition at line 261 of file navigator.h.

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

ROS service to load all keyframes from disk.

Definition at line 233 of file navigator.h.

bool ccny_rgbd::Navigator::manual_add_ [private]

flag indicating whetehr a manual add has been requested

Definition at line 268 of file navigator.h.

double ccny_rgbd::Navigator::max_map_z_ [private]

maximum z (in fixed frame) when exporting maps.

Definition at line 264 of file navigator.h.

double ccny_rgbd::Navigator::max_range_ [protected]

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

Definition at line 188 of file navigator.h.

double ccny_rgbd::Navigator::max_stdev_ [protected]

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

Definition at line 189 of file navigator.h.

double ccny_rgbd::Navigator::min_map_z_ [private]

min z (in fixed frame) when exporting maps.

Definition at line 265 of file navigator.h.

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

public nodehandle

Definition at line 181 of file navigator.h.

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

private nodepcdhandle

Definition at line 182 of file navigator.h.

double ccny_rgbd::Navigator::octomap_res_ [private]

tree resolution for octomap (in meters)

Definition at line 260 of file navigator.h.

bool ccny_rgbd::Navigator::octomap_with_color_ [private]

whetehr to save Octomaps with color info

Definition at line 263 of file navigator.h.

PathMsg ccny_rgbd::Navigator::path_msg_ [private]

Definition at line 277 of file navigator.h.

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

ROS publisher for the keyframe path.

Definition at line 208 of file navigator.h.

double ccny_rgbd::Navigator::pcd_map_res_ [private]

downsampling resolution of pcd map (in meters)

Definition at line 259 of file navigator.h.

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

ROS publisher for the keyframe poses.

Definition at line 206 of file navigator.h.

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

ROS service to publish a single keyframe.

Definition at line 218 of file navigator.h.

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

ROS service to publish a subset of keyframes.

Definition at line 221 of file navigator.h.

int ccny_rgbd::Navigator::queue_size_ [protected]

Subscription queue size.

Definition at line 186 of file navigator.h.

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

Image transport for RGB message subscription.

Definition at line 241 of file navigator.h.

int ccny_rgbd::Navigator::rgbd_frame_index_ [private]

Definition at line 270 of file navigator.h.

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

ROS service to save all keyframes to disk.

Definition at line 224 of file navigator.h.

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

ROS service to save octomap to disk.

Definition at line 230 of file navigator.h.

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

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

Definition at line 227 of file navigator.h.

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

ROS service to optimize the graph.

Definition at line 215 of file navigator.h.

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

Definition at line 274 of file navigator.h.

ImageSubFilter ccny_rgbd::Navigator::sub_depth_ [private]

Depth message subscriber.

Definition at line 253 of file navigator.h.

CameraInfoSubFilter ccny_rgbd::Navigator::sub_info_ [private]

Camera info message subscriber.

Definition at line 256 of file navigator.h.

ImageSubFilter ccny_rgbd::Navigator::sub_rgb_ [private]

RGB message subscriber.

Definition at line 250 of file navigator.h.

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

Callback syncronizer.

Definition at line 247 of file navigator.h.

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

ROS transform listener.

Definition at line 238 of file navigator.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