Program Listing for File ippe.h

↰ Return to documentation for file (/tmp/ws/src/aruco_ros/aruco/include/aruco/ippe.h)

// This is the core header file for IPPE. Infinitesimal Plane-based Pose Estimation (IPPE)
// is a very fast and accurate way to compute a camera's pose from a single image of a
// planar object using point correspondences. This has uses in several applications,
// including augmented reality, 3D tracking and pose estimation with planar markers, and
// 3D scene understanding. This package is free and covered by the BSD licence without any
// warranty. We hope you find this code useful and if so please cite our paper in your
// work:

//@article{ year={2014}, issn={0920-5691}, journal={International Journal of Computer
//Vision}, volume={109}, number={3}, doi={10.1007/s11263-014-0725-5}, title={Infinitesimal
// Plane-Based Pose Estimation}, url={http://dx.doi.org/10.1007/s11263-014-0725-5},
// publisher={Springer US}, keywords={Plane; Pose; SfM; PnP; Homography}, author={Collins,
// Toby and Bartoli, Adrien}, pages={252-286}, language={English} }

// Please contact Toby (toby.collins@gmail.com) if you have any questions about the code, paper and IPPE.


#ifndef _IPPE_H_
#define _IPPE_H_

#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
#include "aruco_export.h"
#include <limits>

#define IPPE_SMALL 1e-7  // a small constant used to test 'small' values close to zero.

namespace aruco
{
// returns the two solutions
std::vector<cv::Mat> solvePnP(const std::vector<cv::Point3f>& objPoints,
                              const std::vector<cv::Point2f>& imgPoints,
                              cv::InputArray cameraMatrix, cv::InputArray distCoeffs);
void solvePnP(const std::vector<cv::Point3f>& objPoints,
              const std::vector<cv::Point2f>& imgPoints, cv::InputArray cameraMatrix,
              cv::InputArray distCoeffs, cv::Mat& rvec, cv::Mat& tvec);
ARUCO_EXPORT std::vector<std::pair<cv::Mat, double>> solvePnP_(
    float size, const std::vector<cv::Point2f>& imgPoints, cv::InputArray cameraMatrix,
    cv::InputArray distCoeffs);

std::vector<std::pair<cv::Mat, double>> solvePnP_(const std::vector<cv::Point3f>& objPoints,
                                                  const std::vector<cv::Point2f>& imgPoints,
                                                  cv::InputArray cameraMatrix,
                                                  cv::InputArray distCoeffs);

}  // namespace aruco
namespace IPPE
{

class PoseSolver
{
public:
  PoseSolver();

  ~PoseSolver();

  void solveGeneric(cv::InputArray _objectPoints, cv::InputArray _imagePoints,
                    cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
                    cv::OutputArray _rvec1, cv::OutputArray _tvec1, float& reprojErr1,
                    cv::OutputArray _rvec2, cv::OutputArray _tvec2, float& reprojErr2);

  void solveSquare(double squareLength, cv::InputArray _imagePoints,
                   cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
                   cv::OutputArray _rvec1, cv::OutputArray _tvec1, float& reprojErr1,
                   cv::OutputArray _rvec2, cv::OutputArray _tvec2, float& reprojErr2);

  void generateSquareObjectCorners3D(double squareLength, cv::OutputArray _objectPoints);

  void generateSquareObjectCorners2D(double squareLength, cv::OutputArray _objectPoints);

  double meanSceneDepth(cv::InputArray objectPoints, cv::InputArray rvec, cv::InputArray tvec);

private:
  void solveGeneric(cv::InputArray _objectPoints, cv::InputArray _normalizedImagePoints,
                    cv::OutputArray _Ma, cv::OutputArray _Mb);

  void solveCanonicalForm(cv::InputArray _canonicalObjPoints, cv::InputArray _normalizedInputPoints,
                          cv::InputArray _H, cv::OutputArray _Ma, cv::OutputArray _Mb);

  void computeTranslation(cv::InputArray _objectPoints, cv::InputArray _normalizedImgPoints,
                          cv::InputArray _R, cv::OutputArray _t);

  void computeRotations(double j00, double j01, double j10, double j11, double p,
                        double q, cv::OutputArray _R1, cv::OutputArray _R2);

  void homographyFromSquarePoints(cv::InputArray _targetPoints, double halfLength,
                                  cv::OutputArray _H);

  void rot2vec(cv::InputArray _R, cv::OutputArray _r);

  void makeCanonicalObjectPoints(cv::InputArray _objectPoints,
                                 cv::OutputArray _canonicalObjectPoints,
                                 cv::OutputArray _MobjectPoints2Canonical);

  void evalReprojError(cv::InputArray _objectPoints, cv::InputArray _imagePoints,
                       cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
                       cv::InputArray _M, float& err);

  void sortPosesByReprojError(cv::InputArray _objectPoints, cv::InputArray _imagePoints,
                              cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
                              cv::InputArray _Ma, cv::InputArray _Mb, cv::OutputArray _M1,
                              cv::OutputArray _M2, float& err1, float& err2);

  void rotateVec2ZAxis(cv::InputArray _a, cv::OutputArray _Ra);


  bool computeObjextSpaceR3Pts(cv::InputArray _objectPoints, cv::OutputArray _R);

  bool computeObjextSpaceRSvD(cv::InputArray _objectPointsZeroMean, cv::OutputArray _R);
};
}  // namespace IPPE

namespace HomographyHO
{

void homographyHO(cv::InputArray srcPoints, cv::InputArray targPoints, cv::OutputArray H);

void normalizeDataIsotropic(cv::InputArray Data, cv::OutputArray DataN, cv::OutputArray T,
                            cv::OutputArray Ti);
}  // namespace HomographyHO

#endif