3D segments using the anchored plucker parametrisation observed in a perspective image. More...

Detailed Description

3D segments using the anchored plucker parametrisation observed in a perspective image.

Definition at line 150 of file segmentAnchorPluckerFeature.hpp.

#include <segmentAnchorPluckerFeature.hpp>

Inheritance diagram for jafar::slam::ImageAnchorPluckerFeatureObserveModel:

Public Member Functions
	ImageAnchorPluckerFeatureObserveModel (SegAnchorPluckerFeatureModel &featureModel_, jafar::camera::CameraPinhole const &camera_)
Observation::ObservationType	typeObs () const
void	setPixCov (jblas::vec const &pixCov_, double stabilizingFactor=1.0)
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	postInitCovariance (jblas::sym_mat_range &P, jblas::mat const &Jx)
	Assign covariances of the unobserved part.
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.
virtual jblas::vec const &	computeInnovation (jblas::vec const &z_, jblas::vec const &zPred_)
	Innovation.
virtual void	computeSensorR (Observation const &obs)
	Build covariances matrix of observation.
void	setup (double beta0=0, double beta1=0, double sigma_beta=0.5)
	Set up initial parameters.
Static Public Member Functions
static jblas::vec2	extToImageModel (jblas::vec const &ext1, jblas::vec const &ext2)
	Compute two lines from the optical center through both segment extremities.
static void	extToImageModelJac (jblas::vec const &ext1, jblas::vec const &ext2, jblas::mat &J)
	The Jacobians of extToLine() wrt the extremes.
Protected Member Functions
boost::tuple< double, double >	computeExtremitiesAbscissa (SegmentAnchorFeature const &segFeature_, geom::T3DEuler const &robotPose_, SegmentObservation const &obs_) const
void	segmentExtremitiesPluckerLines (SegmentObservation 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().
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
SegAnchorPluckerFeatureModel &	segAnchorPluckerFeatureModel
	Feature model.
jafar::camera::CameraPinhole	camera
	Perspective camera.
jblas::mat33	Pl
	Plucker camera matrix.
jblas::mat33	Plinv
	Plucker camera inverse matrix.
jblas::vec2	beta
	initial plane base vector
jblas::mat22	covar_beta
	covariances matrix of initial plane base vector
jblas::mat	JbBeta
	Jacobian of b wrt beta.
jblas::sym_mat	p_pixCov
	covariance on pixel coordinate (u,v)

Member Function Documentation

virtual void jafar::slam::ImageAnchorPluckerFeatureObserveModel::computeSensorR ( Observation const & obs ) [virtual]

Build covariances matrix of observation.

Parameters:

obs	observation as a couple of endpoints

Reimplemented from jafar::slam::BaseFeatureObserveModel.

static jblas::vec2 jafar::slam::ImageAnchorPluckerFeatureObserveModel::extToImageModel	(	jblas::vec const &	ext1,
		jblas::vec const &	ext2
	)		`[static]`

Compute two lines from the optical center through both segment extremities.

Parameters:

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

static void jafar::slam::ImageAnchorPluckerFeatureObserveModel::extToImageModelJac	(	jblas::vec const &	ext1,
		jblas::vec const &	ext2,
		jblas::mat &	J
	)		`[static]`

The Jacobians of extToLine() wrt the extremes.

Parameters:

ext1	the first extreme of the detected segment
ext2	the second extreme
J	The returned Jacobian

jblas::vec jafar::slam::ImageAnchorPluckerFeatureObserveModel::inverseObservationInSensorFrame ( Observation const & obs_ ) [protected, virtual]

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

Implements jafar::slam::FeatureObserveModel.

void jafar::slam::ImageAnchorPluckerFeatureObserveModel::postInitCovariance	(	jblas::sym_mat_range &	P,
		jblas::mat const &	Jx
	)		`[virtual]`

Assign covariances of the unobserved part.

Parameters:

P	covariance of the landmark to initialize in global frame
Jx	Jacobian of the feature in global frame wrt the feat. in robot frame

Reimplemented from jafar::slam::FeatureObserveModel.

void jafar::slam::ImageAnchorPluckerFeatureObserveModel::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::ImageAnchorPluckerFeatureObserveModel::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

void jafar::slam::ImageAnchorPluckerFeatureObserveModel::setup	(	double	beta0 = `0`,
		double	beta1 = `0`,
		double	sigma_beta = `0.5`
	)

Set up initial parameters.

Parameters:

beta0	The first component of the lines director vector for initialization
beta1	the second component
sigma_beta	The standard deviation of beta0 and beta1

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

modules/slam/include/slam/segmentAnchorPluckerFeature.hpp

Detailed Description

Public Member Functions

Static Public Member Functions

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Member Function Documentation