Visual Servoing Platform  version 3.6.1 under development (2024-05-26)

#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 setDistToPlaneForCoplanTest (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< vpPointgetRansacInliers () 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< vpPointgetPoints () 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)
 
template<typename DataId >
static std::optional< vpHomogeneousMatrixcomputePlanarObjectPoseWithAtLeast3Points (const vpPlane &plane_in_camera_frame, const std::map< DataId, vpPoint > &pts, const std::map< DataId, vpImagePoint > &ips, const vpCameraParameters &camera_intrinsics, std::optional< vpHomogeneousMatrix > cMo_init=std::nullopt, bool enable_vvs=true)
 
static std::optional< vpHomogeneousMatrixposeVirtualVSWithDepth (const std::vector< vpPoint > &points, const vpHomogeneousMatrix &cMo)
 

Public Attributes

unsigned int npt
 
std::list< vpPointlistP
 
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

Member Enumeration Documentation

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

◆ 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 81 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)
explicit

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)

◆ 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 300 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 244 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++) {
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);
if (opt_verbose) {
std::cout << "cMo[" << i << "]: \n" << cMo_vec[i] << std::endl;
std::cout << "cMo[" << i << "] using depth: \n" << cMo << std::endl;
}
}
}
static const vpColor red
Definition: vpColor.h:211
static const vpColor none
Definition: vpColor.h:223
static const vpColor orange
Definition: vpColor.h:221
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Implementation of an homogeneous matrix and operations on such kind of matrices.
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)
Definition: vpPoseRGBD.cpp:151
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 151 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.

◆ computePlanarObjectPoseWithAtLeast3Points()

template<typename DataId >
static std::optional<vpHomogeneousMatrix> vpPose::computePlanarObjectPoseWithAtLeast3Points ( const vpPlane plane_in_camera_frame,
const std::map< DataId, vpPoint > &  pts,
const std::map< DataId, vpImagePoint > &  ips,
const vpCameraParameters camera_intrinsics,
std::optional< vpHomogeneousMatrix cMo_init = std::nullopt,
bool  enable_vvs = true 
)
inlinestatic

Compute the pose of a planar object from corresponding 2D-3D point coordinates and plane equation. Here at least 3 points are required.

Parameters
[in]plane_in_camera_frame: Plane in camera frame.
[in]pts: Object points.
[in]ips: Points in the image.
[in]camera_intrinsics: Camera parameters.
[in]cMo_init: Camera to object frame transformation used as initialization. When set to std::nullopt, this transformation is computed internally.
[in]enable_vvs: When true, refine estimated pose using a virtual visual servoing scheme.
Returns
Homogeneous matrix ${^c}{\bf M}_o$ between camera frame and object frame when estimation succeed, nullopt otherwise.
Examples
tutorial-pose-from-planar-object.cpp.

Definition at line 691 of file vpPose.h.

References vpHomogeneousMatrix::compute3d3dTransformation(), vpPlane::computeZ(), vpPixelMeterConversion::convertPoint(), vpException::fatalError, vpHomogeneousMatrix::isValid(), poseVirtualVSWithDepth(), vpPoint::set_x(), vpPoint::set_y(), and vpPoint::set_Z().

◆ 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, testRobotViper850Pose.cpp, and tutorial-ukf.cpp.

Definition at line 341 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
cMothe 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 455 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 518 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 290 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 307 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.
squaredResidualInput/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 313 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 587 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_type1: 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_aif different from null, it will be set to equal the a coefficient of the potential plan.
p_bif different from null, it will be set to equal the b coefficient of the potential plan.
p_cif different from null, it will be set to equal the c coefficient of the potential plan.
p_dif 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
IImage where the pose is displayed in overlay.
cMoConsidered pose to display.
camCamera parameters associated to image I.
sizelength in meter of the axis that will be displayed.
colColor 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 559 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
IImage where the pose is displayed in overlay.
cMoConsidered pose to display.
camCamera parameters associated to image I.
sizelength in meter of the axis that will be displayed.
colColor 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 565 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 570 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 582 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 556 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 438 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 465 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 496 of file vpPose.h.

◆ getRansacInlierIndex()

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

Get the vector of indexes corresponding to inliers.

Definition at line 414 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 419 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 409 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 482 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 425 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,p4the image of the corners of the rectangle (respectively the image of (0,0),(lx,0),(lx,lx/s) and (0,lx/s)) (input)
camthe camera used (input)
lxthe rectangle size on the x axis (input)
cMothe camera pose (output)
Returns
int : OK if no pb occurs

Definition at line 594 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 448 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 243 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 256 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 313 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)

◆ poseVirtualVSrobust()

◆ poseVirtualVSWithDepth()

std::optional< vpHomogeneousMatrix > vpPose::poseVirtualVSWithDepth ( const std::vector< vpPoint > &  points,
const vpHomogeneousMatrix cMo 
)
static

Compute the pose by virtual visual servoing using x,y and Z point coordinates as visual features. We recall that x,y are the coordinates of a point in the image plane which are obtained by perspective projection, while Z is the 3D coordinate of the point along the camera frame Z-axis.

Parameters
[in]cMo: Pose initial value used to initialize the non linear pose estimation algorithm.
[in]points: A vector of points with [x,y,Z] values used as visual features.
Returns
Estimated pose when the minimization converged, of std::nullopt when it failed.

Definition at line 273 of file vpPoseVirtualVisualServoing.cpp.

References vpExponentialMap::direct(), vpHomogeneousMatrix::inverse(), vpArray2D< Type >::size(), vpMath::sqr(), and vpColVector::sumSquare().

Referenced by computePlanarObjectPoseWithAtLeast3Points().

◆ printPoint()

void vpPose::printPoint ( )

Print to std::cout points used as input.

Definition at line 546 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 427 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.

◆ setDistToPlaneForCoplanTest()

void vpPose::setDistToPlaneForCoplanTest ( 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 362 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 475 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 458 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 404 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 385 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 390 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 489 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 367 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 380 of file vpPose.h.

Member Data Documentation

◆ listP

◆ m_dementhonSvThresh

double vpPose::m_dementhonSvThresh
protected

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

Definition at line 766 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 765 of file vpPose.h.

Referenced by poseVirtualVS(), and poseVirtualVSrobust().

◆ npt

unsigned int vpPose::npt

◆ residual

double vpPose::residual

Residual in meter.

Definition at line 117 of file vpPose.h.