#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 , DEMENTHON_LAGRANGE_VIRTUAL_VS }

enum	RANSAC_FILTER_FLAGS { NO_FILTER , PREFILTER_DEGENERATE_POINTS , CHECK_DEGENERATE_POINTS }

Public Member Functions
	vpPose ()

	vpPose (const std::vector< vpPoint > &lP)

virtual	~vpPose ()

void	addPoint (const vpPoint &P)

void	addPoints (const std::vector< vpPoint > &lP)

void	clearPoint ()

bool	computePose (vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)

bool	computePoseDementhonLagrangeVVS (vpHomogeneousMatrix &cMo)

double	computeResidual (const vpHomogeneousMatrix &cMo) const

double	computeResidual (const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam) const

double	computeResidual (const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, vpColVector &squaredResidual) const

bool	coplanar (int &coplanar_plane_type, double p_a=nullptr, double p_b=nullptr, double p_c=nullptr, double p_d=nullptr)

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

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

void	poseDementhonPlan (vpHomogeneousMatrix &cMo)

void	poseDementhonNonPlan (vpHomogeneousMatrix &cMo)

void	poseLagrangePlan (vpHomogeneousMatrix &cMo, bool p_isPlan=nullptr, double p_a=nullptr, double p_b=nullptr, double p_c=nullptr, double *p_d=nullptr)

void	poseLagrangeNonPlan (vpHomogeneousMatrix &cMo)

void	poseLowe (vpHomogeneousMatrix &cMo)

bool	poseRansac (vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)

void	poseVirtualVSrobust (vpHomogeneousMatrix &cMo)

void	poseVirtualVS (vpHomogeneousMatrix &cMo)

void	printPoint ()

void	setDementhonSvThreshold (const double &svThresh)

void	setDistanceToPlaneForCoplanarityTest (double d)

void	setLambda (double lambda)

void	setVvsEpsilon (const double eps)

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

void	setRansacFilterFlag (const RANSAC_FILTER_FLAGS &flag)

int	getNbParallelRansacThreads () const

void	setNbParallelRansacThreads (int nb)

bool	getUseParallelRansac () const

void	setUseParallelRansac (bool use)

std::vector< vpPoint >	getPoints () const

Static Public Member Functions
static bool	computePlanarObjectPoseFromRGBD (const vpImage< float > &depthMap, const std::vector< vpImagePoint > &corners, const vpCameraParameters &colorIntrinsics, const std::vector< vpPoint > &point3d, vpHomogeneousMatrix &cMo, double *confidence_index=nullptr)

static bool	computePlanarObjectPoseFromRGBD (const vpImage< float > &depthMap, const std::vector< std::vector< vpImagePoint > > &corners, const vpCameraParameters &colorIntrinsics, const std::vector< std::vector< vpPoint > > &point3d, vpHomogeneousMatrix &cMo, double *confidence_index=nullptr, bool coplanar_points=true)

static int	computeRansacIterations (double probability, double epsilon, const int sampleSize=4, int maxIterations=2000)

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 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, bool useParallelRansac=true, unsigned int nthreads=0, bool(*func)(const vpHomogeneousMatrix &)=nullptr)

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

Public Attributes
unsigned int	npt

std::list< vpPoint >	listP

double	residual

Protected Member Functions
double	computeResidualDementhon (const vpHomogeneousMatrix &cMo)

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

Protected Attributes
double	m_lambda

double	m_dementhonSvThresh

Detailed Description

Class used for pose computation from N points (pose from point only). Some of the algorithms implemented in this class are described in [34].

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, servoAfma6FourPoints2DCamVelocityLs_cur.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoViper650FourPoints2DArtVelocityLs_cur.cpp, servoViper650FourPoints2DCamVelocityLs_cur-SR300.cpp, servoViper650FourPoints2DCamVelocityLs_cur.cpp, servoViper850FourPoints2DArtVelocityLs_cur.cpp, servoViper850FourPoints2DCamVelocityLs_cur.cpp, servoViper850FourPointsKinect.cpp, testAprilTag.cpp, testKeyPoint-4.cpp, testPose.cpp, testPoseRansac.cpp, testRobotAfma6Pose.cpp, and testRobotViper850Pose.cpp.

Definition at line 78 of file vpPose.h.

Member Enumeration Documentation

◆ RANSAC_FILTER_FLAGS

enum vpPose::RANSAC_FILTER_FLAGS

Filter applied in Ransac

Enumerator
NO_FILTER	No filter is applied.
PREFILTER_DEGENERATE_POINTS	Remove degenerate points (same 3D or 2D coordinates) before the RANSAC.
CHECK_DEGENERATE_POINTS	Check for degenerate points during the RANSAC.

Definition at line 108 of file vpPose.h.

◆ vpPoseMethodType

enum vpPose::vpPoseMethodType

Methods that could be used to estimate the pose from points.

