#include <vpPose.h>

Public Types
enum	vpPoseMethodType { LAGRANGE, DEMENTHON, LOWE, RANSAC, LAGRANGE_LOWE, DEMENTHON_LOWE, VIRTUAL_VS, DEMENTHON_VIRTUAL_VS, LAGRANGE_VIRTUAL_VS }

Public Member Functions
	vpPose ()

virtual	~vpPose ()

void	addPoint (const vpPoint &P)

void	clearPoint ()

bool	computePose (vpPoseMethodType methode, vpHomogeneousMatrix &cMo, bool(func)(vpHomogeneousMatrix )=NULL)

double	computeResidual (const vpHomogeneousMatrix &cMo) const

bool	coplanar (int &coplanar_plane_type)

void	displayModel (vpImage< unsigned char > &I, vpCameraParameters &cam, vpColor col=vpColor::none)

void	displayModel (vpImage< vpRGBa > &I, vpCameraParameters &cam, vpColor col=vpColor::none)

void	init ()

void	poseDementhonPlan (vpHomogeneousMatrix &cMo)

void	poseDementhonNonPlan (vpHomogeneousMatrix &cMo)

void	poseLagrangePlan (vpHomogeneousMatrix &cMo, const int coplanar_plane_type=0)

void	poseLagrangeNonPlan (vpHomogeneousMatrix &cMo)

void	poseLowe (vpHomogeneousMatrix &cMo)

bool	poseRansac (vpHomogeneousMatrix &cMo, bool(func)(vpHomogeneousMatrix )=NULL)

void	poseVirtualVSrobust (vpHomogeneousMatrix &cMo)

void	poseVirtualVS (vpHomogeneousMatrix &cMo)

void	printPoint ()

void	setDistanceToPlaneForCoplanarityTest (double d)

void	setLambda (double a)

void	setVvsIterMax (int nb)

void	setRansacNbInliersToReachConsensus (const unsigned int &nbC)

void	setRansacThreshold (const double &t)

void	setRansacMaxTrials (const int &rM)

unsigned int	getRansacNbInliers () const

std::vector< vpPoint >	getRansacInliers () const

void	setCovarianceComputation (const bool &flag)

vpMatrix	getCovarianceMatrix () const

Static Public Member Functions
static void	display (vpImage< unsigned char > &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col=vpColor::none)

static void	display (vpImage< vpRGBa > &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col=vpColor::none)

static double	poseFromRectangle (vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam, vpHomogeneousMatrix &cMo)

static void	findMatch (std::vector< vpPoint > &p2D, std::vector< vpPoint > &p3D, const unsigned int &numberOfInlierToReachAConsensus, const double &threshold, unsigned int &ninliers, std::vector< vpPoint > &listInliers, vpHomogeneousMatrix &cMo, const int &maxNbTrials=10000)

Public Attributes
unsigned int	npt

std::list< vpPoint >	listP

double	residual

double	distanceToPlaneForCoplanarityTest

Protected Member Functions
double	computeResidualDementhon (const vpHomogeneousMatrix &cMo)

int	calculArbreDementhon (vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo)

Protected Attributes
double	lambda

Deprecated functions
static void	computeTransformation (vpColVector &x, unsigned int *ind, vpColVector &M)

static double	computeResidual (const vpColVector &x, const vpColVector &M, vpColVector &d)

static bool	degenerateConfiguration (vpColVector &x, unsigned int *ind)

static void	ransac (const unsigned int n, const double x, const double y, const unsigned int m, const double X, const double Y, const double *Z, const int numberOfInlierToReachAConsensus, const double threshold, unsigned int &ninliers, vpColVector &xi, vpColVector &yi, vpColVector &Xi, vpColVector &Yi, vpColVector &Zi, vpHomogeneousMatrix &cMo, const int maxNbTrials=10000)

static vp_deprecated void	ransac (const unsigned int n, const vpPoint p, const unsigned int m, const vpPoint P, const int numberOfInlierToReachAConsensus, const double threshold, unsigned int &ninliers, std::list< vpPoint > &Pi, vpHomogeneousMatrix &cMo, const int maxNbTrials=10000)

static vp_deprecated void	ransac (std::list< vpPoint > &p, std::list< vpPoint > &P, const int numberOfInlierToReachAConsensus, const double threshold, unsigned int &ninliers, std::list< vpPoint > &lPi, vpHomogeneousMatrix &cMo, const int maxNbTrials=10000)

