localization_common Namespace Reference

Classes
class	Averager
class	CombinedNavState
class	CombinedNavStateCovariances
struct	CombinedNavStateNoise
struct	DefaultInterpolationParams
class	DistanceScaledPoseCovarianceInterpolater
struct	DistanceScaledPoseCovarianceInterpolaterParams
struct	FeaturePoint3d
struct	FeaturePoint3dNoise
struct	ImageCorrespondences
class	MarginalsPoseCovarianceInterpolater
class	PoseCovarianceInterpolater
struct	PoseWithCovariance
struct	PoseWithCovarianceInterpolaterParams
class	RateTimer
class	RosTimer
class	Sampler
class	StatsLogger
class	Timer
class	TimestampedInterpolater
class	TimestampedSet
struct	TimestampedValue

Typedefs
using	FeatureId = int
using	FeaturePoint3ds = std::vector< FeaturePoint3d >
using	PoseInterpolater = TimestampedInterpolater< Eigen::Isometry3d >
using	PoseCovariance = Eigen::Matrix< double, 6, 6 >
using	PoseWithCovarianceInterpolater = TimestampedInterpolater< PoseWithCovariance, PoseWithCovarianceInterpolaterParams >
using	Time = double

Enumerations
enum	Confidence { kGood, kPoor, kLost }

