mrpt::srba::sensor_model< landmarks::Euclidean3D, observations::RangeBearing_3D > Struct Template Reference

Detailed Description

template<>
struct mrpt::srba::sensor_model< landmarks::Euclidean3D, observations::RangeBearing_3D >

Sensor model: 3D landmarks in Euclidean coordinates + 3D Range-Bearing observations.

Definition at line 517 of file sensors.h.

#include <mrpt/srba/models/sensors.h>

Public Types
typedef observations::RangeBearing_3D	OBS_T
typedef landmarks::Euclidean3D	LANDMARK_T
typedef Eigen::Matrix< double, OBS_DIMS, LM_DIMS >	TJacobian_dh_dx
	A Jacobian of the correct size for each dh_dx. More...
typedef landmark_traits< LANDMARK_T >::array_landmark_t	array_landmark_t
	a 2D or 3D point More...
typedef OBS_T::TObservationParams	TObservationParams

Static Public Member Functions
template<class POSE_T >
static void	observe_error (observation_traits< OBS_T >::array_obs_t &out_obs_err, const observation_traits< OBS_T >::array_obs_t &z_obs, const POSE_T &base_pose_wrt_observer, const landmark_traits< LANDMARK_T >::array_landmark_t &lm_pos, const OBS_T::TObservationParams &params)
	Executes the (negative) observation-error model: "-( h(lm_pos,pose) - z_obs)". More...
static bool	eval_jacob_dh_dx (TJacobian_dh_dx &dh_dx, const array_landmark_t &xji_l, const TObservationParams &sensor_params)
	Evaluates the partial Jacobian dh_dx: More...
static void	inverse_sensor_model (landmark_traits< LANDMARK_T >::array_landmark_t &out_lm_pos, const observation_traits< OBS_T >::obs_data_t &obs, const OBS_T::TObservationParams &params)
	Inverse observation model for first-seen landmarks. More...

Static Public Attributes
static const size_t	OBS_DIMS = OBS_T::OBS_DIMS
static const size_t	LM_DIMS = LANDMARK_T::LM_DIMS

Member Typedef Documentation

typedef landmark_traits<LANDMARK_T>::array_landmark_t mrpt::srba::sensor_model< landmarks::Euclidean3D, observations::RangeBearing_3D >::array_landmark_t

a 2D or 3D point

Definition at line 531 of file sensors.h.

typedef landmarks::Euclidean3D mrpt::srba::sensor_model< landmarks::Euclidean3D, observations::RangeBearing_3D >::LANDMARK_T

Definition at line 524 of file sensors.h.

typedef observations::RangeBearing_3D mrpt::srba::sensor_model< landmarks::Euclidean3D, observations::RangeBearing_3D >::OBS_T

Definition at line 523 of file sensors.h.

typedef Eigen::Matrix<double,OBS_DIMS,LM_DIMS> mrpt::srba::sensor_model< landmarks::Euclidean3D, observations::RangeBearing_3D >::TJacobian_dh_dx

A Jacobian of the correct size for each dh_dx.

Definition at line 530 of file sensors.h.

typedef OBS_T::TObservationParams mrpt::srba::sensor_model< landmarks::Euclidean3D, observations::RangeBearing_3D >::TObservationParams

Definition at line 532 of file sensors.h.

Member Function Documentation

static bool mrpt::srba::sensor_model< landmarks::Euclidean3D, observations::RangeBearing_3D >::eval_jacob_dh_dx ( TJacobian_dh_dx &  dh_dx, const array_landmark_t &  xji_l, const TObservationParams &  sensor_params  )

inlinestatic

Evaluates the partial Jacobian dh_dx:

           d h(x')
dh_dx = -------------
            d x' 

With:

• x' = x^{j,i}_l The relative location of the observed landmark wrt to the robot/camera at the instant of observation. (See notation on papers)
• h(x): Observation model: h(): landmark location –> observation

Parameters

[out]	dh_dx	The output matrix Jacobian. Values at input are undefined (i.e. they cannot be asssumed to be zeros by default).
[in]	xji_l	The relative location of the observed landmark wrt to the robot/camera at the instant of observation.
[in]	sensor_params	Sensor-specific parameters, as set by the user.

Returns

true if the Jacobian is well-defined, false to mark it as ill-defined and ignore it during this step of the optimization

Definition at line 584 of file sensors.h.

References MRPT_UNUSED_PARAM, mrpt::poses::CPose3DQuat::sphericalCoordinates(), and mrpt::math::UNINITIALIZED_MATRIX.

static void mrpt::srba::sensor_model< landmarks::Euclidean3D, observations::RangeBearing_3D >::inverse_sensor_model ( landmark_traits< LANDMARK_T >::array_landmark_t &  out_lm_pos, const observation_traits< OBS_T >::obs_data_t &  obs, const OBS_T::TObservationParams &  params  )

inlinestatic

Inverse observation model for first-seen landmarks.

Needed to avoid having landmarks at (0,0,0) which leads to undefined Jacobians. This is invoked only when both "unknown_relative_position_init_val" and "is_fixed" are "false" in an observation. The LM location must not be exact at all, just make sure it doesn't have an undefined Jacobian.

Parameters

[out]	out_lm_pos	The relative landmark position wrt the current observing KF.
[in]	obs	The observation itself.
[in]	params	The sensor-specific parameters.

Definition at line 615 of file sensors.h.

References MRPT_UNUSED_PARAM.

template<class POSE_T >

static void mrpt::srba::sensor_model< landmarks::Euclidean3D, observations::RangeBearing_3D >::observe_error ( observation_traits< OBS_T >::array_obs_t &  out_obs_err, const observation_traits< OBS_T >::array_obs_t &  z_obs, const POSE_T &  base_pose_wrt_observer, const landmark_traits< LANDMARK_T >::array_landmark_t &  lm_pos, const OBS_T::TObservationParams &  params  )

inlinestatic

Executes the (negative) observation-error model: "-( h(lm_pos,pose) - z_obs)".

Parameters

[out]	out_obs_err	The output of the predicted sensor value
[in]	z_obs	The real observation, to be contrasted to the prediction of this sensor model
[in]	base_pose_wrt_observer	The relative pose of the observed landmark's base KF, wrt to the current sensor pose (which may be different than the observer KF pose if the sensor is not at the "robot origin").
[in]	lm_pos	The relative landmark position wrt its base KF.
[in]	params	The sensor-specific parameters.

Definition at line 543 of file sensors.h.

References mrpt::poses::CPose3D::sphericalCoordinates(), mrpt::math::TPoint3D::x, mrpt::math::TPoint3D::y, and mrpt::math::TPoint3D::z.

Member Data Documentation

const size_t mrpt::srba::sensor_model< landmarks::Euclidean3D, observations::RangeBearing_3D >::LM_DIMS = LANDMARK_T::LM_DIMS

static

Definition at line 528 of file sensors.h.

const size_t mrpt::srba::sensor_model< landmarks::Euclidean3D, observations::RangeBearing_3D >::OBS_DIMS = OBS_T::OBS_DIMS

static

Definition at line 527 of file sensors.h.