Detailed Description

Class used for pose computation from N points (pose from point only).

Note: It is also possible to estimate a pose from other features using vpPoseFeatures class.

To see how to use this class you can follow the Tutorial: Pose estimation from points.

Examples:: AROgre.cpp, AROgreBasic.cpp, poseVirtualVS.cpp, servoAfma62DhalfCamVelocity.cpp, servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, testPose.cpp, testPoseRansac.cpp, testRobotAfma6Pose.cpp, testRobotViper850Pose.cpp, tutorial-pose-from-points-image.cpp, and tutorial-pose-from-points-tracking.cpp.

Definition at line 78 of file vpPose.h.

Member Enumeration Documentation

enum vpPose::vpPoseMethodType

Enumerator
LAGRANGE
DEMENTHON
LOWE
RANSAC
LAGRANGE_LOWE
DEMENTHON_LOWE
VIRTUAL_VS
DEMENTHON_VIRTUAL_VS
LAGRANGE_VIRTUAL_VS

Definition at line 81 of file vpPose.h.

Constructor & Destructor Documentation

vpPose::vpPose ( )

Defaukt constructor.

Definition at line 96 of file vpPose.cpp.

References init().

vpPose::~vpPose ( )

virtual

destructor

Destructor that deletes the array of point (freed the memory).

Definition at line 117 of file vpPose.cpp.

References listP.

Member Function Documentation

void vpPose::addPoint ( const vpPoint & newP )

Add a new point in this array.

Add a new point in the array of point.

Parameters

newP	: New point to add in the array of point.

Warning: Considering a point from the class vpPoint, X, Y, and Z will represent the 3D information and x and y its 2D information. These 5 fields must be initialized to be used within this library

Examples:: AROgre.cpp, AROgreBasic.cpp, poseVirtualVS.cpp, servoAfma62DhalfCamVelocity.cpp, servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, testPose.cpp, testPoseRansac.cpp, testRobotAfma6Pose.cpp, testRobotViper850Pose.cpp, tutorial-pose-from-points-image.cpp, and tutorial-pose-from-points-tracking.cpp.

Definition at line 155 of file vpPose.cpp.

References listP, and npt.

Referenced by vpKeyPoint::computePose(), computeTransformation(), findMatch(), vpMbTracker::initClick(), vpMbTracker::initFromPoints(), poseFromRectangle(), and poseRansac().

int vpPose::calculArbreDementhon	(	vpMatrix &	b,
		vpColVector &	U,
		vpHomogeneousMatrix &	cMo
	)

protected

Definition at line 284 of file vpPoseDementhon.cpp.

References computeResidualDementhon(), vpColVector::dotProd(), npt, vpMatrix::sumSquare(), and vpColVector::t().

Referenced by poseDementhonPlan().

void vpPose::clearPoint ( )

suppress all the point in the array of point

Delete the array of point

Examples:: AROgre.cpp, AROgreBasic.cpp, poseVirtualVS.cpp, servoAfma62DhalfCamVelocity.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, and testPose.cpp.

Definition at line 133 of file vpPose.cpp.

References listP, and npt.

Referenced by computeTransformation(), and vpMbTracker::initClick().

bool vpPose::computePose	(	vpPoseMethodType	methode,
		vpHomogeneousMatrix &	cMo,
		bool()(vpHomogeneousMatrix )	func = `NULL`
	)

compute the pose for a given method

Compute the pose according to the desired method.

the different method are

LAGRANGE Lagrange approach (test is done to switch between planar and non planar algorithm)

DEMENTHON Dementhon approach (test is done to switch between planar and non planar algorithm)

VIRTUAL_VS Virtual visual servoing approach

DEMENTHON_VIRTUAL_VS Virtual visual servoing approach initialized using Dementhon approach

LAGRANGE_VIRTUAL_VS Virtual visual servoing initialized using Lagrange approach

Examples:: AROgre.cpp, AROgreBasic.cpp, poseVirtualVS.cpp, servoAfma62DhalfCamVelocity.cpp, servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, testPose.cpp, testPoseRansac.cpp, testRobotAfma6Pose.cpp, testRobotViper850Pose.cpp, tutorial-pose-from-points-image.cpp, and tutorial-pose-from-points-tracking.cpp.

Definition at line 386 of file vpPose.cpp.

