#include <visp3/vision/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< unsigned int >	getRansacInlierIndex () 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

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, testKeyPoint-4.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 74 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 77 of file vpPose.h.

Constructor & Destructor Documentation

vpPose::vpPose ( )

Defaukt constructor.

Definition at line 92 of file vpPose.cpp.

References init().

vpPose::~vpPose ( )

virtual

destructor

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

Definition at line 113 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, testKeyPoint-4.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 151 of file vpPose.cpp.

References listP, and npt.

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

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

protected

Definition at line 280 of file vpPoseDementhon.cpp.

References computeResidualDementhon(), vpColVector::dotProd(), npt, vpColVector::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 129 of file vpPose.cpp.

References listP, and npt.

Referenced by 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, testKeyPoint-4.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 382 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(), 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 340 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::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 688 of file vpPoseDementhon.cpp.

References npt, and vpMath::sqr().

Referenced by calculArbreDementhon(), and poseDementhonPlan().

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 186 of file vpPose.cpp.

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

Referenced by computePose().

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 585 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 596 of file vpPose.cpp.

References vpDisplay::displayFrame().

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

Definition at line 611 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 634 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 420 of file vpPoseRansac.cpp.

References addPoint(), computePose(), getRansacInliers(), getRansacNbInliers(), listP, vpPoseException::notEnoughPointError, RANSAC, vpPoint::set_x(), vpPoint::set_y(), setRansacMaxTrials(), setRansacNbInliersToReachConsensus(), setRansacThreshold(), 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 194 of file vpPose.h.

References vpTRACE.

Referenced by vpKeyPoint::computePose().

std::vector<unsigned int> vpPose::getRansacInlierIndex ( ) const

inline

Definition at line 177 of file vpPose.h.

Referenced by vpKeyPoint::computePose().

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

inline

Examples:: testPoseRansac.cpp.

Definition at line 178 of file vpPose.h.

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

unsigned int vpPose::getRansacNbInliers ( ) const

inline

Definition at line 176 of file vpPose.h.

Referenced by findMatch().

void vpPose::init ( void )

basic initialisation (called by the constructors)

Definition at line 63 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 62 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(), vpColVector::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 471 of file vpPoseDementhon.cpp.

References calculArbreDementhon(), computeResidualDementhon(), vpColVector::dotProd(), vpPoint::get_oX(), vpPoint::get_oY(), vpPoint::get_oZ(), vpMatrix::getCol(), vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), listP, npt, vpArray2D< Type >::resize(), vpPoint::set_oX(), vpPoint::set_oY(), vpPoint::set_oZ(), vpColVector::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 672 of file vpPose.cpp.

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

void vpPose::poseLagrangeNonPlan ( vpHomogeneousMatrix & cMo )

compute the pose using Lagrange approach (non planar object)

Definition at line 465 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 vpColVector::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 246 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 vpColVector::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 284 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