Functions
double	RandomDouble ()
double	RandomPositiveDouble ()
double	RandomDouble (const double min, const double max)
double	RandomGaussianDouble (const double mean, const double stddev)
bool	RandomBool ()
Time	RandomTime ()
double	Noise (const double stddev)
template<int Dim>
Eigen::Matrix< double, Dim, 1 >	RandomVector ()
Eigen::Vector3d	RandomVector3d ()
Eigen::Vector3d	RandomPoint3d ()
Eigen::Vector3d	RandomVelocity ()
Eigen::Vector2d	RandomVector2d ()
Eigen::Vector2d	RandomPoint2d ()
template<int D>
Eigen::Matrix< double, D, D >	RandomCovariance ()
gtsam::Pose3	RandomPose ()
PoseCovariance	RandomPoseCovariance ()
PoseWithCovariance	RandomPoseWithCovariance ()
Eigen::Isometry3d	RandomIsometry3d ()
Eigen::Affine3d	RandomAffine3d ()
CombinedNavState	RandomCombinedNavState ()
Eigen::Matrix3d	RandomIntrinsics ()
Eigen::Isometry3d	RandomIdentityCenteredIsometry3d (const double translation_stddev, const double rotation_stddev)
Eigen::Isometry3d	AddNoiseToIsometry3d (const Eigen::Isometry3d &pose, const double translation_stddev, const double rotation_stddev)
gtsam::Pose3	AddNoiseToPose (const gtsam::Pose3 &pose, const double translation_stddev, const double rotation_stddev)
template<int N>
Eigen::Matrix< double, N, 1 >	AddNoiseToVector (const Eigen::Matrix< double, N, 1 > &vector, const double noise_stddev)
Eigen::Isometry3d	RandomFrontFacingPose (const double rho_min, const double rho_max, const double phi_min, const double phi_max, const double z_rho_scale, const double yaw_min, const double yaw_max, const double pitch_min, const double pitch_max, const double roll_min, const double roll_max)
Eigen::Isometry3d	RandomFrontFacingPose ()
std::vector< Eigen::Vector3d >	RandomFrontFacingPoints (const int num_points)
Eigen::Vector3d	RandomFrontFacingPoint ()
Eigen::Vector3d	RandomFrontFacingPoint (const double rho_min, const double rho_max, const double phi_min, const double phi_max, const double z_rho_scale)
bool	MatrixEquality (const Eigen::MatrixXd &lhs, const Eigen::MatrixXd &rhs, const double tolerance)
Time	GetTime (const int seconds, const int nanoseconds)
gtsam::Pose3	LoadTransform (config_reader::ConfigReader &config, const std::string &transform_config_name, const std::string &prefix="")
gtsam::Vector3	LoadVector3 (config_reader::ConfigReader &config, const std::string &config_name, const std::string &prefix="")
Eigen::Matrix3d	LoadCameraIntrinsicsMatrix (config_reader::ConfigReader &config, const std::string &intrinsics_config_name, const std::string &prefix="")
gtsam::Cal3_S2	LoadCameraIntrinsics (config_reader::ConfigReader &config, const std::string &intrinsics_config_name, const std::string &prefix="")
gtsam::Pose3	GtPose (const Eigen::Isometry3d &eigen_pose)
Eigen::Isometry3d	EigenPose (const CombinedNavState &combined_nav_state)
Eigen::Isometry3d	EigenPose (const gtsam::Pose3 &pose)
gtsam::Pose3	PoseFromMsgWithExtrinsics (const geometry_msgs::Pose &pose, const gtsam::Pose3 &sensor_T_body)
void	LoadGraphLocalizerConfig (config_reader::ConfigReader &config, const std::string &path_prefix="")
void	LoadGraphVIOConfig (config_reader::ConfigReader &config, const std::string &path_prefix="")
void	SetEnvironmentConfigs (const std::string &world="iss", const std::string &robot_config_file="bumble.config")
ros::Time	RosTimeFromHeader (const std_msgs::Header &header)
Time	TimeFromHeader (const std_msgs::Header &header)
Time	TimeFromRosTime (const ros::Time &time)
void	TimeToHeader (const Time timestamp, std_msgs::Header &header)
void	TimeToMsg (const Time timestamp, ros::Time &time_msg)
gtsam::Pose3	PoseFromMsg (const geometry_msgs::PoseStamped &msg)
gtsam::Pose3	PoseFromMsg (const geometry_msgs::Pose &msg_pose)
void	PoseToMsg (const gtsam::Pose3 &pose, geometry_msgs::Pose &msg_pose)
geometry_msgs::TransformStamped	PoseToTF (const Eigen::Isometry3d &pose, const std::string &parent_frame, const std::string &child_frame, const Time timestamp, const std::string &platform_name="")
geometry_msgs::TransformStamped	PoseToTF (const gtsam::Pose3 &pose, const std::string &parent_frame, const std::string &child_frame, const Time timestamp, const std::string &platform_name="")
CombinedNavState	CombinedNavStateFromMsg (const ff_msgs::CombinedNavState &msg)
CombinedNavStateCovariances	CombinedNavStateCovariancesFromMsg (const ff_msgs::CombinedNavState &msg)
TimestampedSet< PoseCovariance >	CorrelationCovariancesFromMsg (const ff_msgs::CombinedNavState &msg)
gtsam::Vector3	RemoveGravityFromAccelerometerMeasurement (const gtsam::Vector3 &global_F_gravity, const gtsam::Pose3 &body_T_imu, const gtsam::Pose3 &global_T_body, const gtsam::Vector3 &uncorrected_accelerometer_measurement)
ff_msgs::CombinedNavState	CombinedNavStateToMsg (const CombinedNavState &combined_nav_state, const PoseCovariance &pose_covariance, const Eigen::Matrix3d &velocity_covariance, const Eigen::Matrix< double, 6, 6 > &imu_bias_covariance, const TimestampedSet< PoseCovariance > &correlation_covariances={})
template<class LocMsgType >
void	CombinedNavStateToLocMsg (const CombinedNavState &combined_nav_state, LocMsgType &loc_msg)
template<class LocMsgType >
void	CombinedNavStateCovariancesToLocMsg (const CombinedNavStateCovariances &covariances, LocMsgType &loc_msg)
template<typename MatrixType >
double	LogDeterminant (const MatrixType &matrix)
double	PoseCovarianceSane (const Eigen::Matrix< double, 6, 6 > &pose_covariance, const double position_covariance_threshold=0, const double orientation_covariance_threshold=0, const bool check_position_covariance=true, const bool check_orientation_covariance=true)
template<typename RotationType >
Eigen::Isometry3d	Isometry3d (const Eigen::Vector3d &translation, const RotationType &rotation)
double	Deg2Rad (const double degrees)
double	Rad2Deg (const double radians)
Eigen::Vector3d	CylindricalToCartesian (const Eigen::Vector3d &cylindrical_coordinates)
Eigen::Matrix3d	RotationFromEulerAngles (const double yaw, const double pitch, const double roll)
PoseCovariance	RelativePoseCovariance (const PoseWithCovariance &a, const PoseWithCovariance &b, const boost::optional< PoseCovariance > covariance_a_b=boost::none)
Eigen::Isometry3d	FrameChangeRelativePose (const Eigen::Isometry3d &a_F_relative_pose, const Eigen::Isometry3d &b_T_a)
Eigen::Matrix< double, 6, 6 >	FrameChangeRelativeCovariance (const Eigen::Matrix< double, 6, 6 > &a_F_relative_pose_covariance, const Eigen::Isometry3d &b_T_a)
PoseWithCovariance	FrameChangeRelativePoseWithCovariance (const PoseWithCovariance &a_F_relative_pose_with_covariance, const Eigen::Isometry3d &b_T_a)
PoseWithCovariance	InvertPoseWithCovariance (const PoseWithCovariance &pose_with_covariance)
template<int CostDim, int StateDim>
boost::optional< Eigen::Matrix< double, StateDim, StateDim > >	LeastSquaresCovariance (const std::vector< Eigen::Matrix< double, CostDim, StateDim >> &cost_jacobians)
template<typename T >
std::vector< T >	Transform (const std::vector< T > &a_T_elements, const Eigen::Isometry3d &b_T_a)
template<typename T >
std::vector< T >	Rotate (const std::vector< T > &a_F_a_T_elements, const Eigen::Matrix3d &b_R_a)
std::pair< std::vector< Eigen::Vector3d >, std::vector< Eigen::Vector3d > >	TransformPointsWithNormals (const std::vector< Eigen::Vector3d > &points, const std::vector< Eigen::Vector3d > &normals, const Eigen::Isometry3d &b_T_a)
Eigen::Isometry3d	Interpolate (const Eigen::Isometry3d &lower_bound_pose, const Eigen::Isometry3d &upper_bound_pose, const double alpha)
PoseWithCovariance	Interpolate (const PoseWithCovariance &lower_bound_pose, const PoseWithCovariance &upper_bound_pose, const double alpha)
gtsam::noiseModel::Robust::shared_ptr	Robust (const gtsam::SharedNoiseModel &noise, const double huber_k)
template<typename FactorType >
void	DeleteFactors (gtsam::NonlinearFactorGraph &graph)
template<typename FactorType >
int	NumFactors (const gtsam::NonlinearFactorGraph &graph)
template<typename FactorType >
std::vector< const FactorType * >	Factors (const gtsam::NonlinearFactorGraph &graph)

Typedef Documentation

FeatureId

using localization_common::FeatureId = typedef int

FeaturePoint3ds

using localization_common::FeaturePoint3ds = typedef std::vector<FeaturePoint3d>

PoseCovariance

using localization_common::PoseCovariance = typedef Eigen::Matrix<double, 6, 6>

PoseInterpolater

using localization_common::PoseInterpolater = typedef TimestampedInterpolater<Eigen::Isometry3d>

PoseWithCovarianceInterpolater

using localization_common::PoseWithCovarianceInterpolater = typedef TimestampedInterpolater<PoseWithCovariance, PoseWithCovarianceInterpolaterParams>

Time

using localization_common::Time = typedef double

Enumeration Type Documentation

Confidence

enum localization_common::Confidence

Enumerator
kGood
kPoor
kLost

Function Documentation

AddNoiseToIsometry3d()

Eigen::Isometry3d localization_common::AddNoiseToIsometry3d	(	const Eigen::Isometry3d &	pose,
		const double	translation_stddev,
		const double	rotation_stddev
	)

AddNoiseToPose()

gtsam::Pose3 localization_common::AddNoiseToPose	(	const gtsam::Pose3 &	pose,
		const double	translation_stddev,
		const double	rotation_stddev
	)

AddNoiseToVector()

template<int N>

Eigen::Matrix< double, N, 1 > localization_common::AddNoiseToVector	(	const Eigen::Matrix< double, N, 1 > &	vector,
		const double	noise_stddev
	)

CombinedNavStateCovariancesFromMsg()

CombinedNavStateCovariances localization_common::CombinedNavStateCovariancesFromMsg

(

const ff_msgs::CombinedNavState &

msg

)

CombinedNavStateCovariancesToLocMsg()

template<class LocMsgType >

void localization_common::CombinedNavStateCovariancesToLocMsg	(	const CombinedNavStateCovariances &	covariances,
		LocMsgType &	loc_msg
	)

CombinedNavStateFromMsg()

CombinedNavState localization_common::CombinedNavStateFromMsg

(

const ff_msgs::CombinedNavState &

msg

)

CombinedNavStateToLocMsg()

template<class LocMsgType >

void localization_common::CombinedNavStateToLocMsg	(	const CombinedNavState &	combined_nav_state,
		LocMsgType &	loc_msg
	)

CombinedNavStateToMsg()

ff_msgs::CombinedNavState localization_common::CombinedNavStateToMsg	(	const CombinedNavState &	combined_nav_state,
		const PoseCovariance &	pose_covariance,
		const Eigen::Matrix3d &	velocity_covariance,
		const Eigen::Matrix< double, 6, 6 > &	imu_bias_covariance,
		const TimestampedSet< PoseCovariance > &	correlation_covariances = {}
	)

CorrelationCovariancesFromMsg()

TimestampedSet< PoseCovariance > localization_common::CorrelationCovariancesFromMsg

(

const ff_msgs::CombinedNavState &

msg

)

CylindricalToCartesian()

Eigen::Vector3d localization_common::CylindricalToCartesian

(

const Eigen::Vector3d &

cylindrical_coordinates

)

Deg2Rad()

double localization_common::Deg2Rad

(

const double

degrees

)

DeleteFactors()

template<typename FactorType >

void localization_common::DeleteFactors

(

gtsam::NonlinearFactorGraph &

graph

)

EigenPose() [1/2]

Eigen::Isometry3d localization_common::EigenPose

(

const CombinedNavState &

combined_nav_state

)

EigenPose() [2/2]

Eigen::Isometry3d localization_common::EigenPose

(

const gtsam::Pose3 &

pose

)

Factors()

template<typename FactorType >

std::vector< const FactorType * > localization_common::Factors

(

const gtsam::NonlinearFactorGraph &

graph

)

FrameChangeRelativeCovariance()

Eigen::Matrix< double, 6, 6 > localization_common::FrameChangeRelativeCovariance	(	const Eigen::Matrix< double, 6, 6 > &	a_F_relative_pose_covariance,
		const Eigen::Isometry3d &	b_T_a
	)

FrameChangeRelativePose()

Eigen::Isometry3d localization_common::FrameChangeRelativePose	(	const Eigen::Isometry3d &	a_F_relative_pose,
		const Eigen::Isometry3d &	b_T_a
	)

FrameChangeRelativePoseWithCovariance()

PoseWithCovariance localization_common::FrameChangeRelativePoseWithCovariance	(	const PoseWithCovariance &	a_F_relative_pose_with_covariance,
		const Eigen::Isometry3d &	b_T_a
	)

GetTime()

Time localization_common::GetTime	(	const int	seconds,
		const int	nanoseconds
	)

GtPose()

gtsam::Pose3 localization_common::GtPose

(

const Eigen::Isometry3d &

eigen_pose

)

Interpolate() [1/2]

Eigen::Isometry3d localization_common::Interpolate	(	const Eigen::Isometry3d &	lower_bound_pose,
		const Eigen::Isometry3d &	upper_bound_pose,
		const double	alpha
	)

Interpolate() [2/2]

PoseWithCovariance localization_common::Interpolate	(	const PoseWithCovariance &	lower_bound_pose,
		const PoseWithCovariance &	upper_bound_pose,
		const double	alpha
	)

InvertPoseWithCovariance()

PoseWithCovariance localization_common::InvertPoseWithCovariance

(

const PoseWithCovariance &

pose_with_covariance

)

Isometry3d()

template<typename RotationType >

Eigen::Isometry3d localization_common::Isometry3d	(	const Eigen::Vector3d &	translation,
		const RotationType &	rotation
	)

LeastSquaresCovariance()

template<int CostDim, int StateDim>

boost::optional< Eigen::Matrix< double, StateDim, StateDim > > localization_common::LeastSquaresCovariance

(

const std::vector< Eigen::Matrix< double, CostDim, StateDim >> &

cost_jacobians

)

LoadCameraIntrinsics()

gtsam::Cal3_S2 localization_common::LoadCameraIntrinsics	(	config_reader::ConfigReader &	config,
		const std::string &	intrinsics_config_name,
		const std::string &	prefix = ""
	)

LoadCameraIntrinsicsMatrix()

Eigen::Matrix3d localization_common::LoadCameraIntrinsicsMatrix	(	config_reader::ConfigReader &	config,
		const std::string &	intrinsics_config_name,
		const std::string &	prefix = ""
	)

LoadGraphLocalizerConfig()