References coplanar(), DEMENTHON, DEMENTHON_LOWE, DEMENTHON_VIRTUAL_VS, LAGRANGE, LAGRANGE_LOWE, LAGRANGE_VIRTUAL_VS, LOWE, vpPoseException::notEnoughPointError, npt, poseDementhonNonPlan(), poseDementhonPlan(), poseLagrangeNonPlan(), poseLagrangePlan(), poseLowe(), poseRansac(), poseVirtualVS(), RANSAC, VIRTUAL_VS, and vpERROR_TRACE.

Referenced by vpKeyPoint::computePose(), computeTransformation(), findMatch(), vpMbTracker::initClick(), vpMbTracker::initFromPoints(), poseFromRectangle(), and poseRansac().

double vpPose::computeResidual ( const vpHomogeneousMatrix & cMo ) const

Compute and return the residual expressed in meter for the pose matrix 'cMo'.

compute the residual (i.e., the quality of the result) compute the residual (in meter for pose M)

Parameters

cMo	: Input pose. The matrix that defines the pose to be tested.

Returns: The value of he residual in meter.

Examples:: servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, testPose.cpp, tutorial-pose-from-points-image.cpp, and tutorial-pose-from-points-tracking.cpp.

Definition at line 344 of file vpPose.cpp.

References vpPoint::get_x(), vpPoint::get_y(), listP, vpMath::sqr(), and vpForwardProjection::track().

Referenced by vpMbTracker::initClick(), vpMbTracker::initFromPoints(), and poseRansac().

double vpPose::computeResidual	(	const vpColVector &	x,
		const vpColVector &	M,
		vpColVector &	d
	)

static

Deprecated:: Evaluate distances between points and model.

This function can certainly be optimized...

Definition at line 517 of file vpPoseRansac.cpp.

References vpPoint::changeFrame(), vpMatrix::data, vpMatrix::getRows(), vpPoint::projection(), vpColVector::resize(), vpPoint::setWorldCoordinates(), and vpMath::sqr().

double vpPose::computeResidualDementhon ( const vpHomogeneousMatrix & cMo )

protected

Compute and return the residual expressed in meter for the pose matrix 'pose'.

Parameters

cMo	: the matrix that defines the pose to be tested.

Returns: the value of he residual in meter

Definition at line 692 of file vpPoseDementhon.cpp.

References npt, and vpMath::sqr().

Referenced by calculArbreDementhon(), and poseDementhonPlan().

void vpPose::computeTransformation	(	vpColVector &	x,
		unsigned int *	ind,
		vpColVector &	M
	)

static

Deprecated:: Fit model to this random selection of data points.

We chose the Dementhon algorithm to compute the pose.

Definition at line 470 of file vpPoseRansac.cpp.

References addPoint(), clearPoint(), computePose(), vpMatrix::data, DEMENTHON, vpColVector::resize(), vpPoint::set_x(), vpPoint::set_y(), vpHomogeneousMatrix::setIdentity(), and vpPoint::setWorldCoordinates().

bool vpPose::coplanar ( int & coplanar_plane_type )

test the coplanarity of the points

test the coplanarity of the set of points

Parameters

coplanar_plane_type 1: if plane x=cst 2: if plane y=cst 3: if plane z=cst 4: if the points are collinear. 0: any other plane

Returns: true if points are coplanar false if not

Definition at line 190 of file vpPose.cpp.

References vpColVector::crossProd(), distanceToPlaneForCoplanarityTest, vpPoint::get_oX(), vpPoint::get_oY(), vpPoint::get_oZ(), listP, vpPoseException::notEnoughPointError, npt, vpMath::sqr(), vpMatrix::sumSquare(), vpDEBUG_TRACE, and vpERROR_TRACE.

Referenced by computePose().

bool vpPose::degenerateConfiguration	(	vpColVector &	x,
		unsigned int *	ind
	)

static

Definition at line 446 of file vpPoseRansac.cpp.

void vpPose::display	(	vpImage< unsigned char > &	I,
		vpHomogeneousMatrix &	cMo,
		vpCameraParameters &	cam,
		double	size,
		vpColor	col = `vpColor::none`
	)

static

Examples:: AROgre.cpp, AROgreBasic.cpp, and poseVirtualVS.cpp.

Definition at line 589 of file vpPose.cpp.

References vpDisplay::displayFrame().

