Skip to content

O
opencv
Commit 33cb9c5c authored by catree's avatar catree
Browse files
Options

Add SOLVEPNP_IPPE for planar pose estimation. Add solvePnPGeneric function that… 

Add SOLVEPNP_IPPE for planar pose estimation. Add solvePnPGeneric function that returns all the pose solutions and the reprojection errors.
parent 7bb8d89e
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 146 additions and 71 deletions
doc/opencv.bib
View file @ 33cb9c5c
......@@ -209,7 +209,21 @@
hal_id = {inria-00350283},
hal_version = {v1},
}
@article{Collins14
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}
}
@article{Daniilidis98,
author = {Konstantinos Daniilidis},
title = {Hand-Eye Calibration Using Dual Quaternions},
......
modules/calib3d/include/opencv2/calib3d.hpp
View file @ 33cb9c5c
This diff is collapsed.
modules/calib3d/src/ap3p.cpp
View file @ 33cb9c5c
#include "precomp.hpp"
#include "ap3p.h"
#include <cmath>
......@@ -154,10 +155,11 @@ ap3p::ap3p(double _fx, double _fy, double _cx, double _cy) {
// worldPoints: The positions of the 3 feature points stored as column vectors
// solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame
// solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame
int ap3p::computePoses(const double featureVectors[3][3],
const double worldPoints[3][3],
int ap3p::computePoses(const double featureVectors[3][4],
const double worldPoints[3][4],
double solutionsR[4][3][3],
double solutionsT[4][3]) {
double solutionsT[4][3],
bool p4p) {
//world point vectors
double w1[3] = {worldPoints[0][0], worldPoints[1][0], worldPoints[2][0]};
......@@ -246,6 +248,13 @@ int ap3p::computePoses(const double featureVectors[3][3],
double b3p[3];
vect_scale((delta / k3b3), b3, b3p);
double X3 = worldPoints[0][3];
double Y3 = worldPoints[1][3];
double Z3 = worldPoints[2][3];
double mu3 = featureVectors[0][3];
double mv3 = featureVectors[1][3];
double reproj_errors[4];
int nb_solutions = 0;
for (int i = 0; i < 4; ++i) {
double ctheta1p = s[i];
......@@ -290,9 +299,29 @@ int ap3p::computePoses(const double featureVectors[3][3],
solutionsR[nb_solutions][1][2] = R[2][1];
solutionsR[nb_solutions][2][2] = R[2][2];
if (p4p) {
double X3p = solutionsR[nb_solutions][0][0] * X3 + solutionsR[nb_solutions][0][1] * Y3 + solutionsR[nb_solutions][0][2] * Z3 + solutionsT[nb_solutions][0];
double Y3p = solutionsR[nb_solutions][1][0] * X3 + solutionsR[nb_solutions][1][1] * Y3 + solutionsR[nb_solutions][1][2] * Z3 + solutionsT[nb_solutions][1];
double Z3p = solutionsR[nb_solutions][2][0] * X3 + solutionsR[nb_solutions][2][1] * Y3 + solutionsR[nb_solutions][2][2] * Z3 + solutionsT[nb_solutions][2];
double mu3p = X3p / Z3p;
double mv3p = Y3p / Z3p;
reproj_errors[nb_solutions] = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
}
nb_solutions++;
}
//sort the solutions
if (p4p) {
for (int i = 1; i < nb_solutions; i++) {
for (int j = i; j > 0 && reproj_errors[j-1] > reproj_errors[j]; j--) {
std::swap(reproj_errors[j], reproj_errors[j-1]);
std::swap(solutionsR[j], solutionsR[j-1]);
std::swap(solutionsT[j], solutionsT[j-1]);
}
}
}
return nb_solutions;
}
......@@ -311,9 +340,10 @@ bool ap3p::solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Ma
else
extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
bool result = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5],
points[6], points[7], points[8], points[9], points[10], points[11], points[12], points[13],
points[14],
bool result = solve(rotation_matrix, translation,
points[0], points[1], points[2], points[3], points[4],
points[5], points[6], points[7], points[8], points[9],
points[10], points[11], points[12], points[13],points[14],
points[15], points[16], points[17], points[18], points[19]);
cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R);
......@@ -335,10 +365,13 @@ int ap3p::solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv:
else
extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
const bool p4p = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)) == 4;
int solutions = solve(rotation_matrix, translation,
points[0], points[1], points[2], points[3], points[4],
points[5], points[6], points[7], points[8], points[9],
points[10], points[11], points[12], points[13], points[14]);
points[10], points[11], points[12], points[13], points[14],
points[15], points[16], points[17], points[18], points[19],
p4p);
for (int i = 0; i < solutions; i++) {
cv::Mat R, tvec;
......@@ -353,42 +386,33 @@ int ap3p::solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv:
}
bool
ap3p::solve(double R[3][3], double t[3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1,
double mv1,
double X1, double Y1, double Z1, double mu2, double mv2, double X2, double Y2, double Z2, double mu3,
double mv3, double X3, double Y3, double Z3) {
ap3p::solve(double R[3][3], double t[3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
double mu2, double mv2, double X2, double Y2, double Z2,
double mu3, double mv3, double X3, double Y3, double Z3) {
double Rs[4][3][3], ts[4][3];
int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2);
const bool p4p = true;
int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2, mu3, mv3, X3, Y3, Z3, p4p);
if (n == 0)
return false;
int ns = 0;
double min_reproj = 0;
for (int i = 0; i < n; i++) {
double X3p = Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0];
double Y3p = Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1];
double Z3p = Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2];
double mu3p = cx + fx * X3p / Z3p;
double mv3p = cy + fy * Y3p / Z3p;
double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
if (i == 0 || min_reproj > reproj) {
ns = i;
min_reproj = reproj;
}
}
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++)
R[i][j] = Rs[ns][i][j];
t[i] = ts[ns][i];
R[i][j] = Rs[0][i][j];
t[i] = ts[0][i];
}
return true;
}
int ap3p::solve(double R[4][3][3], double t[4][3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1,
double mv1, double X1, double Y1, double Z1, double mu2, double mv2, double X2, double Y2, double Z2) {
int ap3p::solve(double R[4][3][3], double t[4][3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
double mu2, double mv2, double X2, double Y2, double Z2,
double mu3, double mv3, double X3, double Y3, double Z3,
bool p4p) {
double mk0, mk1, mk2;
double norm;
......@@ -413,13 +437,17 @@ int ap3p::solve(double R[4][3][3], double t[4][3], double mu0, double mv0, doubl
mu2 *= mk2;
mv2 *= mk2;
double featureVectors[3][3] = {{mu0, mu1, mu2},
{mv0, mv1, mv2},
{mk0, mk1, mk2}};
double worldPoints[3][3] = {{X0, X1, X2},
{Y0, Y1, Y2},
{Z0, Z1, Z2}};
mu3 = inv_fx * mu3 - cx_fx;
mv3 = inv_fy * mv3 - cy_fy;
double mk3 = 1; //not used
double featureVectors[3][4] = {{mu0, mu1, mu2, mu3},
{mv0, mv1, mv2, mv3},
{mk0, mk1, mk2, mk3}};
double worldPoints[3][4] = {{X0, X1, X2, X3},
{Y0, Y1, Y2, Y3},
{Z0, Z1, Z2, Z3}};
return computePoses(featureVectors, worldPoints, R, t);
return computePoses(featureVectors, worldPoints, R, t, p4p);
}
}
modules/calib3d/src/ap3p.h
View file @ 33cb9c5c
#ifndef P3P_P3P_H
#define P3P_P3P_H
#include "precomp.hpp"
#include <opencv2/core.hpp>
namespace cv {
class ap3p {
......@@ -18,7 +18,7 @@ private:
void extract_points(const cv::Mat &opoints, const cv::Mat &ipoints, std::vector<double> &points) {
points.clear();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
points.resize(5*npoints);
points.resize(5*4); //resize vector to fit for p4p case
for (int i = 0; i < npoints; i++) {
points[i * 5] = ipoints.at<IpointType>(i).x * fx + cx;
points[i * 5 + 1] = ipoints.at<IpointType>(i).y * fy + cy;
......@@ -26,6 +26,12 @@ private:
points[i * 5 + 3] = opoints.at<OpointType>(i).y;
points[i * 5 + 4] = opoints.at<OpointType>(i).z;
}
//Fill vectors with unused values for p3p case
for (int i = npoints; i < 4; i++) {
for (int j = 0; j < 5; j++) {
points[i * 5 + j] = 0;
}
}
}
void init_inverse_parameters();
......@@ -45,7 +51,9 @@ public:
int solve(double R[4][3][3], double t[4][3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
double mu2, double mv2, double X2, double Y2, double Z2);
double mu2, double mv2, double X2, double Y2, double Z2,
double mu3, double mv3, double X3, double Y3, double Z3,
bool p4p);
bool solve(double R[3][3], double t[3],
double mu0, double mv0, double X0, double Y0, double Z0,
......@@ -59,8 +67,8 @@ public:
// worldPoints: Positions of the 3 feature points stored as column vectors
// solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame
// solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame
int computePoses(const double featureVectors[3][3], const double worldPoints[3][3], double solutionsR[4][3][3],
double solutionsT[4][3]);
int computePoses(const double featureVectors[3][4], const double worldPoints[3][4], double solutionsR[4][3][3],
double solutionsT[4][3], bool p4p);
};
}
......
modules/calib3d/src/ippe.cpp 0 → 100644
View file @ 33cb9c5c
This diff is collapsed.
modules/calib3d/src/ippe.hpp 0 → 100644
View file @ 33cb9c5c
This diff is collapsed.
modules/calib3d/src/p3p.cpp
View file @ 33cb9c5c
......@@ -49,9 +49,11 @@ bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat
else
extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);
bool result = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5],
points[6], points[7], points[8], points[9], points[10], points[11], points[12], points[13], points[14],
points[15], points[16], points[17], points[18], points[19]);
bool result = solve(rotation_matrix, translation,
points[0], points[1], points[2], points[3], points[4],
points[5], points[6], points[7], points[8], points[9],
points[10], points[11], points[12], points[13], points[14],
points[15], points[16], points[17], points[18], points[19]);
cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R);
return result;
......@@ -75,10 +77,13 @@ int p3p::solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, const cv::
else
extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);
const bool p4p = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)) == 4;
int solutions = solve(rotation_matrix, translation,
points[0], points[1], points[2], points[3], points[4],
points[5], points[6], points[7], points[8], points[9],
points[10], points[11], points[12], points[13], points[14]);
points[10], points[11], points[12], points[13], points[14],
points[15], points[16], points[17], points[18], points[19],
p4p);
for (int i = 0; i < solutions; i++) {
cv::Mat R, tvec;
......@@ -100,39 +105,27 @@ bool p3p::solve(double R[3][3], double t[3],
{
double Rs[4][3][3], ts[4][3];
int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2);
const bool p4p = true;
int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2, mu3, mv3, X3, Y3, Z3, p4p);
if (n == 0)
return false;
int ns = 0;
double min_reproj = 0;
for(int i = 0; i < n; i++) {
double X3p = Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0];
double Y3p = Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1];
double Z3p = Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2];
double mu3p = cx + fx * X3p / Z3p;
double mv3p = cy + fy * Y3p / Z3p;
double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
if (i == 0 || min_reproj > reproj) {
ns = i;
min_reproj = reproj;
}
}
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++)
R[i][j] = Rs[ns][i][j];
t[i] = ts[ns][i];
R[i][j] = Rs[0][i][j];
t[i] = ts[0][i];
}
return true;
}
int p3p::solve(double R[4][3][3], double t[4][3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
double mu2, double mv2, double X2, double Y2, double Z2)
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
double mu2, double mv2, double X2, double Y2, double Z2,
double mu3, double mv3, double X3, double Y3, double Z3,
bool p4p)
{
double mk0, mk1, mk2;
double norm;
......@@ -152,6 +145,9 @@ int p3p::solve(double R[4][3][3], double t[4][3],
norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1);
mk2 = 1. / norm; mu2 *= mk2; mv2 *= mk2;
mu3 = inv_fx * mu3 - cx_fx;
mv3 = inv_fy * mv3 - cy_fy;
double distances[3];
distances[0] = sqrt( (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2) );
distances[1] = sqrt( (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2) );
......@@ -167,6 +163,7 @@ int p3p::solve(double R[4][3][3], double t[4][3],
int n = solve_for_lengths(lengths, distances, cosines);
int nb_solutions = 0;
double reproj_errors[4];
for(int i = 0; i < n; i++) {
double M_orig[3][3];
......@@ -185,9 +182,29 @@ int p3p::solve(double R[4][3][3], double t[4][3],
if (!align(M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions], t[nb_solutions]))
continue;
if (p4p) {
double X3p = R[nb_solutions][0][0] * X3 + R[nb_solutions][0][1] * Y3 + R[nb_solutions][0][2] * Z3 + t[nb_solutions][0];
double Y3p = R[nb_solutions][1][0] * X3 + R[nb_solutions][1][1] * Y3 + R[nb_solutions][1][2] * Z3 + t[nb_solutions][1];
double Z3p = R[nb_solutions][2][0] * X3 + R[nb_solutions][2][1] * Y3 + R[nb_solutions][2][2] * Z3 + t[nb_solutions][2];
double mu3p = X3p / Z3p;
double mv3p = Y3p / Z3p;
reproj_errors[nb_solutions] = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
}
nb_solutions++;
}
if (p4p) {
//sort the solutions
for (int i = 1; i < nb_solutions; i++) {
for (int j = i; j > 0 && reproj_errors[j-1] > reproj_errors[j]; j--) {
std::swap(reproj_errors[j], reproj_errors[j-1]);
std::swap(R[j], R[j-1]);
std::swap(t[j], t[j-1]);
}
}
}
return nb_solutions;
}
......
modules/calib3d/src/p3p.h
View file @ 33cb9c5c
......@@ -15,7 +15,9 @@ class p3p
int solve(double R[4][3][3], double t[4][3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
double mu2, double mv2, double X2, double Y2, double Z2);
double mu2, double mv2, double X2, double Y2, double Z2,
double mu3, double mv3, double X3, double Y3, double Z3,
bool p4p);
bool solve(double R[3][3], double t[3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
......@@ -36,7 +38,7 @@ class p3p
{
points.clear();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
points.resize(5*npoints);
points.resize(5*4); //resize vector to fit for p4p case
for(int i = 0; i < npoints; i++)
{
points[i*5] = ipoints.at<IpointType>(i).x*fx + cx;
......@@ -45,6 +47,12 @@ class p3p
points[i*5+3] = opoints.at<OpointType>(i).y;
points[i*5+4] = opoints.at<OpointType>(i).z;
}
//Fill vectors with unused values for p3p case
for (int i = npoints; i < 4; i++) {
for (int j = 0; j < 5; j++) {
points[i * 5 + j] = 0;
}
}
}
void init_inverse_parameters();
int solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3]);
......
modules/calib3d/src/solvepnp.cpp
View file @ 33cb9c5c
This diff is collapsed.
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment