Visual Servoing Platform  version 3.5.1 under development (2022-07-01)

#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
}
 
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 &)=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 poseDementhonPlan (vpHomogeneousMatrix &cMo)
 
void poseDementhonNonPlan (vpHomogeneousMatrix &cMo)
 
void poseLagrangePlan (vpHomogeneousMatrix &cMo)
 
void poseLagrangeNonPlan (vpHomogeneousMatrix &cMo)
 
void poseLowe (vpHomogeneousMatrix &cMo)
 
bool poseRansac (vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
 
void poseVirtualVSrobust (vpHomogeneousMatrix &cMo)
 
void poseVirtualVS (vpHomogeneousMatrix &cMo)
 
void printPoint ()
 
void setDistanceToPlaneForCoplanarityTest (double d)
 
void setLambda (double a)
 
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
 
Deprecated functions
vp_deprecated void init ()
 

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=NULL)
 
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=NULL, 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 &)=NULL)
 
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 (std::vector< vpPoint > points, 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 lambda
 

Detailed Description

Member Enumeration Documentation

◆ RANSAC_FILTER_FLAGS

Enumerator
NO_FILTER 
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 112 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

Definition at line 93 of file vpPose.h.

Constructor & Destructor Documentation

◆ vpPose() [1/2]

vpPose::vpPose ( )

Default constructor.

Definition at line 96 of file vpPose.cpp.

◆ vpPose() [2/2]

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

Definition at line 105 of file vpPose.cpp.

◆ ~vpPose()

vpPose::~vpPose ( )
virtual

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

Definition at line 118 of file vpPose.cpp.

References listP.

Member Function Documentation

◆ addPoint()

void vpPose::addPoint ( const vpPoint newP)

Add a new point in the array of points.

Parameters
newP: New 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 148 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.
Examples
tutorial-chessboard-pose.cpp.

Definition at line 163 of file vpPose.cpp.

References listP, and npt.

Referenced by computePlanarObjectPoseFromRGBD().

◆ calculArbreDementhon()

int vpPose::calculArbreDementhon ( vpMatrix Ap,
vpColVector U,
vpHomogeneousMatrix cMo 
)
protected

Return 0 if success, -1 if failure.

Definition at line 301 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 133 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 = NULL,
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 invidually for each object otherwise.
Returns
true if pose estimation succeed, false otherwise.

Definition at line 291 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 = NULL 
)
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);
}
}
static const vpColor red
Definition: vpColor.h:217
static const vpColor none
Definition: vpColor.h:229
static const vpColor orange
Definition: vpColor.h:227
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))
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=NULL)
Definition: vpPoseRGBD.cpp:169
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 169 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 386 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 = NULL 
)

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::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-chessboard-pose.cpp.

Definition at line 373 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, and VIRTUAL_VS.

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

◆ 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 600 of file vpPoseRansac.cpp.

References vpMath::nul().

◆ computeResidual()

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.
Examples
servoAfma6FourPoints2DCamVelocityLs_cur.cpp, servoViper650FourPoints2DArtVelocityLs_cur.cpp, servoViper650FourPoints2DCamVelocityLs_cur-SR300.cpp, servoViper650FourPoints2DCamVelocityLs_cur.cpp, servoViper850FourPoints2DArtVelocityLs_cur.cpp, servoViper850FourPoints2DCamVelocityLs_cur.cpp, servoViper850FourPointsKinect.cpp, testPose.cpp, and tutorial-chessboard-pose.cpp.

Definition at line 335 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().

◆ 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 562 of file vpPoseDementhon.cpp.

References npt, and vpMath::sqr().

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

◆ coplanar()

bool vpPose::coplanar ( int &  coplanar_plane_type)

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
Returns
true if points are coplanar false otherwise.

Definition at line 183 of file vpPose.cpp.

References vpColVector::crossProd(), 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().

◆ 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-axiw, y-axis and z-axis respectively.
Examples
AROgre.cpp, AROgreBasic.cpp, and poseVirtualVS.cpp.

Definition at line 489 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-axiw, y-axis and z-axis respectively.

Definition at line 504 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 513 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 531 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 = NULL 
)
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.
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 668 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 276 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 303 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 334 of file vpPose.h.