void vpPose::display	(	vpImage< vpRGBa > &	I,
		vpHomogeneousMatrix &	cMo,
		vpCameraParameters &	cam,
		double	size,
		vpColor	col = `vpColor::none`
	)

static

Definition at line 600 of file vpPose.cpp.

References vpDisplay::displayFrame().

void vpPose::displayModel	(	vpImage< unsigned char > &	I,
		vpCameraParameters &	cam,
		vpColor	col = `vpColor::none`
	)

Definition at line 615 of file vpPose.cpp.

References vpMeterPixelConversion::convertPoint(), vpDisplay::displayCross(), listP, and vpTracker::p.

void vpPose::displayModel	(	vpImage< vpRGBa > &	I,
		vpCameraParameters &	cam,
		vpColor	col = `vpColor::none`
	)

Definition at line 638 of file vpPose.cpp.

References vpMeterPixelConversion::convertPoint(), vpDisplay::displayCross(), listP, and vpTracker::p.

void vpPose::findMatch	(	std::vector< vpPoint > &	p2D,
		std::vector< vpPoint > &	p3D,
		const unsigned int &	numberOfInlierToReachAConsensus,
		const double &	threshold,
		unsigned int &	ninliers,
		std::vector< vpPoint > &	listInliers,
		vpHomogeneousMatrix &	cMo,
		const int &	maxNbTrials = `10000`
	)

static

Match a vector p2D of 2D point (x,y) and a vector p3D of 3D points (X,Y,Z) using the Ransac algorithm.

At least numberOfInlierToReachAConsensus of true correspondance are required to validate the pose

The inliers are given in a vector of vpPoint listInliers.

The pose is returned in cMo.

Parameters

p2D	: Vector of 2d points (x and y attributes are used).
p3D	: Vector of 3d points (oX, oY and oZ attributes are used).
numberOfInlierToReachAConsensus	: The minimum number of inlier to have to consider a trial as correct.
threshold	: The maximum error allowed between the 2d points and the reprojection of its associated 3d points by the current pose (in meter).
ninliers	: Number of inliers found for the best solution.
listInliers	: Vector of points (2d and 3d) that are inliers for the best solution.
cMo	: The computed pose (best solution).
maxNbTrials	: Maximum number of trials before considering a solution fitting the required numberOfInlierToReachAConsensus and threshold cannot be found.

Examples:: testFindMatch.cpp.

Definition at line 388 of file vpPoseRansac.cpp.

References addPoint(), computePose(), getRansacInliers(), getRansacNbInliers(), listP, vpPoseException::notEnoughPointError, RANSAC, vpPoint::set_x(), vpPoint::set_y(), setRansacMaxTrials(), setRansacNbInliersToReachConsensus(), setRansacThreshold(), vpPoint::setWorldCoordinates(), and vpERROR_TRACE.

vpMatrix vpPose::getCovarianceMatrix ( ) const

inline

Get the covariance matrix computed in the Virtual Visual Servoing approach.

Warning: The compute covariance flag has to be true if you want to compute the covariance matrix.

See also: setCovarianceComputation

Definition at line 196 of file vpPose.h.

References vpTRACE.

Referenced by vpKeyPoint::computePose().

std::vector<vpPoint> vpPose::getRansacInliers ( ) const

inline

Examples:: testPoseRansac.cpp.

Definition at line 180 of file vpPose.h.

Referenced by vpKeyPoint::computePose(), and findMatch().

unsigned int vpPose::getRansacNbInliers ( ) const

inline

Definition at line 179 of file vpPose.h.

Referenced by findMatch().

void vpPose::init ( )

basic initialisation (called by the constructors)

Definition at line 67 of file vpPose.cpp.

References distanceToPlaneForCoplanarityTest, lambda, listP, npt, and residual.

Referenced by vpPose().

void vpPose::poseDementhonNonPlan ( vpHomogeneousMatrix & cMo )

compute the pose using Dementhon approach (non planar object)

Compute the pose using Dementhon approach for non planar objects this is a direct implementation of the algorithm proposed by Dementhon and Davis in their 1995 paper [9].

Definition at line 66 of file vpPoseDementhon.cpp.

