jafar::slam::StereoImagePluckerFeatureObserveModel Class Reference

Observation Model using a Stereo head, two uvd points for init, two rho-thetas for updates. More...

---

Detailed Description

Observation Model using a Stereo head, two uvd points for init, two rho-thetas for updates.

Definition at line 798 of file segmentFeature.hpp.

#include <segmentFeature.hpp>

Inheritance diagram for jafar::slam::StereoImagePluckerFeatureObserveModel:

List of all members.

Public Member Functions
	StereoImagePluckerFeatureObserveModel (SegmentFeatureModel &featureModel_, jafar::camera::StereoBench const &stereoBench_)
Observation::ObservationType	typeObs () const
	Get observation type.
void	setPixCov (jblas::vec const &pixCov_, double stabilizingFactor=1.0)
	Set pixel covariances matrix.
void	setDispVar (double dispVar, double stabilizingFactor=1.0)
	Set pixel disparity variance.
void	initInternalState (BaseFeature &feature, jafar::geom::T3DEuler const &robotPose_, Observation const &obs_) const
	Init extremities of the segment.
void	updateInternalState (BaseFeature &feature, jafar::geom::T3DEuler const &robotPose_, Observation const &obs_) const
	Update extremities of the segment.
void	predictExtObs (jblas::vec_range const &pose, jblas::sym_mat_range const &poseCov, jblas::vec3 const &ext, jblas::sym_mat const &extCov, jblas::vec &zPredExt, jblas::sym_mat &zPredExtCov)
	Predict the extremes of the observed line, with their covariance in the image plane.
void	predictExtObsJac (jblas::vec_range const &pose, jblas::vec3 const Ext, jblas::vec &eInImage, jblas::mat &JeR, jblas::mat &JeExt)
	Predict the extreme and Jacobians.
jblas::vec const &	computeInnovation (jblas::vec const &z_, jblas::vec const &zPred_)
	Innovation.
void	computeSensorR (Observation const &obs)
	Build covariances matrix of observation.
jblas::vec2	extToRhoTheta (jblas::vec const &ext1, jblas::vec const &ext2)
	Endpoints to Rho-Theta transform.
void	extToRhoThetaJac (jblas::vec const &ext1, jblas::vec const &ext2, jblas::mat &J)
	Endpoints to Rho-Theta transform.
Protected Member Functions
jblas::vec3	planeBaseToDirVector (jblas::vec3 const &an_, jblas::vec2 const &beta_)
	Plane sub-vector a to plane base vector beta.
void	planeBaseToDirVectorJac (jblas::vec_range const &an_, jblas::vec2 const &beta_, jblas::vec_range &b, jblas::mat33 &B_a)
	Plane sub-vector a to plane base vector -- Jacobians.
boost::tuple< double, double >	computeExtremitiesAbscissa (SegmentFeature const &segFeature_, geom::T3DEuler const &robotPose_, StereoSegmentObservation const &obs_) const
void	segmentExtremitiesPluckerLines (StereoSegmentObservation const &segObs_, jblas::vec &lineExt1Sensor, jblas::vec &lineExt2Sensor) const
jblas::vec const &	predictObservationInSensorFrame (const jblas::vec &feature_)
	Prediction of the observation, the feature is expressed in the sensor frame featureInSensorFrame_.
void	predictObservationInSensorFrameJac (const jblas::vec &feature_)
	Jacobian of predictObservationInSensorFrame().
void	computeSensorRInit (Observation const &obs)
	Compute sensor observation noise for initialization purposes.
jblas::vec	inverseObservationInSensorFrame (Observation const &obs_)
void	inverseObservationInSensorFrameJac (Observation const &obs_)
	jacobian of inverseObservationInSensorFrame()
Static Protected Member Functions
static boost::tuple< double, double, double >	pluckerLinesDistance (jblas::vec const &line1_, jblas::vec const &line2_)
Protected Attributes
SegmentFeatureModel &	segmentFeatureModel
jafar::camera::StereoBench	stereoBench
jblas::mat33	Pl
jblas::mat33	Plinv
jblas::sym_mat	p_pixCov
	covariance on pixel coordinate (u,v)
double	p_dispVar
	covariance on pixel disparity

---

Member Function Documentation

void jafar::slam::StereoImagePluckerFeatureObserveModel::computeSensorR

(

Observation const &

obs

)

[virtual]

Build covariances matrix of observation.

Parameters:

obs	observation as a couple of stereo endpoints This function computes the covariances matrix of the measurement in the space defined for the Kalman updates, which is a couple of rho-theta lines, the stereo-rho-theta line.

Reimplemented from jafar::slam::BaseFeatureObserveModel.

jblas::vec2 jafar::slam::StereoImagePluckerFeatureObserveModel::extToRhoTheta	(	jblas::vec const &	ext1,
		jblas::vec const &	ext2
	)

Endpoints to Rho-Theta transform.

Parameters:

ext1	the first extremity of the detected segment
ext2	the second extremity

void jafar::slam::StereoImagePluckerFeatureObserveModel::extToRhoThetaJac	(	jblas::vec const &	ext1,
		jblas::vec const &	ext2,
		jblas::mat &	J
	)

Endpoints to Rho-Theta transform.

Parameters:

ext1	the first extremity of the detected segment
ext2	the second extremity
J	the Jacobian wrt (ext,ext2)

jblas::vec jafar::slam::StereoImagePluckerFeatureObserveModel::inverseObservationInSensorFrame

(

Observation const &

obs_

)

[protected, virtual]

inverse observation function in sensor frame, to be defined by user.

Implements jafar::slam::FeatureObserveModel.

void jafar::slam::StereoImagePluckerFeatureObserveModel::predictExtObs	(	jblas::vec_range const &	pose,
		jblas::sym_mat_range const &	poseCov,
		jblas::vec3 const &	ext,
		jblas::sym_mat const &	extCov,
		jblas::vec &	zPredExt,
		jblas::sym_mat &	zPredExtCov
	)

Predict the extremes of the observed line, with their covariance in the image plane.

Parameters:

pose	the robot pose
poseCov	the robot pose covariance matrix
ext	one of the extremities in 3D space
extCov	the covariances matrix of the extreme in 3D space
zPredExt	the predicted extreme in the 2D image
zPredExtCov	the covariances matrix of the predicted extreme CAUTION: THIS IMPLEMENTATION IGNORES THE CROSS-VARIANCES ROBOT/FEATURE AND LEADS TO TOO BIG ELLIPSES IN THE IMAGE.

void jafar::slam::StereoImagePluckerFeatureObserveModel::predictExtObsJac	(	jblas::vec_range const &	pose,
		jblas::vec3 const	Ext,
		jblas::vec &	eInImage,
		jblas::mat &	JeR,
		jblas::mat &	JeExt
	)

Predict the extreme and Jacobians.

Parameters:

pose	the robot pose
Ext	one of the extremities in 3D space
eInImage	the predicted extreme in the 2D image
JeR	and
JeExt	Jacobians of the prediction with respect to pose and extremity

---

The documentation for this class was generated from the following file:

• modules/slam/include/slam/segmentFeature.hpp