◆ getRansacInlierIndex()

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

Definition at line 256 of file vpPose.h.

Referenced by vpKeyPoint::computePose().

◆ getRansacInliers()

std::vector<vpPoint> vpPose::getRansacInliers ( ) const
inline
Examples
testPoseRansac.cpp.

Definition at line 257 of file vpPose.h.

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

◆ getRansacNbInliers()

unsigned int vpPose::getRansacNbInliers ( ) const
inline

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

◆ init()

void vpPose::init ( void  )
Deprecated:
This function is deprecated since same initialisation is done in constructors.

Definition at line 63 of file vpPose.cpp.

References vpMatrix::clear(), lambda, listP, NO_FILTER, npt, and residual.

◆ 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 108 of file vpPoseDementhon.cpp.

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

Referenced by computePose().

◆ 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.

Author
Francois Chaumette (simplified by Eric Marchand)

Definition at line 429 of file vpPoseDementhon.cpp.

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

Referenced by computePose().

◆ 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 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,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 564 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()

◆ poseLagrangePlan()

void vpPose::poseLagrangePlan ( vpHomogeneousMatrix cMo)

Compute the pose of a planar object using Lagrange approach.

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

Definition at line 256 of file vpPoseLagrange.cpp.

References vpColVector::crossProd(), 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().

◆ 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 [25].

Definition at line 262 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 = NULL 
)

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 333 of file vpPoseRansac.cpp.

References addPoint(), CHECK_DEGENERATE_POINTS, computePose(), computeResidual(), DEMENTHON, vpMath::isNaN(), LAGRANGE, listP, vpPoseException::notInitializedError, PREFILTER_DEGENERATE_POINTS, setCovarianceComputation(), and VIRTUAL_VS.

Referenced by computePose().

◆ poseVirtualVS()

void vpPose::poseVirtualVS ( vpHomogeneousMatrix cMo)

◆ poseVirtualVSrobust()

◆ poseVirtualVSWithDepth()

std::optional< vpHomogeneousMatrix > vpPose::poseVirtualVSWithDepth ( std::vector< vpPoint points,
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 280 of file vpPoseVirtualVisualServoing.cpp.

References vpExponentialMap::direct(), vpHomogeneousMatrix::inverse(), lambda, vpMath::sqr(), and vpColVector::sumSquare().

Referenced by computePlanarObjectPoseWithAtLeast3Points().

◆ printPoint()

void vpPose::printPoint ( )

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

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

◆ setDistanceToPlaneForCoplanarityTest()

void vpPose::setDistanceToPlaneForCoplanarityTest ( double  d)

Definition at line 170 of file vpPose.cpp.

◆ setLambda()

void vpPose::setLambda ( double  a)
inline

Definition at line 233 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 313 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 296 of file vpPose.h.

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

◆ setRansacMaxTrials()

void vpPose::setRansacMaxTrials ( const int &  rM)
inline
Examples
testKeyPoint-4.cpp.

Definition at line 254 of file vpPose.h.

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

◆ setRansacNbInliersToReachConsensus()

void vpPose::setRansacNbInliersToReachConsensus ( const unsigned int &  nbC)
inline
Examples
testKeyPoint-4.cpp, testPose.cpp, and testPoseRansac.cpp.

Definition at line 244 of file vpPose.h.

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

◆ setRansacThreshold()

void vpPose::setRansacThreshold ( const double &  t)
inline
Examples
testKeyPoint-4.cpp, testPose.cpp, and testPoseRansac.cpp.

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

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

◆ setVvsEpsilon()

void vpPose::setVvsEpsilon ( const double  eps)
inline

Definition at line 234 of file vpPose.h.

References vpException::badValue.

◆ setVvsIterMax()

void vpPose::setVvsIterMax ( int  nb)
inline

Definition at line 242 of file vpPose.h.

Member Data Documentation

◆ lambda

double vpPose::lambda
protected

parameters use for the virtual visual servoing approach

Definition at line 124 of file vpPose.h.

Referenced by init(), poseVirtualVS(), poseVirtualVSrobust(), and poseVirtualVSWithDepth().

◆ listP

◆ npt

◆ residual

double vpPose::residual

Residual in meter.

Definition at line 121 of file vpPose.h.

Referenced by init().