References vpColVector::cross(), vpException::divideByZeroError, vpPoint::get_oX(), vpPoint::get_oY(), vpPoint::get_oZ(), listP, vpColVector::normalize(), npt, vpMatrix::pseudoInverse(), vpColVector::resize(), vpPoint::set_oX(), vpPoint::set_oY(), vpPoint::set_oZ(), vpMatrix::sumSquare(), and vpMatrix::t().

Referenced by computePose().

void vpPose::poseDementhonPlan ( vpHomogeneousMatrix & cMo )

compute the pose using Dementhon approach (planar object)

Compute the pose using Dementhon approach for planar objects this is a direct implementation of the algorithm proposed by Dementhon in his PhD.

Author: Francois Chaumette (simplified by Eric Marchand)

Definition at line 475 of file vpPoseDementhon.cpp.

References calculArbreDementhon(), computeResidualDementhon(), vpColVector::dotProd(), vpPoint::get_oX(), vpPoint::get_oY(), vpPoint::get_oZ(), vpMatrix::getCol(), vpMatrix::getCols(), vpMatrix::getRows(), listP, npt, vpMatrix::resize(), vpPoint::set_oX(), vpPoint::set_oY(), vpPoint::set_oZ(), vpMatrix::sumSquare(), vpMatrix::svd(), vpColVector::t(), vpMatrix::t(), and vpERROR_TRACE.

Referenced by computePose().

double vpPose::poseFromRectangle	(	vpPoint &	p1,
		vpPoint &	p2,
		vpPoint &	p3,
		vpPoint &	p4,
		double	lx,
		vpCameraParameters &	cam,
		vpHomogeneousMatrix &	cMo
	)

static

Carries out the camera pose the image of a rectangle and the intrinsec parameters, the length on x axis is known but the proprtion of the rectangle are unknown.

This method is taken from "Markerless Tracking using Planar Structures in the Scene" by Gilles Simon. The idea is to compute the homography H giving the image point of the rectangle by associating them with the coordinates (0,0)(1,0)(1,1/s)(0,1/s) (the rectangle is on the Z=0 plane). If K is the intrinsec parameters matrix, we have s = ||Kh1||/ ||Kh2||. s gives us the proportion of the rectangle

Parameters

p1,p2,p3,p4	the image of the corners of the rectangle (respectively the image of (0,0),(lx,0),(lx,lx/s) and (0,lx/s)) (input)
cam	the camera used (input)
lx	the rectangle size on the x axis (input)
cMo	the camera pose (output)

Returns: int : OK if no pb occurs

Definition at line 676 of file vpPose.cpp.

References addPoint(), computePose(), DEMENTHON_LOWE, vpCameraParameters::get_K(), vpPoint::get_x(), vpPoint::get_y(), vpMatrix::getCol(), vpHomography::HLM(), vpMatrix::pseudoInverse(), vpMatrix::setIdentity(), vpPoint::setWorldCoordinates(), and vpMatrix::sumSquare().

void vpPose::poseLagrangeNonPlan ( vpHomogeneousMatrix & cMo )

compute the pose using Lagrange approach (non planar object)

Definition at line 469 of file vpPoseLagrange.cpp.

References vpException::divideByZeroError, vpPoint::get_oX(), vpPoint::get_oY(), vpPoint::get_oZ(), vpPoint::get_x(), vpPoint::get_y(), listP, npt, and vpMatrix::sumSquare().

Referenced by computePose().

void vpPose::poseLagrangePlan	(	vpHomogeneousMatrix &	cMo,
		const int	coplanar_plane_type = `0`
	)

compute the pose using Lagrange approach (planar object)

Compute the pose of a planar object using Lagrange approach.

Parameters

cMo	: Estimated pose. No initialisation is requested to estimate cMo.
coplanar_plane_type	: Type of coplanar plane: 1: if plane x=cst 2: if plane y=cst 3: if plane z=cst 0: any other plane

Definition at line 250 of file vpPoseLagrange.cpp.

References vpException::divideByZeroError, vpPoint::get_oX(), vpPoint::get_oY(), vpPoint::get_oZ(), vpPoint::get_x(), vpPoint::get_y(), listP, npt, and vpMatrix::sumSquare().

Referenced by computePose().

void vpPose::poseLowe ( vpHomogeneousMatrix & cMo )

Compute the pose using the Lowe non linear approach it consider the minimization of a residual using the levenberg marquartd approach.

compute the pose using the Lowe approach (i.e., using the Levenberg Marquartd non linear minimization approach)

The approach has been proposed by D.G Lowe in 1992 paper [15].