void localization_common::LoadGraphLocalizerConfig	(	config_reader::ConfigReader &	config,
		const std::string &	path_prefix = ""
	)

LoadGraphVIOConfig()

void localization_common::LoadGraphVIOConfig	(	config_reader::ConfigReader &	config,
		const std::string &	path_prefix = ""
	)

LoadTransform()

gtsam::Pose3 localization_common::LoadTransform	(	config_reader::ConfigReader &	config,
		const std::string &	transform_config_name,
		const std::string &	prefix = ""
	)

LoadVector3()

gtsam::Vector3 localization_common::LoadVector3	(	config_reader::ConfigReader &	config,
		const std::string &	config_name,
		const std::string &	prefix = ""
	)

LogDeterminant()

template<typename MatrixType >

double localization_common::LogDeterminant

(

const MatrixType &

matrix

)

MatrixEquality()

bool localization_common::MatrixEquality	(	const Eigen::MatrixXd &	lhs,
		const Eigen::MatrixXd &	rhs,
		const double	tolerance
	)

Noise()

double localization_common::Noise

(

const double

stddev

)

NumFactors()

template<typename FactorType >

int localization_common::NumFactors

(

const gtsam::NonlinearFactorGraph &

graph

)

PoseCovarianceSane()

double localization_common::PoseCovarianceSane	(	const Eigen::Matrix< double, 6, 6 > &	pose_covariance,
		const double	position_covariance_threshold = 0,
		const double	orientation_covariance_threshold = 0,
		const bool	check_position_covariance = true,
		const bool	check_orientation_covariance = true
	)

PoseFromMsg() [1/2]

gtsam::Pose3 localization_common::PoseFromMsg

(

const geometry_msgs::Pose &

msg_pose

)

PoseFromMsg() [2/2]

gtsam::Pose3 localization_common::PoseFromMsg

(

const geometry_msgs::PoseStamped &

msg

)

PoseFromMsgWithExtrinsics()

gtsam::Pose3 localization_common::PoseFromMsgWithExtrinsics	(	const geometry_msgs::Pose &	pose,
		const gtsam::Pose3 &	sensor_T_body
	)

PoseToMsg()

void localization_common::PoseToMsg	(	const gtsam::Pose3 &	pose,
		geometry_msgs::Pose &	msg_pose
	)

PoseToTF() [1/2]

geometry_msgs::TransformStamped localization_common::PoseToTF	(	const Eigen::Isometry3d &	pose,
		const std::string &	parent_frame,
		const std::string &	child_frame,
		const Time	timestamp,
		const std::string &	platform_name = ""
	)

PoseToTF() [2/2]

geometry_msgs::TransformStamped localization_common::PoseToTF	(	const gtsam::Pose3 &	pose,
		const std::string &	parent_frame,
		const std::string &	child_frame,
		const Time	timestamp,
		const std::string &	platform_name = ""
	)

Rad2Deg()

double localization_common::Rad2Deg

(

const double

radians

)

RandomAffine3d()

Eigen::Affine3d localization_common::RandomAffine3d

(

)

RandomBool()

bool localization_common::RandomBool

(

)

RandomCombinedNavState()

CombinedNavState localization_common::RandomCombinedNavState

(

)

RandomCovariance()

template<int D>

Eigen::Matrix< double, D, D > localization_common::RandomCovariance

(

)

RandomDouble() [1/2]

double localization_common::RandomDouble

(

)

RandomDouble() [2/2]

double localization_common::RandomDouble	(	const double	min,
		const double	max
	)

RandomFrontFacingPoint() [1/2]

Eigen::Vector3d localization_common::RandomFrontFacingPoint

(

)

RandomFrontFacingPoint() [2/2]

Eigen::Vector3d localization_common::RandomFrontFacingPoint	(	const double	rho_min,
		const double	rho_max,
		const double	phi_min,
		const double	phi_max,
		const double	z_rho_scale
	)

RandomFrontFacingPoints()

std::vector< Eigen::Vector3d > localization_common::RandomFrontFacingPoints

(

const int

num_points

)

RandomFrontFacingPose() [1/2]

Eigen::Isometry3d localization_common::RandomFrontFacingPose

(

)

RandomFrontFacingPose() [2/2]

