SRBA: Sparser Relative Bundle Adjustment
|
Sensor model: 3D landmarks in Euclidean coordinates + Monocular camera observations (no distortion)
#include <sensors.h>
Public Types | |
typedef observations::MonocularCamera | 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. | |
typedef landmark_traits < LANDMARK_T > ::array_landmark_t | array_landmark_t |
a 2D or 3D point | |
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 ¶ms) |
static bool | eval_jacob_dh_dx (TJacobian_dh_dx &dh_dx, const array_landmark_t &xji_l, const TObservationParams &sensor_params) |
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 ¶ms) |
Static Public Attributes | |
static const size_t | OBS_DIMS = OBS_T::OBS_DIMS |
static const size_t | LM_DIMS = LANDMARK_T::LM_DIMS |
static bool srba::sensor_model< landmarks::Euclidean3D, observations::MonocularCamera >::eval_jacob_dh_dx | ( | TJacobian_dh_dx & | dh_dx, |
const array_landmark_t & | xji_l, | ||
const TObservationParams & | sensor_params | ||
) | [inline, static] |
Evaluates the partial Jacobian dh_dx:
d h(x') dh_dx = ------------- d x'
With:
[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. |
static void srba::sensor_model< landmarks::Euclidean3D, observations::MonocularCamera >::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 | ||
) | [inline, static] |
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.
[out] | out_lm_pos | The relative landmark position wrt the current observing KF. |
[in] | obs | The observation itself. |
[in] | params | The sensor-specific parameters. |
static void srba::sensor_model< landmarks::Euclidean3D, observations::MonocularCamera >::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 | ||
) | [inline, static] |
Executes the (negative) observation-error model: "-( h(lm_pos,pose) - z_obs)"
[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. |