Definition at line 288 of file vpPoseLowe.cpp.

References vpHomogeneousMatrix::extract(), vpPoint::get_oX(), vpPoint::get_oY(), vpPoint::get_oZ(), vpPoint::get_x(), vpPoint::get_y(), vpHomogeneousMatrix::insert(), and listP.

Referenced by computePose().

bool vpPose::poseRansac	(	vpHomogeneousMatrix &	cMo,
		bool()(vpHomogeneousMatrix )	func = `NULL`
	)

compute the pose using the Ransac approach

Compute the pose using the Ransac approach.

Parameters

cMo	: Computed pose
func	: Pointer to a function that takes in parameter a vpHomogeneousMatrix and returns true if the pose check is OK or false otherwise

Returns: True if we found at least 4 points with a reprojection error below ransacThreshold.

Definition at line 75 of file vpPoseRansac.cpp.

References addPoint(), computePose(), computeResidual(), DEMENTHON, vpPoint::get_oX(), vpPoint::get_oY(), vpPoint::get_oZ(), vpPoint::get_x(), vpPoint::get_y(), LAGRANGE, listP, vpPoseException::notInitializedError, npt, setCovarianceComputation(), vpMath::sqr(), vpForwardProjection::track(), VIRTUAL_VS, and vpERROR_TRACE.

Referenced by computePose().

void vpPose::poseVirtualVS ( vpHomogeneousMatrix & cMo )

compute the pose using virtual visual servoing approach

Compute the pose using virtual visual servoing approach.

This approach is described in [19].

Definition at line 62 of file vpPoseVirtualVisualServoing.cpp.

References vpMatrix::computeCovarianceMatrix(), vpExponentialMap::direct(), vpPoint::get_x(), vpPoint::get_y(), vpPoint::get_Z(), vpHomogeneousMatrix::inverse(), lambda, listP, vpMatrix::pseudoInverse(), vpMatrix::sumSquare(), vpForwardProjection::track(), and vpERROR_TRACE.

Referenced by computePose().

void vpPose::poseVirtualVSrobust ( vpHomogeneousMatrix & cMo )

compute the pose using a robust virtual visual servoing approach

Compute the pose using virtual visual servoing approach and a robust cotrol law.

This approach is described in [7].

Definition at line 165 of file vpPoseVirtualVisualServoing.cpp.

References vpMatrix::computeCovarianceMatrix(), vpExponentialMap::direct(), vpPoint::get_x(), vpPoint::get_y(), vpPoint::get_Z(), vpMatrix::getRows(), vpHomogeneousMatrix::inverse(), lambda, listP, vpRobust::MEstimator(), vpColVector::resize(), vpMatrix::resize(), vpRobust::setIteration(), vpRobust::setThreshold(), vpMath::sqr(), vpMatrix::sumSquare(), vpForwardProjection::track(), vpRobust::TUKEY, and vpERROR_TRACE.

void vpPose::printPoint ( )

Definition at line 574 of file vpPose.cpp.

References vpTracker::cP, listP, vpForwardProjection::oP, vpTracker::p, and vpColVector::t().

void vpPose::ransac	(	const unsigned int	n,
		const double *	x,
		const double *	y,
		const unsigned int	m,
		const double *	X,
		const double *	Y,
		const double *	Z,
		const int	numberOfInlierToReachAConsensus,
		const double	threshold,
		unsigned int &	ninliers,
		vpColVector &	xi,
		vpColVector &	yi,
		vpColVector &	Xi,
		vpColVector &	Yi,
		vpColVector &	Zi,
		vpHomogeneousMatrix &	cMo,
		const int	maxNbTrials = `10000`
	)

static

Deprecated:: Compute the pose from a set of n 2D point (x,y) and m 3D points (X,Y,Z) using the Ransac algorithm. It is not assumed that the 2D and 3D points are registred (there is nm posibilities).

At least numberOfInlierToReachAConsensus of true correspondance are required to validate the pose.

The inliers are given in xi, yi, Xi, Yi, Zi.
The pose is returned in cMo.

Parameters