Enumerator
LAGRANGE	Linear Lagrange approach (doesn't need an initialization)
DEMENTHON	Linear Dementhon aproach (doesn't need an initialization)
LOWE	Lowe aproach based on a Levenberg Marquartd non linear minimization scheme that needs an initialization from Lagrange or Dementhon aproach
RANSAC	Robust Ransac aproach (doesn't need an initialization)
LAGRANGE_LOWE	Non linear Lowe aproach initialized by Lagrange approach
DEMENTHON_LOWE	Non linear Lowe aproach initialized by Dementhon approach
VIRTUAL_VS	Non linear virtual visual servoing approach that needs an initialization from Lagrange or Dementhon aproach
DEMENTHON_VIRTUAL_VS	Non linear virtual visual servoing approach initialized by Dementhon approach
LAGRANGE_VIRTUAL_VS	Non linear virtual visual servoing approach initialized by Lagrange approach
DEMENTHON_LAGRANGE_VIRTUAL_VS	Non linear virtual visual servoing approach initialized by either Dementhon or Lagrange approach, depending on which method has the smallest residual.

Definition at line 82 of file vpPose.h.

Constructor & Destructor Documentation

◆ vpPose() [1/2]

vpPose::vpPose ( )

Default constructor.

Definition at line 54 of file vpPose.cpp.

◆ vpPose() [2/2]

vpPose::vpPose ( const std::vector< vpPoint > & lP )

Constructor from a vector of points.

Definition at line 63 of file vpPose.cpp.

◆ ~vpPose()

vpPose::~vpPose ( )

virtual

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

Definition at line 73 of file vpPose.cpp.

References listP.

Member Function Documentation

◆ addPoint()

void vpPose::addPoint ( const vpPoint & P )

Add a new point in the array of points.

Parameters

P	: Point to add in the array of point.

Warning: Considering a point from the class vpPoint, oX, oY, and oZ will represent the 3D coordinates of the point in the object frame and x and y its 2D coordinates in the image plane. These 5 fields must be initialized to be used within this function.

Examples: AROgre.cpp, AROgreBasic.cpp, poseVirtualVS.cpp, servoAfma62DhalfCamVelocity.cpp, servoAfma6FourPoints2DCamVelocityLs_cur.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoViper650FourPoints2DArtVelocityLs_cur.cpp, servoViper650FourPoints2DCamVelocityLs_cur-SR300.cpp, servoViper650FourPoints2DCamVelocityLs_cur.cpp, servoViper850FourPoints2DArtVelocityLs_cur.cpp, servoViper850FourPoints2DCamVelocityLs_cur.cpp, servoViper850FourPointsKinect.cpp, testKeyPoint-4.cpp, testPose.cpp, testPoseRansac.cpp, testRobotAfma6Pose.cpp, and testRobotViper850Pose.cpp.

Definition at line 93 of file vpPose.cpp.

References listP, and npt.

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

◆ addPoints()

void vpPose::addPoints ( const std::vector< vpPoint > & lP )

Add (append) a list of points in the array of points.

Parameters

lP	: List of points to add (append).

Warning: Considering a point from the class vpPoint, oX, oY, and oZ will represent the 3D coordinates of the point in the object frame and x and y its 2D coordinates in the image plane. These 5 fields must be initialized to be used within this function.

Definition at line 100 of file vpPose.cpp.

References listP, and npt.

Referenced by computePlanarObjectPoseFromRGBD().

◆ calculArbreDementhon()

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

protected

Method used in poseDementhonPlan() Return 0 if success, -1 if failure.

Definition at line 289 of file vpPoseDementhon.cpp.

References computeResidualDementhon(), vpMath::deg(), vpHomogeneousMatrix::extract(), and npt.

Referenced by poseDementhonPlan().

◆ clearPoint()

void vpPose::clearPoint ( )

Delete the array of point

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

Definition at line 86 of file vpPose.cpp.

References listP, and npt.

Referenced by vpMbTracker::initClick().

◆ computePlanarObjectPoseFromRGBD() [1/2]

bool vpPose::computePlanarObjectPoseFromRGBD	(	const vpImage< float > &	depthMap,
		const std::vector< std::vector< vpImagePoint > > &	corners,
		const vpCameraParameters &	colorIntrinsics,
		const std::vector< std::vector< vpPoint > > &	point3d,
		vpHomogeneousMatrix &	cMo,
		double *	confidence_index = `nullptr`,
		bool	coplanar_points = `true`
	)

static

Compute the pose of multiple planar object from corresponding 2D-3D point coordinates and depth map. Depth map is here used to estimate the 3D plane of each planar object.

This implementation is reserved for the case where multiple planar objects are considered and where an object like robot arm obstruct the view and interfere with plane equation estimation for each single object. Therefore this function considers only the 3D point inside the visible tags.

Parameters

[in]	depthMap	: Depth map aligned to the color image from where corners are extracted.
[in]	corners	: Vector where each element is a vector containing 2D pixel coordinates of the 2D polygon that defines the object edges in an image.
[in]	colorIntrinsics	: Camera parameters used to convert corners from pixel to meters.
[in]	point3d	: Vector where each element is a vector containing 3D points coordinates of the 3D polygon that defines the model of the planar object.
[out]	cMo	: Computed pose.
[out]	confidence_index	: Confidence index in range [0, 1]. When values are close to 1, it means that pose estimation confidence is high. Values close to 0 indicate that pose is not well estimated. This confidence index corresponds to the product between the normalized number of depth data covering the tag and the normalized M-estimator weights returned by the robust estimation of the tag 3D plane.
[in]	coplanar_points	: There are cases where all the planar objects are not in the same plane. In order to differentiate these cases, this parameter will be used to compute the common plane for all objects if its value is true and compute the plane individually for each object otherwise.

Returns: true if pose estimation succeed, false otherwise.

Definition at line 237 of file vpPoseRGBD.cpp.

References addPoints(), vpHomogeneousMatrix::compute3d3dTransformation(), computePose(), vpPlane::computeZ(), vpPixelMeterConversion::convertPoint(), vpException::fatalError, vpImagePoint::get_u(), vpImagePoint::get_v(), vpPolygon::getArea(), vpRect::getBottom(), vpPolygon::getBoundingBox(), vpImage< Type >::getHeight(), vpRect::getLeft(), vpRect::getRight(), vpRect::getTop(), vpImage< Type >::getWidth(), vpImage< Type >::insert(), vpPolygon::isInside(), vpHomogeneousMatrix::isValid(), and VIRTUAL_VS.

◆ computePlanarObjectPoseFromRGBD() [2/2]

bool vpPose::computePlanarObjectPoseFromRGBD	(	const vpImage< float > &	depthMap,
		const std::vector< vpImagePoint > &	corners,
		const vpCameraParameters &	colorIntrinsics,
		const std::vector< vpPoint > &	point3d,
		vpHomogeneousMatrix &	cMo,
		double *	confidence_index = `nullptr`
	)

static

Compute the pose of a planar object from corresponding 2D-3D point coordinates and depth map. Depth map is here used to estimate the 3D plane of the object.

Parameters

[in]	depthMap	: Depth map aligned to the color image from where corners are extracted.
[in]	corners	: Vector of 2D pixel coordinates of the object in an image.
[in]	colorIntrinsics	: Camera parameters used to convert corners from pixel to meters.
[in]	point3d	: Vector of 3D points corresponding to the model of the planar object.
[out]	cMo	: Computed pose.
[out]	confidence_index	: Confidence index in range [0, 1]. When values are close to 1, it means that pose estimation confidence is high. Values close to 0 indicate that pose is not well estimated. This confidence index corresponds to the product between the normalized number of depth data covering the tag and the normalized M-estimator weights returned by the robust estimation of the tag 3D plane.

The following code snippet implemented in tutorial-apriltag-detector-live-rgbd-realsense.cpp shows how to use this function to estimate the pose of an AprilTag using this method:

      std::vector<std::vector<vpImagePoint> > tags_corners = detector.getPolygon();
      std::vector<int> tags_id = detector.getTagsId();
      std::map<int, double> tags_size;
      tags_size[-1] = tagSize; // Default tag size
      std::vector<std::vector<vpPoint> > tags_points3d = detector.getTagsPoints3D(tags_id, tags_size);
      for (size_t i = 0; i < tags_corners.size(); i++) {
        vpHomogeneousMatrix cMo;
        double confidence_index;
        if (vpPose::computePlanarObjectPoseFromRGBD(depthMap, tags_corners[i], cam, tags_points3d[i], cMo,
                                                    &confidence_index)) {
          if (confidence_index > 0.5) {
            vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::none, 3);
          } else if (confidence_index > 0.25) {
            vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::orange, 3);
          } else {
            vpDisplay::displayFrame(I_color2, cMo, cam, tagSize / 2, vpColor::red, 3);
          }
          std::stringstream ss;
          ss << "Tag id " << tags_id[i] << " confidence: " << confidence_index;
          vpDisplay::displayText(I_color2, 35 + i * 15, 20, ss.str(), vpColor::red);
        }
      }

Returns: true if pose estimation succeed, false otherwise.

Examples: tutorial-apriltag-detector-live-rgbd-realsense.cpp, and tutorial-apriltag-detector-live-rgbd-structure-core.cpp.

Definition at line 147 of file vpPoseRGBD.cpp.

References addPoints(), vpHomogeneousMatrix::compute3d3dTransformation(), computePose(), vpPlane::computeZ(), vpPixelMeterConversion::convertPoint(), vpException::fatalError, vpImagePoint::get_u(), vpImagePoint::get_v(), vpPolygon::getArea(), vpRect::getBottom(), vpPolygon::getBoundingBox(), vpImage< Type >::getHeight(), vpRect::getLeft(), vpRect::getRight(), vpRect::getTop(), vpImage< Type >::getWidth(), vpPolygon::isInside(), vpHomogeneousMatrix::isValid(), and VIRTUAL_VS.

◆ computePose()

bool vpPose::computePose	(	vpPoseMethodType	method,
		vpHomogeneousMatrix &	cMo,
		bool(*)(const vpHomogeneousMatrix &)	func = `nullptr`
	)

Compute the pose according to the desired method which are:

vpPose::LAGRANGE: Linear Lagrange approach (test is done to switch between planar and non planar algorithm)
vpPose::DEMENTHON: Linear Dementhon approach (test is done to switch between planar and non planar algorithm)
vpPose::LOWE: Lowe aproach based on a Levenberg Marquartd non linear minimization scheme that needs an initialization from Lagrange or Dementhon aproach
vpPose::LAGRANGE_LOWE: Non linear Lowe aproach initialized by Lagrange approach
vpPose::DEMENTHON_LOWE: Non linear Lowe aproach initialized by Dementhon approach
vpPose::VIRTUAL_VS: Non linear virtual visual servoing approach that needs an initialization from Lagrange or Dementhon aproach
vpPose::DEMENTHON_VIRTUAL_VS: Non linear virtual visual servoing approach initialized by Dementhon approach
vpPose::LAGRANGE_VIRTUAL_VS: Non linear virtual visual servoing approach initialized by Lagrange approach
vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS: Non linear virtual visual servoing approach initialized by either Dementhon or Lagrange approach, depending on which method has the smallest residual.
vpPose::RANSAC: Robust Ransac aproach (doesn't need an initialization)

Examples: AROgre.cpp, AROgreBasic.cpp, poseVirtualVS.cpp, servoAfma62DhalfCamVelocity.cpp, servoAfma6FourPoints2DCamVelocityLs_cur.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoViper650FourPoints2DArtVelocityLs_cur.cpp, servoViper650FourPoints2DCamVelocityLs_cur-SR300.cpp, servoViper650FourPoints2DCamVelocityLs_cur.cpp, servoViper850FourPoints2DArtVelocityLs_cur.cpp, servoViper850FourPoints2DCamVelocityLs_cur.cpp, servoViper850FourPointsKinect.cpp, testKeyPoint-4.cpp, testPose.cpp, testPoseRansac.cpp, testRobotAfma6Pose.cpp, and testRobotViper850Pose.cpp.

Definition at line 333 of file vpPose.cpp.

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

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

◆ computePoseDementhonLagrangeVVS()

bool vpPose::computePoseDementhonLagrangeVVS ( vpHomogeneousMatrix & cMo )

Method that first computes the pose cMo using the linear approaches of Dementhon and Lagrange and then uses the non-linear Virtual Visual Servoing approach to affine the pose which had the lowest residual.

Parameters

cMo	the pose of the object with regard to the camera.

Returns: true the pose computation was successful.; false an error occurred during the pose computation.

Definition at line 437 of file vpPose.cpp.

References computePose(), computeResidual(), coplanar(), poseDementhonNonPlan(), poseDementhonPlan(), poseLagrangeNonPlan(), poseLagrangePlan(), and VIRTUAL_VS.

Referenced by computePose().

◆ computeRansacIterations()

int vpPose::computeRansacIterations	(	double	probability,
		double	epsilon,
		const int	sampleSize = `4`,
		int	maxIterations = `2000`
	)

static

Compute the number of RANSAC iterations to ensure with a probability p that at least one of the random samples of s points is free from outliers.

Note: See: Hartley and Zisserman, Multiple View Geometry in Computer Vision, p119 (2. How many samples?).

Parameters

probability	: Probability that at least one of the random samples is free from outliers (typically p=0.99).
epsilon	: Probability that a selected point is an outlier (between 0 and 1).
sampleSize	: Minimum number of points to estimate the model (4 for a pose estimation).
maxIterations	: Upper bound on the number of iterations or -1 for INT_MAX.

Returns: The number of RANSAC iterations to ensure with a probability p that at least one of the random samples of s points is free from outliers or maxIterations if it exceeds the desired upper bound or INT_MAX if maxIterations=-1.

Definition at line 495 of file vpPoseRansac.cpp.

References vpMath::nul().

◆ computeResidual() [1/3]

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

Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.

Parameters

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

Returns: The value of the sum of squared residuals in meter^2.

Note

There is also the possibility to compute the residual expressed in pixel^2 using one of the following methods:

vpPose::computeResidual(const vpHomogeneousMatrix &, const vpCameraParameters &)
vpPose::computeResidual(const vpHomogeneousMatrix &, const vpCameraParameters &am, vpColVector &)

Examples: testPose.cpp.

Definition at line 285 of file vpPose.cpp.

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

Referenced by computePoseDementhonLagrangeVVS(), and computeResidual().

◆ computeResidual() [2/3]

double vpPose::computeResidual	(	const vpHomogeneousMatrix &	cMo,
		const vpCameraParameters &	cam
	)		const

Compute and return the sum of squared residuals expressed in pixel^2 for the pose matrix cMo.

Parameters

cMo	: Input pose. The matrix that defines the pose to be tested.
cam	: Camera parameters used to observe the points.

Returns: The value of the sum of squared residuals in pixel^2.

Note: There is also the possibility to compute the residual expressed in meter^2 using vpPose::computeResidual(const vpHomogeneousMatrix &)

Definition at line 301 of file vpPose.cpp.

References computeResidual().

◆ computeResidual() [3/3]

double vpPose::computeResidual	(	const vpHomogeneousMatrix &	cMo,
		const vpCameraParameters &	cam,
		vpColVector &	squaredResidual
	)		const

Compute and return the sum of squared residuals expressed in pixel^2 for the pose matrix cMo.

Parameters

cMo	: Input pose. The matrix that defines the pose to be tested.
cam	: Camera parameters used to observe the points.
squaredResidual	Input/output vector that will be resized and will contain the squared residuals expressed in pixel^2 of each point.

Returns: The value of the sum of squared residuals in pixel^2.

Note: There is also the possibility to compute the residual expressed in meter^2 using vpPose::computeResidual(const vpHomogeneousMatrix &)

Definition at line 307 of file vpPose.cpp.

References vpMeterPixelConversion::convertPoint(), vpPoint::get_x(), vpPoint::get_y(), listP, vpColVector::resize(), vpMath::sqr(), and vpForwardProjection::track().

◆ computeResidualDementhon()

double vpPose::computeResidualDementhon ( const vpHomogeneousMatrix & cMo )

protected

Compute and return the residual corresponding to the sum of squared residuals in meter^2 for the pose matrix cMo.

Parameters

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

Returns: the value of the sum of squared residuals in meter^2.

Definition at line 563 of file vpPoseDementhon.cpp.

References npt, and vpMath::sqr().

Referenced by calculArbreDementhon(), poseDementhonNonPlan(), and poseDementhonPlan().

◆ coplanar()

bool vpPose::coplanar	(	int &	coplanar_plane_type,
		double *	p_a = `nullptr`,
		double *	p_b = `nullptr`,
		double *	p_c = `nullptr`,
		double *	p_d = `nullptr`
	)

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
p_a	if different from null, it will be set to equal the a coefficient of the potential plan.
p_b	if different from null, it will be set to equal the b coefficient of the potential plan.
p_c	if different from null, it will be set to equal the c coefficient of the potential plan.
p_d	if different from null, it will be set to equal the d coefficient of the potential plan.

Returns: true if points are coplanar false otherwise.

Definition at line 117 of file vpPose.cpp.

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

Referenced by computePose(), computePoseDementhonLagrangeVVS(), and poseLagrangePlan().

◆ display() [1/2]

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

static

Display in the image I the pose represented by its homogenous transformation cMo as a 3 axis frame.

Parameters

I	Image where the pose is displayed in overlay.
cMo	Considered pose to display.
cam	Camera parameters associated to image I.
size	length in meter of the axis that will be displayed.
col	Color used to display the 3 axis. If vpColor::none, red, green and blue will represent x-axis, y-axis and z-axis respectively.

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

Definition at line 540 of file vpPose.cpp.

References vpDisplay::displayFrame().

◆ display() [2/2]

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

static

Display in the image I the pose represented by its homogenous transformation cMo as a 3 axis frame.

Parameters

I	Image where the pose is displayed in overlay.
cMo	Considered pose to display.
cam	Camera parameters associated to image I.
size	length in meter of the axis that will be displayed.
col	Color used to display the 3 axis. If vpColor::none, red, green and blue will represent x-axis, y-axis and z-axis respectively.

Definition at line 546 of file vpPose.cpp.

References vpDisplay::displayFrame().

◆ displayModel() [1/2]

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

Display the coordinates of the points in the image plane that are used to compute the pose in image I.

Definition at line 551 of file vpPose.cpp.

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

◆ displayModel() [2/2]

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

Display the coordinates of the points in the image plane that are used to compute the pose in image I.

Definition at line 565 of file vpPose.cpp.

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

◆ findMatch()

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`,
		bool	useParallelRansac = `true`,
		unsigned int	nthreads = `0`,
		bool(*)(const vpHomogeneousMatrix &)	func = `nullptr`
	)

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 correspondence 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.
useParallelRansac	: If true, use parallel RANSAC version (if C++11 is available).
nthreads	: Number of threads to use, if 0 the number of CPU threads will be determined.
func	: Pointer to a function that takes in parameter a vpHomogeneousMatrix and returns true if the pose check is OK or false otherwise

Examples: testFindMatch.cpp.

Definition at line 533 of file vpPoseRansac.cpp.

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

◆ getCovarianceMatrix()

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 439 of file vpPose.h.

References vpTRACE.

Referenced by vpKeyPoint::computePose().

◆ getNbParallelRansacThreads()

int vpPose::getNbParallelRansacThreads ( ) const

inline

Get the number of threads for the parallel RANSAC implementation.

See also: setNbParallelRansacThreads

Definition at line 466 of file vpPose.h.

◆ getPoints()

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

inline

Get the vector of points.

Returns: The vector of points.

Definition at line 497 of file vpPose.h.

◆ getRansacInlierIndex()

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

inline

Get the vector of indexes corresponding to inliers.

Definition at line 415 of file vpPose.h.

Referenced by vpKeyPoint::computePose().

◆ getRansacInliers()

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

inline

Get the vector of inliers.

Examples: testPoseRansac.cpp.

Definition at line 420 of file vpPose.h.

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

◆ getRansacNbInliers()

unsigned int vpPose::getRansacNbInliers ( ) const

inline

Get the number of inliers.

Definition at line 410 of file vpPose.h.

Referenced by findMatch().

◆ getUseParallelRansac()

bool vpPose::getUseParallelRansac ( ) const

inline

Returns: True if the parallel RANSAC version should be used (depends also to C++11 availability).

See also: setUseParallelRansac

Definition at line 483 of file vpPose.h.

◆ poseDementhonNonPlan()

void vpPose::poseDementhonNonPlan ( vpHomogeneousMatrix & cMo )

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 [14].

Definition at line 97 of file vpPoseDementhon.cpp.

References computeResidualDementhon(), vpMath::deg(), vpHomogeneousMatrix::extract(), vpException::fatalError, vpPoint::get_oX(), vpPoint::get_oY(), vpPoint::get_oZ(), listP, m_dementhonSvThresh, npt, vpMatrix::pseudoInverse(), vpPoint::set_oX(), vpPoint::set_oY(), and vpPoint::set_oZ().

Referenced by computePose(), and computePoseDementhonLagrangeVVS().

◆ poseDementhonPlan()

void vpPose::poseDementhonPlan ( vpHomogeneousMatrix & cMo )

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

Definition at line 412 of file vpPoseDementhon.cpp.

References calculArbreDementhon(), computeResidualDementhon(), vpMath::deg(), vpHomogeneousMatrix::extract(), vpException::fatalError, vpPoint::get_oX(), vpPoint::get_oY(), vpPoint::get_oZ(), listP, m_dementhonSvThresh, npt, vpMatrix::pseudoInverse(), vpPoint::set_oX(), vpPoint::set_oY(), vpPoint::set_oZ(), and vpColVector::t().

Referenced by computePose(), and computePoseDementhonLagrangeVVS().

◆ poseFromRectangle()

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 intrinsic parameters, the length on x axis is known but the proportion 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 intrinsic 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 579 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().

◆ poseLagrangeNonPlan()

void vpPose::poseLagrangeNonPlan ( vpHomogeneousMatrix & cMo )

Compute the pose of a non planar object using Lagrange approach.

Parameters

cMo	: Estimated pose. No initialisation is requested to estimate cMo.

Definition at line 455 of file vpPoseLagrange.cpp.

References vpException::dimensionError, 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(), and computePoseDementhonLagrangeVVS().

◆ poseLagrangePlan()

void vpPose::poseLagrangePlan	(	vpHomogeneousMatrix &	cMo,
		bool *	p_isPlan = `nullptr`,
		double *	p_a = `nullptr`,
		double *	p_b = `nullptr`,
		double *	p_c = `nullptr`,
		double *	p_d = `nullptr`
	)

Compute the pose of a planar object using Lagrange approach.

Parameters

cMo	: Estimated pose. No initialisation is requested to estimate cMo.
p_isPlan	: if different from nullptr, indicates if the object is planar or not.
p_a	: if different from nullptr, the a coefficient of the plan formed by the points.
p_b	: if different from nullptr, the b coefficient of the plan formed by the points.
p_c	: if different from nullptr, the c coefficient of the plan formed by the points.
p_d	: if different from nullptr, the d coefficient of the plan formed by the points.

Definition at line 246 of file vpPoseLagrange.cpp.

References coplanar(), vpColVector::crossProd(), vpException::divideByZeroError, vpException::fatalError, vpPoint::get_oX(), vpPoint::get_oY(), vpPoint::get_oZ(), vpPoint::get_x(), vpPoint::get_y(), listP, npt, and vpColVector::sumSquare().

Referenced by computePose(), and computePoseDementhonLagrangeVVS().

◆ poseLowe()

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.

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

Definition at line 251 of file vpPoseLowe.cpp.

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

Referenced by computePose().

◆ poseRansac()

bool vpPose::poseRansac	(	vpHomogeneousMatrix &	cMo,
		bool(*)(const vpHomogeneousMatrix &)	func = `nullptr`
	)

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.

Note: You can enable a multithreaded version if you have C++11 enabled using setUseParallelRansac(). The number of threads used can then be set with setNbParallelRansacThreads(). Filter flag can be used with setRansacFilterFlag().

Definition at line 291 of file vpPoseRansac.cpp.

References addPoint(), CHECK_DEGENERATE_POINTS, computePose(), DEMENTHON_LAGRANGE_VIRTUAL_VS, listP, vpPoseException::notInitializedError, PREFILTER_DEGENERATE_POINTS, and setCovarianceComputation().

Referenced by computePose().

◆ poseVirtualVS()

void vpPose::poseVirtualVS ( vpHomogeneousMatrix & cMo )

Compute the pose using virtual visual servoing approach.

This approach is described in [31].

Definition at line 45 of file vpPoseVirtualVisualServoing.cpp.

References vpMatrix::computeCovarianceMatrixVVS(), vpExponentialMap::direct(), vpPoint::get_x(), vpPoint::get_y(), vpPoint::get_Z(), vpHomogeneousMatrix::inverse(), listP, m_lambda, vpColVector::sumSquare(), vpForwardProjection::track(), and vpERROR_TRACE.

Referenced by computePose().

◆ poseVirtualVSrobust()

void vpPose::poseVirtualVSrobust ( vpHomogeneousMatrix & cMo )

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

This approach is described in [8].

Definition at line 143 of file vpPoseVirtualVisualServoing.cpp.

References vpMatrix::computeCovarianceMatrix(), vpExponentialMap::direct(), vpPoint::get_x(), vpPoint::get_y(), vpPoint::get_Z(), vpArray2D< Type >::getRows(), vpHomogeneousMatrix::inverse(), listP, m_lambda, vpRobust::MEstimator(), vpColVector::resize(), vpArray2D< Type >::resize(), vpRobust::setMinMedianAbsoluteDeviation(), vpMath::sqr(), vpColVector::sumSquare(), vpForwardProjection::track(), vpRobust::TUKEY, and vpERROR_TRACE.

◆ printPoint()

void vpPose::printPoint ( )

Print to std::cout points used as input.

Definition at line 528 of file vpPose.cpp.

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

◆ setCovarianceComputation()

void vpPose::setCovarianceComputation ( const bool & flag )

inline

Set if the covariance matrix has to be computed in the Virtual Visual Servoing approach.

Parameters

flag	: True if the covariance has to be computed, false otherwise.

Definition at line 428 of file vpPose.h.

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

◆ setDementhonSvThreshold()

void vpPose::setDementhonSvThreshold ( const double & svThresh )

Set singular value threshold in Dementhon pose estimation method.

Definition at line 109 of file vpPose.cpp.

References vpException::badValue, and m_dementhonSvThresh.

◆ setDistanceToPlaneForCoplanarityTest()

void vpPose::setDistanceToPlaneForCoplanarityTest ( double d )

Set distance threshold to consider that when a point belongs to a plane.

Definition at line 107 of file vpPose.cpp.

◆ setLambda()

void vpPose::setLambda ( double lambda )

inline

Set virtual visual servoing gain.

Definition at line 363 of file vpPose.h.

◆ setNbParallelRansacThreads()

void vpPose::setNbParallelRansacThreads ( int nb )

inline

Set the number of threads for the parallel RANSAC implementation.

Note: You have to enable the parallel version with setUseParallelRansac(). If the number of threads is 0, the number of threads to use is automatically determined with C++11.

See also: setUseParallelRansac

Definition at line 476 of file vpPose.h.

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

◆ setRansacFilterFlag()

void vpPose::setRansacFilterFlag ( const RANSAC_FILTER_FLAGS & flag )

inline

Set RANSAC filter flag.

Parameters

flag	: RANSAC flag to use to prefilter or perform degenerate configuration check.

See also: RANSAC_FILTER_FLAGS

Warning: Prefilter degenerate points consists to not add subsequent degenerate points. This means that it is possible to discard a valid point and keep an invalid point if the invalid point is added first. It is faster to prefilter for duplicate points instead of checking for degenerate configuration at each time.

Note: By default the flag is set to NO_FILTER.

Definition at line 459 of file vpPose.h.

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

◆ setRansacMaxTrials()

void vpPose::setRansacMaxTrials ( const int & rM )

inline

Set Ransac number of trials.

Examples: testKeyPoint-4.cpp.

Definition at line 405 of file vpPose.h.

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

◆ setRansacNbInliersToReachConsensus()

void vpPose::setRansacNbInliersToReachConsensus ( const unsigned int & nbC )

inline

Set Ransac requested number of inliers to reach consensus.

Examples: testKeyPoint-4.cpp, testPose.cpp, and testPoseRansac.cpp.

Definition at line 386 of file vpPose.h.

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

◆ setRansacThreshold()

void vpPose::setRansacThreshold ( const double & t )

inline

Set Ransac threshold.

Examples: testKeyPoint-4.cpp, testPose.cpp, and testPoseRansac.cpp.

Definition at line 391 of file vpPose.h.

References vpException::badValue.

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

◆ setUseParallelRansac()

void vpPose::setUseParallelRansac ( bool use )

inline

Set if parallel RANSAC version should be used or not (only if C++11).

Note: Need C++11 or higher.

Definition at line 490 of file vpPose.h.

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

◆ setVvsEpsilon()

void vpPose::setVvsEpsilon ( const double eps )

inline

Set virtual visual servoing epsilon value used in the pseudo-inverse.

Definition at line 368 of file vpPose.h.

References vpException::badValue.

◆ setVvsIterMax()

void vpPose::setVvsIterMax ( int nb )

inline

Set virtual visual servoing pose estimator maximum number od iterations.

Definition at line 381 of file vpPose.h.

Member Data Documentation

◆ listP

std::list<vpPoint> vpPose::listP

Array of point (use here class vpPoint)

Examples: testPose.cpp.

Definition at line 116 of file vpPose.h.

Referenced by addPoint(), addPoints(), clearPoint(), computeResidual(), displayModel(), findMatch(), poseDementhonNonPlan(), poseDementhonPlan(), poseLagrangeNonPlan(), poseLagrangePlan(), poseLowe(), poseRansac(), poseVirtualVS(), poseVirtualVSrobust(), printPoint(), and ~vpPose().

◆ m_dementhonSvThresh

double vpPose::m_dementhonSvThresh

protected

SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.

Definition at line 767 of file vpPose.h.

Referenced by poseDementhonNonPlan(), poseDementhonPlan(), and setDementhonSvThreshold().

◆ m_lambda

double vpPose::m_lambda

protected

Parameters use for the virtual visual servoing approach.

Definition at line 766 of file vpPose.h.

Referenced by poseVirtualVS(), and poseVirtualVSrobust().

◆ npt

unsigned int vpPose::npt

Number of point used in pose computation.

Examples: testKeyPoint-4.cpp.

Definition at line 115 of file vpPose.h.

Referenced by addPoint(), addPoints(), calculArbreDementhon(), clearPoint(), computePose(), computeResidualDementhon(), coplanar(), poseDementhonNonPlan(), poseDementhonPlan(), poseLagrangeNonPlan(), poseLagrangePlan(), and poseLowe().

◆ residual

double vpPose::residual

Residual in meter.

Definition at line 118 of file vpPose.h.

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Detailed Description

Member Enumeration Documentation

◆ RANSAC_FILTER_FLAGS

◆ vpPoseMethodType

Constructor & Destructor Documentation

◆ vpPose() [1/2]

◆ vpPose() [2/2]

◆ ~vpPose()

Member Function Documentation

◆ addPoint()

◆ addPoints()

◆ calculArbreDementhon()

◆ clearPoint()

◆ computePlanarObjectPoseFromRGBD() [1/2]

◆ computePlanarObjectPoseFromRGBD() [2/2]

◆ computePose()

◆ computePoseDementhonLagrangeVVS()

◆ computeRansacIterations()

◆ computeResidual() [1/3]

◆ computeResidual() [2/3]

◆ computeResidual() [3/3]

◆ computeResidualDementhon()

◆ coplanar()

◆ display() [1/2]

◆ display() [2/2]

◆ displayModel() [1/2]

◆ displayModel() [2/2]

◆ findMatch()

◆ getCovarianceMatrix()

◆ getNbParallelRansacThreads()

◆ getPoints()

◆ getRansacInlierIndex()

◆ getRansacInliers()

◆ getRansacNbInliers()

◆ getUseParallelRansac()

◆ poseDementhonNonPlan()

◆ poseDementhonPlan()

◆ poseFromRectangle()

◆ poseLagrangeNonPlan()

◆ poseLagrangePlan()

◆ poseLowe()

◆ poseRansac()

◆ poseVirtualVS()

◆ poseVirtualVSrobust()

◆ printPoint()

◆ setCovarianceComputation()

◆ setDementhonSvThreshold()

◆ setDistanceToPlaneForCoplanarityTest()

◆ setLambda()

◆ setNbParallelRansacThreads()

◆ setRansacFilterFlag()

◆ setRansacMaxTrials()

◆ setRansacNbInliersToReachConsensus()

◆ setRansacThreshold()

◆ setUseParallelRansac()

◆ setVvsEpsilon()

◆ setVvsIterMax()

Member Data Documentation

◆ listP

◆ m_dementhonSvThresh

◆ m_lambda

◆ npt

◆ residual