Image Component Library (ICL)
|
Utility class that allows for 6D pose estimation from a set of at least 4 coplanar points. More...
#include <CoplanarPointPoseEstimator.h>
Public Types | |
enum | ReferenceFrame { cameraFrame, worldFrame } |
Reference frame enumeration. More... | |
enum | PoseEstimationAlgorithm { HomographyBasedOnly, SimplexSampling, SamplingCoarse, SamplingMedium, SamplingFine, SamplingCustom } |
Algorithm, that is used for pose-estimation. More... | |
Public Member Functions | |
CoplanarPointPoseEstimator (ReferenceFrame returnedPosesReferenceFrame=worldFrame, PoseEstimationAlgorithm a=SimplexSampling) | |
Default constructor with given reference-frame for the returned poses. | |
~CoplanarPointPoseEstimator () | |
Destructor. | |
CoplanarPointPoseEstimator (const CoplanarPointPoseEstimator &other) | |
Copy constructor. | |
CoplanarPointPoseEstimator & | operator= (const CoplanarPointPoseEstimator &other) |
Assignment operator. | |
ReferenceFrame | getReferenceFrame () const |
returns the current reference frame value | |
void | setReferenceFrame (ReferenceFrame f) |
sets the refernce-frame property | |
math::FixedMatrix< float, 4, 4 > | getPose (int n, const utils::Point32f *modelPoints, const utils::Point32f *imagePoints, const Camera &cam) |
main working function that estimates the pose from given model- and image points | |
Private Member Functions | |
void | propertyChangedCallback (const Property &p) |
Internally used to sync property settings with internal data. | |
void | robustPoseCorrection (int n, const utils::Point32f *modelPoints, std::vector< utils::Point32f > &ips) |
Internally used to correct the first transformation matrix using robust pose estimation algorithm. | |
Private Attributes | |
Data * | data |
Internally used data pointer. |
Utility class that allows for 6D pose estimation from a set of at least 4 coplanar points.
In contrast to the POSIT algorithms (see icl::Posit), this class is able to compute the 6D-pose of an object from coplanar points.
Given at least 4 model-points (which are 2D because of the fact, that they have to be coplanar), where no subset of 3 points is collinear, and a set of corresponding pixel-locations of these points, the pose-estimation problem can be redefined as homography problem.
As all reference points of the object are coplanar, you can define the objects coordinate in that way that all reference points lie within the z=0 plane. As the algorithm uses this as prior knowledge, the CoplanarPointPoseEstimator::getPose interface does allow to pass 2D object reference points only.
We implemented the algorithm from the paper "Pose estimation based on four coplanar point correspondences" written by Yang et. al. and published 2009. Internally, a 9 by 2N matrix A is created. The solution for the pose detection problem is basically achieved by minimizing |Ax| using SVD. Finally, the resulting x can be used to obtain the transformation matrix. We also added one heuristic to the method described in the paper above. As the projection of a plane is ambiguous (you can project from the front and from the back), we always use always use the solution, where the resulting z-value is in front of the camera (Otherwise, we would not be able to see it). Mathematically, this means, that we do not always use x, but sometimes also -x, which obviously leads to the same error |Ax|.
CoplanarPointPoseEstimator instances can be set up to return the estimated pose w.r.t. the world frame or with respect to the camera frame. If you obtain the pose w.r.t. the camera frame, you have to multiply it with the inverted camera coordinate system transformation matrix.
In order to use the CoplanarPointPoseEstimator to estimate an objects pose matrix for visualization using an instance of the icl::Scene class, you can simply define an objects base vertices in the object coordinate frame:
e.g. (1,1,1,1), (1, 1,-1,1), ... for a unity cube. For moving these points to their real location in the 3D scene, you simply have to transform these vectors by the obtained 'w.r.t. world frame' transformation matrix.
Simple Pose estimation with 4 points needs about 80 ns on an Intel(R) Xeon(R) E5530 (2.40GHz). If 9 points are used, it needs about 110 ns.
Algorithm, that is used for pose-estimation.
TODO: describe the algorithms
HomographyBasedOnly |
uses the above described algorithm (Algorithms) only |
SimplexSampling |
performs simplex sampling for optimization (very fast and very accurate! |
SamplingCoarse |
use some predefined sampling parameters for brute force coase sampling (fast) |
SamplingMedium |
use some predefined sampling parameters for brute force medium sampling (average speed) |
SamplingFine |
use some predefined sampling parameters for brute force fine sampling (slow) |
SamplingCustom |
uses custom properties to define sampling parameters for brute force sampling |
icl::geom::CoplanarPointPoseEstimator::CoplanarPointPoseEstimator | ( | ReferenceFrame | returnedPosesReferenceFrame = worldFrame , |
PoseEstimationAlgorithm | a = SimplexSampling |
||
) |
Default constructor with given reference-frame for the returned poses.
Please note that the Downhill Simplex based pose optimization is very accurate and very fast. Using other modes does usually slowdown the pose estimation process and also decrease the result quality. Hovever the brute force search is still provided due to 'historic' reasons
icl::geom::CoplanarPointPoseEstimator::CoplanarPointPoseEstimator | ( | const CoplanarPointPoseEstimator & | other | ) |
Copy constructor.
math::FixedMatrix<float,4,4> icl::geom::CoplanarPointPoseEstimator::getPose | ( | int | n, |
const utils::Point32f * | modelPoints, | ||
const utils::Point32f * | imagePoints, | ||
const Camera & | cam | ||
) |
main working function that estimates the pose from given model- and image points
note: the camera class is only forward-declared for this file
returns the current reference frame value
CoplanarPointPoseEstimator& icl::geom::CoplanarPointPoseEstimator::operator= | ( | const CoplanarPointPoseEstimator & | other | ) |
Assignment operator.
void icl::geom::CoplanarPointPoseEstimator::propertyChangedCallback | ( | const Property & | p | ) | [private] |
Internally used to sync property settings with internal data.
void icl::geom::CoplanarPointPoseEstimator::robustPoseCorrection | ( | int | n, |
const utils::Point32f * | modelPoints, | ||
std::vector< utils::Point32f > & | ips | ||
) | [private] |
Internally used to correct the first transformation matrix using robust pose estimation algorithm.
sets the refernce-frame property
Data* icl::geom::CoplanarPointPoseEstimator::data [private] |
Internally used data pointer.