n	: Number of 2d points.
x	: Array (of size n) of the x coordinates of the 2d points.
y	: Array (of size n) of the y coordinates of the 2d points.
m	: Number of 3d points.
X	: Array (of size m) of the oX coordinates of the 3d points.
Y	: Array (of size m) of the oY coordinates of the 3d points.
Z	: Array (of size m) of the oZ coordinates of the 3d points.
numberOfInlierToReachAConsensus	: The minimum number of inlier to have to consider a trial as correct.
threshold	: The maximum error allowed between the 2d points and the reprojection of its associated 3d points by the current pose (in meter).
ninliers	: Number of inliers found for the best solution.
xi	: Array (of size ninliers) of the x coordinates of the inliers.
yi	: Array (of size ninliers) of the y coordinates of the inliers.
Xi	: Array (of size ninliers) of the oX coordinates of the inliers.
Yi	: Array (of size ninliers) of the oY coordinates of the inliers.
Zi	: Array (of size ninliers) of the oZ coordinates of the inliers.
cMo	: The computed pose (best solution).
maxNbTrials	: Maximum number of trials before considering a solution fitting the required numberOfInlierToReachAConsensus and threshold cannot be found.

Definition at line 613 of file vpPoseRansac.cpp.

References vpMatrix::data, vpMath::maximum(), vpTime::measureTimeMs(), vpRansac< vpTransformation >::ransac(), and vpColVector::resize().

Referenced by ransac().

void vpPose::ransac	(	const unsigned int	n,
		const vpPoint *	p,
		const unsigned int	m,
		const vpPoint *	P,
		const int	numberOfInlierToReachAConsensus,
		const double	threshold,
		unsigned int &	ninliers,
		std::list< vpPoint > &	lPi,
		vpHomogeneousMatrix &	cMo,
		const int	maxNbTrials = `10000`
	)

static

Deprecated:: Compute the pose from a set of n 2D point (x,y) in p and m 3D points (X,Y,Z) in P using the Ransac algorithm. It is not assumed that the 2D and 3D points are registred (there is nm posibilities).

At least numberOfInlierToReachAConsensus of true correspondance are required to validate the pose.

The inliers are given in a list of vpPoint.
The pose is returned in cMo.

Parameters

n	: Number of 2d points.
p	: Array (of size n) of 2d points (x and y attributes are used).
m	: Number of 3d points.
P	: Array of size m of 3d points (oX, oY and oZ attributes are used).
numberOfInlierToReachAConsensus	: The minimum number of inlier to have to consider a trial as correct.
threshold	: The maximum error allowed between the 2d points and the reprojection of its associated 3d points by the current pose (in meter).
ninliers	: Number of inliers found for the best solution.
lPi	: List of points (2d and 3d) that are inliers for the best solution.
cMo	: The computed pose (best solution).
maxNbTrials	: Maximum number of trials before considering a solution fitting the required numberOfInlierToReachAConsensus and threshold cannot be found.

Definition at line 706 of file vpPoseRansac.cpp.

References vpPoint::get_oX(), vpPoint::get_oY(), vpPoint::get_oZ(), vpPoint::get_x(), vpPoint::get_y(), ransac(), vpPoint::set_x(), vpPoint::set_y(), and vpPoint::setWorldCoordinates().

void vpPose::ransac	(	std::list< vpPoint > &	lp,
		std::list< vpPoint > &	lP,
		const int	numberOfInlierToReachAConsensus,
		const double	threshold,
		unsigned int &	ninliers,
		std::list< vpPoint > &	lPi,
		vpHomogeneousMatrix &	cMo,
		const int	maxNbTrials = `10000`
	)

static

Deprecated:: Compute the pose from a list lp of 2D point (x,y) and a list lP 3D points (X,Y,Z) in P using the Ransac algorithm. It is not assumed that the 2D and 3D points are registred.

At least numberOfInlierToReachAConsensus of true correspondance are required to validate the pose.

The inliers are given in a list of vpPoint lPi.
The pose is returned in cMo.

Parameters

lp	: List of 2d points (x and y attributes are used).
lP	: List of 3d points (oX, oY and oZ attributes are used).
numberOfInlierToReachAConsensus	: The minimum number of inlier to have to consider a trial as correct.
threshold	: The maximum error allowed between the 2d points and the reprojection of its associated 3d points by the current pose (in meter).
ninliers	: Number of inliers found for the best solution.
lPi	: List of points (2d and 3d) that are inliers for the best solution.
cMo	: The computed pose (best solution).
maxNbTrials	: Maximum number of trials before considering a solution fitting the required numberOfInlierToReachAConsensus and threshold cannot be found.