Visual Servoing Platform  version 3.4.0

#include <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 void display (vpImage< unsigned char > &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col=vpColor::none)
 
static void display (vpImage< vpRGBa > &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col=vpColor::none)
 
static double poseFromRectangle (vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam, vpHomogeneousMatrix &cMo)
 
static int computeRansacIterations (double probability, double epsilon, const int sampleSize=4, int maxIterations=2000)
 
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 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)
 

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

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

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

Constructor & Destructor Documentation

vpPose::vpPose ( )

Default constructor.

Definition at line 96 of file vpPose.cpp.

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

Definition at line 106 of file vpPose.cpp.

vpPose::~vpPose ( )
virtual

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

Definition at line 119 of file vpPose.cpp.

References listP.

Member Function Documentation

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

References listP, and npt.

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

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

References listP, and npt.

Referenced by computePlanarObjectPoseFromRGBD().

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

Return 0 if success, -1 if failure.

Definition at line 298 of file vpPoseDementhon.cpp.

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

Referenced by poseDementhonPlan().

void vpPose::clearPoint ( )
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);
}
}
Returns
true if pose estimation succeed, false otherwise.
Examples:
tutorial-apriltag-detector-live-rgbd-realsense.cpp.

Definition at line 250 of file vpPoseRGBD.cpp.

References addPoints(), computePose(), 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(), and VIRTUAL_VS.

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, testAprilTag.cpp, testKeyPoint-4.cpp, testPose.cpp, testPoseRansac.cpp, testRobotAfma6Pose.cpp, testRobotViper850Pose.cpp, and tutorial-chessboard-pose.cpp.

Definition at line 374 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 vpCalibration::addPoint(), computePlanarObjectPoseFromRGBD(), vpKeyPoint::computePose(), findMatch(), vpMbTracker::initClick(), vpMbTracker::initFromPoints(), poseFromRectangle(), and poseRansac().

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

References vpMath::nul().

double vpPose::computeResidual ( const vpHomogeneousMatrix cMo) const
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 546 of file vpPoseDementhon.cpp.

References npt, and vpMath::sqr().

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

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 184 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().

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().

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().

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.

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.

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

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

References vpTRACE.

Referenced by vpKeyPoint::computePose().

int vpPose::getNbParallelRansacThreads ( ) const
inline

Get the number of threads for the parallel RANSAC implementation.

See also
setNbParallelRansacThreads

Definition at line 296 of file vpPose.h.

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

Get the vector of points.

Returns
The vector of points.

Definition at line 327 of file vpPose.h.

References vpColor::none.

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

Definition at line 249 of file vpPose.h.

Referenced by vpKeyPoint::computePose().

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

Definition at line 250 of file vpPose.h.

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

unsigned int vpPose::getRansacNbInliers ( ) const
inline

Definition at line 248 of file vpPose.h.

Referenced by findMatch().

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

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.

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

Definition at line 108 of file vpPoseDementhon.cpp.

References computeResidualDementhon(), vpMath::deg(), vpColVector::dotProd(), 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(), vpColVector::sumSquare(), and vpColVector::t().

Referenced by computePose().

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 421 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().

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().

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().

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

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(), and listP.

Referenced by computePose().

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

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

Referenced by computePose().

void vpPose::poseVirtualVS ( vpHomogeneousMatrix cMo)
void vpPose::printPoint ( )

Definition at line 468 of file vpPose.cpp.

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

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

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

void vpPose::setDistanceToPlaneForCoplanarityTest ( double  d)

Definition at line 171 of file vpPose.cpp.

void vpPose::setLambda ( double  a)
inline

Definition at line 226 of file vpPose.h.

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

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

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

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

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

Definition at line 247 of file vpPose.h.

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

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

Definition at line 237 of file vpPose.h.

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

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

Definition at line 238 of file vpPose.h.

References vpException::badValue.

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

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

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

void vpPose::setVvsEpsilon ( const double  eps)
inline

Definition at line 227 of file vpPose.h.

References vpException::badValue.

void vpPose::setVvsIterMax ( int  nb)
inline

Definition at line 235 of file vpPose.h.

Member Data Documentation

double vpPose::lambda
protected

parameters use for the virtual visual servoing approach

Definition at line 115 of file vpPose.h.

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

unsigned int vpPose::npt
double vpPose::residual

Residual in meter.

Definition at line 112 of file vpPose.h.

Referenced by init().