Eigen::Isometry3d localization_common::RandomFrontFacingPose	(	const double	rho_min,
		const double	rho_max,
		const double	phi_min,
		const double	phi_max,
		const double	z_rho_scale,
		const double	yaw_min,
		const double	yaw_max,
		const double	pitch_min,
		const double	pitch_max,
		const double	roll_min,
		const double	roll_max
	)

RandomGaussianDouble()

double localization_common::RandomGaussianDouble	(	const double	mean,
		const double	stddev
	)

RandomIdentityCenteredIsometry3d()

Eigen::Isometry3d localization_common::RandomIdentityCenteredIsometry3d	(	const double	translation_stddev,
		const double	rotation_stddev
	)

RandomIntrinsics()

Eigen::Matrix3d localization_common::RandomIntrinsics

(

)

RandomIsometry3d()

Eigen::Isometry3d localization_common::RandomIsometry3d

(

)

RandomPoint2d()

Eigen::Vector2d localization_common::RandomPoint2d

(

)

RandomPoint3d()

Eigen::Vector3d localization_common::RandomPoint3d

(

)

RandomPose()

gtsam::Pose3 localization_common::RandomPose

(

)

RandomPoseCovariance()

PoseCovariance localization_common::RandomPoseCovariance

(

)

RandomPoseWithCovariance()

PoseWithCovariance localization_common::RandomPoseWithCovariance

(

)

RandomPositiveDouble()

double localization_common::RandomPositiveDouble

(

)

RandomTime()

Time localization_common::RandomTime

(

)

RandomVector()

template<int Dim>

Eigen::Matrix< double, Dim, 1 > localization_common::RandomVector

(

)

RandomVector2d()

Eigen::Vector2d localization_common::RandomVector2d

(

)

RandomVector3d()

Eigen::Vector3d localization_common::RandomVector3d

(

)

RandomVelocity()

Eigen::Vector3d localization_common::RandomVelocity

(

)

RelativePoseCovariance()

PoseCovariance localization_common::RelativePoseCovariance	(	const PoseWithCovariance &	a,
		const PoseWithCovariance &	b,
		const boost::optional< PoseCovariance >	covariance_a_b = boost::none
	)

RemoveGravityFromAccelerometerMeasurement()

gtsam::Vector3 localization_common::RemoveGravityFromAccelerometerMeasurement	(	const gtsam::Vector3 &	global_F_gravity,
		const gtsam::Pose3 &	body_T_imu,
		const gtsam::Pose3 &	global_T_body,
		const gtsam::Vector3 &	uncorrected_accelerometer_measurement
	)

Robust()

gtsam::noiseModel::Robust::shared_ptr localization_common::Robust	(	const gtsam::SharedNoiseModel &	noise,
		const double	huber_k
	)

RosTimeFromHeader()

ros::Time localization_common::RosTimeFromHeader

(

const std_msgs::Header &

header

)

Rotate()

template<typename T >

std::vector< T > localization_common::Rotate	(	const std::vector< T > &	a_F_a_T_elements,
		const Eigen::Matrix3d &	b_R_a
	)

RotationFromEulerAngles()

Eigen::Matrix3d localization_common::RotationFromEulerAngles	(	const double	yaw,
		const double	pitch,
		const double	roll
	)

SetEnvironmentConfigs()

void localization_common::SetEnvironmentConfigs	(	const std::string &	world = "iss",
		const std::string &	robot_config_file = "bumble.config"
	)

TimeFromHeader()

Time localization_common::TimeFromHeader

(

const std_msgs::Header &

header

)

TimeFromRosTime()

Time localization_common::TimeFromRosTime

(

const ros::Time &

time

)

TimeToHeader()

void localization_common::TimeToHeader	(	const Time	timestamp,
		std_msgs::Header &	header
	)

TimeToMsg()

void localization_common::TimeToMsg	(	const Time	timestamp,
		ros::Time &	time_msg
	)

Transform()

template<typename T >

std::vector< T > localization_common::Transform	(	const std::vector< T > &	a_T_elements,
		const Eigen::Isometry3d &	b_T_a
	)

TransformPointsWithNormals()

std::pair< std::vector< Eigen::Vector3d >, std::vector< Eigen::Vector3d > > localization_common::TransformPointsWithNormals	(	const std::vector< Eigen::Vector3d > &	points,
		const std::vector< Eigen::Vector3d > &	normals,
		const Eigen::Isometry3d &	b_T_a
	)