Skip to content
Snippets Groups Projects
Commit 5e96ef5e authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Remove foreign file

parent 1d8a6436
No related branches found
No related tags found
2 merge requests!39release after RAL,!38After 2nd RAL submission
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 1050 deletions
src/processor/ippe.cpp deleted 100644 → 0
+ 0
− 1050
View file @ 1d8a6436
#include "base/processor/ippe.h"
#include <opencv2/imgproc.hpp>
#include <iostream>
using namespace cv;
IPPE::PoseSolver::PoseSolver()
{
}
IPPE::PoseSolver::~PoseSolver()
{
}
void IPPE::PoseSolver::solveGeneric(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
cv::OutputArray _rvec1, cv::OutputArray _tvec1, float& err1, cv::OutputArray _rvec2, cv::OutputArray _tvec2, float& err2)
{
cv::Mat normalizedImagePoints; //undistored version of imagePoints
if (_cameraMatrix.empty()) {
//there is no camera matrix and image points are given in normalized pixel coordinates.
_imagePoints.copyTo(normalizedImagePoints);
}
else {
//undistort the image points (i.e. put them in normalized pixel coordinates):
cv::undistortPoints(_imagePoints, normalizedImagePoints, _cameraMatrix, _distCoeffs);
}
//solve:
cv::Mat Ma, Mb;
solveGeneric(_objectPoints, normalizedImagePoints, Ma, Mb);
//the two poses computed by IPPE (sorted):
cv::Mat M1, M2;
//sort poses by reprojection error:
sortPosesByReprojError(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, Ma, Mb, M1, M2, err1, err2);
//fill outputs
rot2vec(M1.colRange(0, 3).rowRange(0, 3), _rvec1);
rot2vec(M2.colRange(0, 3).rowRange(0, 3), _rvec2);
M1.colRange(3, 4).rowRange(0, 3).copyTo(_tvec1);
M2.colRange(3, 4).rowRange(0, 3).copyTo(_tvec2);
}
void IPPE::PoseSolver::solveGeneric(cv::InputArray _objectPoints, cv::InputArray _normalizedInputPoints,
cv::OutputArray _Ma, cv::OutputArray _Mb)
{
//argument checking:
size_t n = _objectPoints.rows() * _objectPoints.cols(); //number of points
int objType = _objectPoints.type();
int type_input = _normalizedInputPoints.type();
assert((objType == CV_32FC3) | (objType == CV_64FC3));
assert((type_input == CV_32FC2) | (type_input == CV_64FC2));
assert((_objectPoints.rows() == 1) | (_objectPoints.cols() == 1));
assert((_objectPoints.rows() >= 4) | (_objectPoints.cols() >= 4));
assert((_normalizedInputPoints.rows() == 1) | (_normalizedInputPoints.cols() == 1));
assert(static_cast<size_t>(_objectPoints.rows() * _objectPoints.cols()) == n);
cv::Mat normalizedInputPoints;
if (type_input == CV_32FC2) {
_normalizedInputPoints.getMat().convertTo(normalizedInputPoints, CV_64FC2);
}
else {
normalizedInputPoints = _normalizedInputPoints.getMat();
}
cv::Mat objectInputPoints;
if (type_input == CV_32FC3) {
_objectPoints.getMat().convertTo(objectInputPoints, CV_64FC3);
}
else {
objectInputPoints = _objectPoints.getMat();
}
cv::Mat canonicalObjPoints;
cv::Mat MmodelPoints2Canonical;
//transform object points to the canonical position (zero centred and on the plane z=0):
makeCanonicalObjectPoints(objectInputPoints, canonicalObjPoints, MmodelPoints2Canonical);
//compute the homography mapping the model's points to normalizedInputPoints
cv::Mat H;
HomographyHO::homographyHO(canonicalObjPoints, _normalizedInputPoints, H);
//now solve
cv::Mat MaCanon, MbCanon;
solveCanonicalForm(canonicalObjPoints, normalizedInputPoints, H, MaCanon, MbCanon);
//transform computed poses to account for canonical transform:
cv::Mat Ma = MaCanon * MmodelPoints2Canonical;
cv::Mat Mb = MbCanon * MmodelPoints2Canonical;
//output poses:
Ma.copyTo(_Ma);
Mb.copyTo(_Mb);
}
void IPPE::PoseSolver::solveCanonicalForm(cv::InputArray _canonicalObjPoints, cv::InputArray _normalizedInputPoints, cv::InputArray _H,
cv::OutputArray _Ma, cv::OutputArray _Mb)
{
_Ma.create(4, 4, CV_64FC1);
_Mb.create(4, 4, CV_64FC1);
cv::Mat Ma = _Ma.getMat();
cv::Mat Mb = _Mb.getMat();
cv::Mat H = _H.getMat();
//initialise poses:
Ma.setTo(0);
Ma.at<double>(3, 3) = 1;
Mb.setTo(0);
Mb.at<double>(3, 3) = 1;
//Compute the Jacobian J of the homography at (0,0):
double j00, j01, j10, j11, v0, v1;
j00 = H.at<double>(0, 0) - H.at<double>(2, 0) * H.at<double>(0, 2);
j01 = H.at<double>(0, 1) - H.at<double>(2, 1) * H.at<double>(0, 2);
j10 = H.at<double>(1, 0) - H.at<double>(2, 0) * H.at<double>(1, 2);
j11 = H.at<double>(1, 1) - H.at<double>(2, 1) * H.at<double>(1, 2);
//Compute the transformation of (0,0) into the image:
v0 = H.at<double>(0, 2);
v1 = H.at<double>(1, 2);
//compute the two rotation solutions:
cv::Mat Ra = Ma.colRange(0, 3).rowRange(0, 3);
cv::Mat Rb = Mb.colRange(0, 3).rowRange(0, 3);
computeRotations(j00, j01, j10, j11, v0, v1, Ra, Rb);
//for each rotation solution, compute the corresponding translation solution:
cv::Mat ta = Ma.colRange(3, 4).rowRange(0, 3);
cv::Mat tb = Mb.colRange(3, 4).rowRange(0, 3);
computeTranslation(_canonicalObjPoints, _normalizedInputPoints, Ra, ta);
computeTranslation(_canonicalObjPoints, _normalizedInputPoints, Rb, tb);
}
void IPPE::PoseSolver::solveSquare(float squareLength, InputArray _imagePoints, InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArray _rvec1, OutputArray _tvec1, float& err1, OutputArray _rvec2, OutputArray _tvec2, float& err2)
{
//allocate outputs:
_rvec1.create(3, 1, CV_64FC1);
_tvec1.create(3, 1, CV_64FC1);
_rvec2.create(3, 1, CV_64FC1);
_tvec2.create(3, 1, CV_64FC1);
cv::Mat normalizedInputPoints; //undistored version of imagePoints
cv::Mat objectPoints2D;
//generate the object points:
generateSquareObjectCorners2D(squareLength, objectPoints2D);
cv::Mat H; //homography from canonical object points to normalized pixels
if (_cameraMatrix.empty()) {
//this means imagePoints are defined in normalized pixel coordinates, so just copy it:
_imagePoints.copyTo(normalizedInputPoints);
}
else {
//undistort the image points (i.e. put them in normalized pixel coordinates).
cv::undistortPoints(_imagePoints, normalizedInputPoints, _cameraMatrix, _distCoeffs);
}
//compute H
homographyFromSquarePoints(normalizedInputPoints, squareLength / 2.0f, H);
//now solve
cv::Mat Ma, Mb;
solveCanonicalForm(objectPoints2D, normalizedInputPoints, H, Ma, Mb);
//sort poses according to reprojection error:
cv::Mat M1, M2;
cv::Mat objectPoints3D;
generateSquareObjectCorners3D(squareLength, objectPoints3D);
sortPosesByReprojError(objectPoints3D, _imagePoints, _cameraMatrix, _distCoeffs, Ma, Mb, M1, M2, err1, err2);
//fill outputs
rot2vec(M1.colRange(0, 3).rowRange(0, 3), _rvec1);
rot2vec(M2.colRange(0, 3).rowRange(0, 3), _rvec2);
M1.colRange(3, 4).rowRange(0, 3).copyTo(_tvec1);
M2.colRange(3, 4).rowRange(0, 3).copyTo(_tvec2);
}
void IPPE::PoseSolver::generateSquareObjectCorners3D(double squareLength, OutputArray _objectPoints)
{
_objectPoints.create(1, 4, CV_64FC3);
cv::Mat objectPoints = _objectPoints.getMat();
objectPoints.ptr<Vec3d>(0)[0] = Vec3d(-squareLength / 2.0, squareLength / 2.0, 0.0);
objectPoints.ptr<Vec3d>(0)[1] = Vec3d(squareLength / 2.0, squareLength / 2.0, 0.0);
objectPoints.ptr<Vec3d>(0)[2] = Vec3d(squareLength / 2.0, -squareLength / 2.0, 0.0);
objectPoints.ptr<Vec3d>(0)[3] = Vec3d(-squareLength / 2.0, -squareLength / 2.0, 0.0);
}
void IPPE::PoseSolver::generateSquareObjectCorners2D(double squareLength, OutputArray _objectPoints)
{
_objectPoints.create(1, 4, CV_64FC2);
cv::Mat objectPoints = _objectPoints.getMat();
objectPoints.ptr<Vec2d>(0)[0] = Vec2d(-squareLength / 2.0, squareLength / 2.0);
objectPoints.ptr<Vec2d>(0)[1] = Vec2d(squareLength / 2.0, squareLength / 2.0);
objectPoints.ptr<Vec2d>(0)[2] = Vec2d(squareLength / 2.0, -squareLength / 2.0);
objectPoints.ptr<Vec2d>(0)[3] = Vec2d(-squareLength / 2.0, -squareLength / 2.0);
}
double IPPE::PoseSolver::meanSceneDepth(InputArray _objectPoints, InputArray _rvec, InputArray _tvec)
{
assert((_objectPoints.type() == CV_64FC3) | (_objectPoints.type() == CV_64FC3));
size_t n = _objectPoints.rows() * _objectPoints.cols();
Mat R;
Mat q;
Rodrigues(_rvec, R);
double zBar = 0;
for (size_t i = 0; i < n; i++) {
cv::Mat p(_objectPoints.getMat().at<Point3d>(i));
q = R * p + _tvec.getMat();
double z;
if (q.depth() == CV_64FC1) {
z = q.at<double>(2);
}
else {
z = static_cast<double>(q.at<float>(2));
}
zBar += z;
//if (z <= 0) {
// std::cout << "Warning: object point " << i << " projects behind the camera! This should not be allowed. " << std::endl;
//}
}
return zBar / static_cast<double>(n);
}
void IPPE::PoseSolver::rot2vec(InputArray _R, OutputArray _r)
{
assert(_R.type() == CV_64FC1);
assert(_R.rows() == 3);
assert(_R.cols() == 3);
_r.create(3, 1, CV_64FC1);
cv::Mat R = _R.getMat();
cv::Mat rvec = _r.getMat();
double trace = R.at<double>(0, 0) + R.at<double>(1, 1) + R.at<double>(2, 2);
double w_norm = acos((trace - 1.0) / 2.0);
double c0, c1, c2;
double eps = std::numeric_limits<float>::epsilon();
double d = 1 / (2 * sin(w_norm)) * w_norm;
if (w_norm < eps) //rotation is the identity
{
rvec.setTo(0);
}
else {
c0 = R.at<double>(2, 1) - R.at<double>(1, 2);
c1 = R.at<double>(0, 2) - R.at<double>(2, 0);
c2 = R.at<double>(1, 0) - R.at<double>(0, 1);
rvec.at<double>(0) = d * c0;
rvec.at<double>(1) = d * c1;
rvec.at<double>(2) = d * c2;
}
}
void IPPE::PoseSolver::computeTranslation(InputArray _objectPoints, InputArray _normalizedImgPoints, InputArray _R, OutputArray _t)
{
//This is solved by building the linear system At = b, where t corresponds to the (unknown) translation.
//This is then inverted with the associated normal equations to give t = inv(transpose(A)*A)*transpose(A)*b
//For efficiency we only store the coefficients of (transpose(A)*A) and (transpose(A)*b)
assert(_objectPoints.type() == CV_64FC2);
assert(_normalizedImgPoints.type() == CV_64FC2);
assert(_R.type() == CV_64FC1);
assert((_R.rows() == 3) & (_R.cols() == 3));
assert((_objectPoints.rows() == 1) | (_objectPoints.cols() == 1));
assert((_normalizedImgPoints.rows() == 1) | (_normalizedImgPoints.cols() == 1));
size_t n = _normalizedImgPoints.rows() * _normalizedImgPoints.cols();
assert(n == static_cast<size_t>(_objectPoints.rows() * _objectPoints.cols()));
cv::Mat objectPoints = _objectPoints.getMat();
cv::Mat imgPoints = _normalizedImgPoints.getMat();
_t.create(3, 1, CV_64FC1);
cv::Mat R = _R.getMat();
//coefficients of (transpose(A)*A)
double ATA00 = n;
double ATA02 = 0;
double ATA11 = n;
double ATA12 = 0;
double ATA20 = 0;
double ATA21 = 0;
double ATA22 = 0;
//coefficients of (transpose(A)*b)
double ATb0 = 0;
double ATb1 = 0;
double ATb2 = 0;
//S gives inv(transpose(A)*A)/det(A)^2
double S00, S01, S02;
double S10, S11, S12;
double S20, S21, S22;
double rx, ry, rz;
double a2;
double b2;
double bx, by;
//now loop through each point and increment the coefficients:
for (size_t i = 0; i < n; i++) {
rx = R.at<double>(0, 0) * objectPoints.at<Vec2d>(i)(0) + R.at<double>(0, 1) * objectPoints.at<Vec2d>(i)(1);
ry = R.at<double>(1, 0) * objectPoints.at<Vec2d>(i)(0) + R.at<double>(1, 1) * objectPoints.at<Vec2d>(i)(1);
rz = R.at<double>(2, 0) * objectPoints.at<Vec2d>(i)(0) + R.at<double>(2, 1) * objectPoints.at<Vec2d>(i)(1);
a2 = -imgPoints.at<Vec2d>(i)(0);
b2 = -imgPoints.at<Vec2d>(i)(1);
ATA02 = ATA02 + a2;
ATA12 = ATA12 + b2;
ATA20 = ATA20 + a2;
ATA21 = ATA21 + b2;
ATA22 = ATA22 + a2 * a2 + b2 * b2;
bx = -a2 * rz - rx;
by = -b2 * rz - ry;
ATb0 = ATb0 + bx;
ATb1 = ATb1 + by;
ATb2 = ATb2 + a2 * bx + b2 * by;
}
double detAInv = 1.0 / (ATA00 * ATA11 * ATA22 - ATA00 * ATA12 * ATA21 - ATA02 * ATA11 * ATA20);
//construct S:
S00 = ATA11 * ATA22 - ATA12 * ATA21;
S01 = ATA02 * ATA21;
S02 = -ATA02 * ATA11;
S10 = ATA12 * ATA20;
S11 = ATA00 * ATA22 - ATA02 * ATA20;
S12 = -ATA00 * ATA12;
S20 = -ATA11 * ATA20;
S21 = -ATA00 * ATA21;
S22 = ATA00 * ATA11;
//solve t:
Mat t = _t.getMat();
t.at<double>(0) = detAInv * (S00 * ATb0 + S01 * ATb1 + S02 * ATb2);
t.at<double>(1) = detAInv * (S10 * ATb0 + S11 * ATb1 + S12 * ATb2);
t.at<double>(2) = detAInv * (S20 * ATb0 + S21 * ATb1 + S22 * ATb2);
}
void IPPE::PoseSolver::computeRotations(double j00, double j01, double j10, double j11, double p, double q, OutputArray _R1, OutputArray _R2)
{
//This is fairly optimized code which makes it hard to understand. The matlab code is certainly easier to read.
_R1.create(3, 3, CV_64FC1);
_R2.create(3, 3, CV_64FC1);
double a00, a01, a10, a11, ata00, ata01, ata11, b00, b01, b10, b11, binv00, binv01, binv10, binv11;
//double rv00, rv01, rv02, rv10, rv11, rv12, rv20, rv21, rv22;
double rtilde00, rtilde01, rtilde10, rtilde11;
double rtilde00_2, rtilde01_2, rtilde10_2, rtilde11_2;
double b0, b1, gamma, dtinv;
double sp;
Mat Rv;
cv::Mat v(3,1,CV_64FC1);
v.at<double>(0) = p;
v.at<double>(1) = q;
v.at<double>(2) = 1;
rotateVec2ZAxis(v,Rv);
Rv = Rv.t();
//setup the 2x2 SVD decomposition:
double rv00, rv01, rv02;
double rv10, rv11, rv12;
double rv20, rv21, rv22;
rv00 = Rv.at<double>(0,0);
rv01 = Rv.at<double>(0,1);
rv02 = Rv.at<double>(0,2);
rv10 = Rv.at<double>(1,0);
rv11 = Rv.at<double>(1,1);
rv12 = Rv.at<double>(1,2);
rv20 = Rv.at<double>(2,0);
rv21 = Rv.at<double>(2,1);
rv22 = Rv.at<double>(2,2);
b00 = rv00 - p * rv20;
b01 = rv01 - p * rv21;
b10 = rv10 - q * rv20;
b11 = rv11 - q * rv21;
dtinv = 1.0 / ((b00 * b11 - b01 * b10));
binv00 = dtinv * b11;
binv01 = -dtinv * b01;
binv10 = -dtinv * b10;
binv11 = dtinv * b00;
a00 = binv00 * j00 + binv01 * j10;
a01 = binv00 * j01 + binv01 * j11;
a10 = binv10 * j00 + binv11 * j10;
a11 = binv10 * j01 + binv11 * j11;
//compute the largest singular value of A:
ata00 = a00 * a00 + a01 * a01;
ata01 = a00 * a10 + a01 * a11;
ata11 = a10 * a10 + a11 * a11;
gamma = sqrt(0.5 * (ata00 + ata11 + sqrt((ata00 - ata11) * (ata00 - ata11) + 4.0 * ata01 * ata01)));
//reconstruct the full rotation matrices:
rtilde00 = a00 / gamma;
rtilde01 = a01 / gamma;
rtilde10 = a10 / gamma;
rtilde11 = a11 / gamma;
rtilde00_2 = rtilde00 * rtilde00;
rtilde01_2 = rtilde01 * rtilde01;
rtilde10_2 = rtilde10 * rtilde10;
rtilde11_2 = rtilde11 * rtilde11;
b0 = sqrt(-rtilde00_2 - rtilde10_2 + 1);
b1 = sqrt(-rtilde01_2 - rtilde11_2 + 1);
sp = (-rtilde00 * rtilde01 - rtilde10 * rtilde11);
if (sp < 0) {
b1 = -b1;
}
//store results:
Mat R1 = _R1.getMat();
Mat R2 = _R2.getMat();
R1.at<double>(0, 0) = (rtilde00)*rv00 + (rtilde10)*rv01 + (b0)*rv02;
R1.at<double>(0, 1) = (rtilde01)*rv00 + (rtilde11)*rv01 + (b1)*rv02;
R1.at<double>(0, 2) = (b1 * rtilde10 - b0 * rtilde11) * rv00 + (b0 * rtilde01 - b1 * rtilde00) * rv01 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv02;
R1.at<double>(1, 0) = (rtilde00)*rv10 + (rtilde10)*rv11 + (b0)*rv12;
R1.at<double>(1, 1) = (rtilde01)*rv10 + (rtilde11)*rv11 + (b1)*rv12;
R1.at<double>(1, 2) = (b1 * rtilde10 - b0 * rtilde11) * rv10 + (b0 * rtilde01 - b1 * rtilde00) * rv11 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv12;
R1.at<double>(2, 0) = (rtilde00)*rv20 + (rtilde10)*rv21 + (b0)*rv22;
R1.at<double>(2, 1) = (rtilde01)*rv20 + (rtilde11)*rv21 + (b1)*rv22;
R1.at<double>(2, 2) = (b1 * rtilde10 - b0 * rtilde11) * rv20 + (b0 * rtilde01 - b1 * rtilde00) * rv21 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv22;
R2.at<double>(0, 0) = (rtilde00)*rv00 + (rtilde10)*rv01 + (-b0) * rv02;
R2.at<double>(0, 1) = (rtilde01)*rv00 + (rtilde11)*rv01 + (-b1) * rv02;
R2.at<double>(0, 2) = (b0 * rtilde11 - b1 * rtilde10) * rv00 + (b1 * rtilde00 - b0 * rtilde01) * rv01 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv02;
R2.at<double>(1, 0) = (rtilde00)*rv10 + (rtilde10)*rv11 + (-b0) * rv12;
R2.at<double>(1, 1) = (rtilde01)*rv10 + (rtilde11)*rv11 + (-b1) * rv12;
R2.at<double>(1, 2) = (b0 * rtilde11 - b1 * rtilde10) * rv10 + (b1 * rtilde00 - b0 * rtilde01) * rv11 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv12;
R2.at<double>(2, 0) = (rtilde00)*rv20 + (rtilde10)*rv21 + (-b0) * rv22;
R2.at<double>(2, 1) = (rtilde01)*rv20 + (rtilde11)*rv21 + (-b1) * rv22;
R2.at<double>(2, 2) = (b0 * rtilde11 - b1 * rtilde10) * rv20 + (b1 * rtilde00 - b0 * rtilde01) * rv21 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv22;
}
void IPPE::PoseSolver::homographyFromSquarePoints(InputArray _targetPoints, double halfLength, OutputArray H_)
{
assert((_targetPoints.type() == CV_32FC2) | (_targetPoints.type() == CV_64FC2));
cv::Mat pts = _targetPoints.getMat();
H_.create(3, 3, CV_64FC1);
Mat H = H_.getMat();
double p1x, p1y;
double p2x, p2y;
double p3x, p3y;
double p4x, p4y;
if (_targetPoints.type() == CV_32FC2) {
p1x = -pts.at<Vec2f>(0)(0);
p1y = -pts.at<Vec2f>(0)(1);
p2x = -pts.at<Vec2f>(1)(0);
p2y = -pts.at<Vec2f>(1)(1);
p3x = -pts.at<Vec2f>(2)(0);
p3y = -pts.at<Vec2f>(2)(1);
p4x = -pts.at<Vec2f>(3)(0);
p4y = -pts.at<Vec2f>(3)(1);
}
else {
p1x = -pts.at<Vec2d>(0)(0);
p1y = -pts.at<Vec2d>(0)(1);
p2x = -pts.at<Vec2d>(1)(0);
p2y = -pts.at<Vec2d>(1)(1);
p3x = -pts.at<Vec2d>(2)(0);
p3y = -pts.at<Vec2d>(2)(1);
p4x = -pts.at<Vec2d>(3)(0);
p4y = -pts.at<Vec2d>(3)(1);
}
//analytic solution:
double detsInv = -1 / (halfLength * (p1x * p2y - p2x * p1y - p1x * p4y + p2x * p3y - p3x * p2y + p4x * p1y + p3x * p4y - p4x * p3y));
H.at<double>(0, 0) = detsInv * (p1x * p3x * p2y - p2x * p3x * p1y - p1x * p4x * p2y + p2x * p4x * p1y - p1x * p3x * p4y + p1x * p4x * p3y + p2x * p3x * p4y - p2x * p4x * p3y);
H.at<double>(0, 1) = detsInv * (p1x * p2x * p3y - p1x * p3x * p2y - p1x * p2x * p4y + p2x * p4x * p1y + p1x * p3x * p4y - p3x * p4x * p1y - p2x * p4x * p3y + p3x * p4x * p2y);
H.at<double>(0, 2) = detsInv * halfLength * (p1x * p2x * p3y - p2x * p3x * p1y - p1x * p2x * p4y + p1x * p4x * p2y - p1x * p4x * p3y + p3x * p4x * p1y + p2x * p3x * p4y - p3x * p4x * p2y);
H.at<double>(1, 0) = detsInv * (p1x * p2y * p3y - p2x * p1y * p3y - p1x * p2y * p4y + p2x * p1y * p4y - p3x * p1y * p4y + p4x * p1y * p3y + p3x * p2y * p4y - p4x * p2y * p3y);
H.at<double>(1, 1) = detsInv * (p2x * p1y * p3y - p3x * p1y * p2y - p1x * p2y * p4y + p4x * p1y * p2y + p1x * p3y * p4y - p4x * p1y * p3y - p2x * p3y * p4y + p3x * p2y * p4y);
H.at<double>(1, 2) = detsInv * halfLength * (p1x * p2y * p3y - p3x * p1y * p2y - p2x * p1y * p4y + p4x * p1y * p2y - p1x * p3y * p4y + p3x * p1y * p4y + p2x * p3y * p4y - p4x * p2y * p3y);
H.at<double>(2, 0) = -detsInv * (p1x * p3y - p3x * p1y - p1x * p4y - p2x * p3y + p3x * p2y + p4x * p1y + p2x * p4y - p4x * p2y);
H.at<double>(2, 1) = detsInv * (p1x * p2y - p2x * p1y - p1x * p3y + p3x * p1y + p2x * p4y - p4x * p2y - p3x * p4y + p4x * p3y);
H.at<double>(2, 2) = 1.0;
}
void IPPE::PoseSolver::makeCanonicalObjectPoints(InputArray _objectPoints, OutputArray _canonicalObjPoints, OutputArray _MmodelPoints2Canonical)
{
int objType = _objectPoints.type();
assert((objType == CV_64FC3) | (objType == CV_32FC3));
size_t n = _objectPoints.rows() * _objectPoints.cols();
_canonicalObjPoints.create(1, n, CV_64FC2);
_MmodelPoints2Canonical.create(4, 4, CV_64FC1);
cv::Mat objectPoints = _objectPoints.getMat();
cv::Mat canonicalObjPoints = _canonicalObjPoints.getMat();
cv::Mat UZero(3, n, CV_64FC1);
double xBar = 0;
double yBar = 0;
double zBar = 0;
bool isOnZPlane = true;
for (size_t i = 0; i < n; i++) {
double x, y, z;
if (objType == CV_32FC3) {
x = static_cast<double>(objectPoints.at<Vec3f>(i)[0]);
y = static_cast<double>(objectPoints.at<Vec3f>(i)[1]);
z = static_cast<double>(objectPoints.at<Vec3f>(i)[2]);
}
else {
x = objectPoints.at<Vec3d>(i)[0];
y = objectPoints.at<Vec3d>(i)[1];
z = objectPoints.at<Vec3d>(i)[2];
if (abs(z) > IPPE_SMALL) {
isOnZPlane = false;
}
}
xBar += x;
yBar += y;
zBar += z;
UZero.at<double>(0, i) = x;
UZero.at<double>(1, i) = y;
UZero.at<double>(2, i) = z;
}
xBar = xBar / (double)n;
yBar = yBar / (double)n;
zBar = zBar / (double)n;
for (size_t i = 0; i < n; i++) {
UZero.at<double>(0, i) -= xBar;
UZero.at<double>(1, i) -= yBar;
UZero.at<double>(2, i) -= zBar;
}
cv::Mat MCenter(4, 4, CV_64FC1);
MCenter.setTo(0);
MCenter.at<double>(0, 0) = 1;
MCenter.at<double>(1, 1) = 1;
MCenter.at<double>(2, 2) = 1;
MCenter.at<double>(3, 3) = 1;
MCenter.at<double>(0, 3) = -xBar;
MCenter.at<double>(1, 3) = -yBar;
MCenter.at<double>(2, 3) = -zBar;
if (isOnZPlane) {
//MmodelPoints2Canonical is given by MCenter
MCenter.copyTo(_MmodelPoints2Canonical);
for (size_t i = 0; i < n; i++) {
canonicalObjPoints.at<Vec2d>(i)[0] = UZero.at<double>(0, i);
canonicalObjPoints.at<Vec2d>(i)[1] = UZero.at<double>(1, i);
}
}
else {
cv::Mat UZeroAligned(3, n, CV_64FC1);
cv::Mat R; //rotation that rotates objectPoints to the plane z=0
if (!computeObjextSpaceR3Pts(objectPoints,R))
{
//we could not compute R, problably because there is a duplicate point in {objectPoints(0),objectPoints(1),objectPoints(2)}. So we compute it with the SVD (which is slower):
computeObjextSpaceRSvD(UZero,R);
}
UZeroAligned = R * UZero;
for (size_t i = 0; i < n; i++) {
canonicalObjPoints.at<Vec2d>(i)[0] = UZeroAligned.at<double>(0, i);
canonicalObjPoints.at<Vec2d>(i)[1] = UZeroAligned.at<double>(1, i);
assert(abs(UZeroAligned.at<double>(2, i))<=IPPE_SMALL);
}
cv::Mat MRot(4, 4, CV_64FC1);
MRot.setTo(0);
MRot.at<double>(3, 3) = 1;
R.copyTo(MRot.colRange(0, 3).rowRange(0, 3));
cv::Mat Mb = MRot * MCenter;
Mb.copyTo(_MmodelPoints2Canonical);
}
}
void IPPE::PoseSolver::evalReprojError(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, cv::InputArray _M, float& err)
{
cv::Mat projectedPoints;
cv::Mat imagePoints = _imagePoints.getMat();
cv::Mat r;
rot2vec(_M.getMat().colRange(0, 3).rowRange(0, 3), r);
if (_cameraMatrix.empty()) {
//there is no camera matrix and image points are in normalized pixel coordinates
cv::Mat K(3, 3, CV_64FC1);
K.setTo(0);
K.at<double>(0, 0) = 1;
K.at<double>(1, 1) = 1;
K.at<double>(2, 2) = 1;
cv::Mat kc;
cv::projectPoints(_objectPoints, r, _M.getMat().colRange(3, 4).rowRange(0, 3), K, kc, projectedPoints);
}
else {
cv::projectPoints(_objectPoints, r, _M.getMat().colRange(3, 4).rowRange(0, 3), _cameraMatrix, _distCoeffs, projectedPoints);
}
err = 0;
size_t n = _objectPoints.rows() * _objectPoints.cols();
float dx, dy;
for (size_t i = 0; i < n; i++) {
if (projectedPoints.depth() == CV_32FC1) {
dx = projectedPoints.at<Vec2f>(i)[0] - imagePoints.at<Vec2f>(i)[0];
// dy = projectedPoints.at<Vec2f>(i)[0] - imagePoints.at<Vec2f>(i)[0];
dy = projectedPoints.at<Vec2f>(i)[1] - imagePoints.at<Vec2f>(i)[1];
}
else {
dx = projectedPoints.at<Vec2d>(i)[0] - imagePoints.at<Vec2d>(i)[0];
// dy = projectedPoints.at<Vec2d>(i)[0] - imagePoints.at<Vec2d>(i)[0];
dy = projectedPoints.at<Vec2d>(i)[1] - imagePoints.at<Vec2d>(i)[1];
}
err += dx * dx + dy * dy;
}
err = sqrt(err / (2.0f * n));
}
void IPPE::PoseSolver::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)
{
float erra, errb;
evalReprojError(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _Ma, erra);
evalReprojError(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _Mb, errb);
if (erra < errb) {
err1 = erra;
_Ma.copyTo(_M1);
err2 = errb;
_Mb.copyTo(_M2);
}
else {
err1 = errb;
_Mb.copyTo(_M1);
err2 = erra;
_Ma.copyTo(_M2);
}
}
void HomographyHO::normalizeDataIsotropic(cv::InputArray _Data, cv::OutputArray _DataN, cv::OutputArray _T, cv::OutputArray _Ti)
{
cv::Mat Data = _Data.getMat();
int numPoints = Data.rows * Data.cols;
assert((Data.rows == 1) | (Data.cols == 1));
assert((Data.channels() == 2) | (Data.channels() == 3));
assert(numPoints >= 4);
int dataType = _Data.type();
assert((dataType == CV_64FC2) | (dataType == CV_64FC3) | (dataType == CV_32FC2) | (dataType == CV_32FC3));
_DataN.create(2, numPoints, CV_64FC1);
_T.create(3, 3, CV_64FC1);
_Ti.create(3, 3, CV_64FC1);
cv::Mat DataN = _DataN.getMat();
cv::Mat T = _T.getMat();
cv::Mat Ti = _Ti.getMat();
_T.setTo(0);
_Ti.setTo(0);
double xm, ym;
int numChannels = Data.channels();
xm = 0;
ym = 0;
for (int i = 0; i < numPoints; i++) {
if (numChannels == 2) {
if (dataType == CV_32FC2) {
xm = xm + Data.at<Vec2f>(i)[0];
ym = ym + Data.at<Vec2f>(i)[1];
}
else {
xm = xm + Data.at<Vec2d>(i)[0];
ym = ym + Data.at<Vec2d>(i)[1];
}
}
else {
if (dataType == CV_32FC3) {
xm = xm + Data.at<Vec3f>(i)[0];
ym = ym + Data.at<Vec3f>(i)[1];
}
else {
xm = xm + Data.at<Vec3d>(i)[0];
ym = ym + Data.at<Vec3d>(i)[1];
}
}
}
xm = xm / (double)numPoints;
ym = ym / (double)numPoints;
double kappa = 0;
double xh, yh;
for (int i = 0; i < numPoints; i++) {
if (numChannels == 2) {
if (dataType == CV_32FC2) {
xh = Data.at<Vec2f>(i)[0] - xm;
yh = Data.at<Vec2f>(i)[1] - ym;
}
else {
xh = Data.at<Vec2d>(i)[0] - xm;
yh = Data.at<Vec2d>(i)[1] - ym;
}
}
else {
if (dataType == CV_32FC3) {
xh = Data.at<Vec3f>(i)[0] - xm;
yh = Data.at<Vec3f>(i)[1] - ym;
}
else {
xh = Data.at<Vec3d>(i)[0] - xm;
yh = Data.at<Vec3d>(i)[1] - ym;
}
}
DataN.at<double>(0, i) = xh;
DataN.at<double>(1, i) = yh;
kappa = kappa + xh * xh + yh * yh;
}
double beta = sqrt(2 * numPoints / kappa);
DataN = DataN * beta;
T.at<double>(0, 0) = 1.0 / beta;
T.at<double>(1, 1) = 1.0 / beta;
T.at<double>(0, 2) = xm;
T.at<double>(1, 2) = ym;
T.at<double>(2, 2) = 1;
Ti.at<double>(0, 0) = beta;
Ti.at<double>(1, 1) = beta;
Ti.at<double>(0, 2) = -beta * xm;
Ti.at<double>(1, 2) = -beta * ym;
Ti.at<double>(2, 2) = 1;
}
void HomographyHO::homographyHO(cv::InputArray _srcPoints, cv::InputArray _targPoints, cv::OutputArray _H)
{
_H.create(3, 3, CV_64FC1);
cv::Mat H = _H.getMat();
cv::Mat DataA, DataB, TA, TAi, TB, TBi;
HomographyHO::normalizeDataIsotropic(_srcPoints, DataA, TA, TAi);
HomographyHO::normalizeDataIsotropic(_targPoints, DataB, TB, TBi);
int n = DataA.cols;
assert(n == DataB.cols);
cv::Mat C1(1, n, CV_64FC1);
cv::Mat C2(1, n, CV_64FC1);
cv::Mat C3(1, n, CV_64FC1);
cv::Mat C4(1, n, CV_64FC1);
cv::Mat Mx(n, 3, CV_64FC1);
cv::Mat My(n, 3, CV_64FC1);
double mC1, mC2, mC3, mC4;
mC1 = 0;
mC2 = 0;
mC3 = 0;
mC4 = 0;
for (int i = 0; i < n; i++) {
C1.at<double>(0, i) = -DataB.at<double>(0, i) * DataA.at<double>(0, i);
C2.at<double>(0, i) = -DataB.at<double>(0, i) * DataA.at<double>(1, i);
C3.at<double>(0, i) = -DataB.at<double>(1, i) * DataA.at<double>(0, i);
C4.at<double>(0, i) = -DataB.at<double>(1, i) * DataA.at<double>(1, i);
mC1 = mC1 + C1.at<double>(0, i);
mC2 = mC2 + C2.at<double>(0, i);
mC3 = mC3 + C3.at<double>(0, i);
mC4 = mC4 + C4.at<double>(0, i);
}
mC1 = mC1 / n;
mC2 = mC2 / n;
mC3 = mC3 / n;
mC4 = mC4 / n;
for (int i = 0; i < n; i++) {
Mx.at<double>(i, 0) = C1.at<double>(0, i) - mC1;
Mx.at<double>(i, 1) = C2.at<double>(0, i) - mC2;
Mx.at<double>(i, 2) = -DataB.at<double>(0, i);
My.at<double>(i, 0) = C3.at<double>(0, i) - mC3;
My.at<double>(i, 1) = C4.at<double>(0, i) - mC4;
My.at<double>(i, 2) = -DataB.at<double>(1, i);
}
cv::Mat DataAT, DataADataAT, DataADataATi, Pp, Bx, By, Ex, Ey, D;
cv::transpose(DataA, DataAT);
DataADataAT = DataA * DataAT;
double dt = DataADataAT.at<double>(0, 0) * DataADataAT.at<double>(1, 1) - DataADataAT.at<double>(0, 1) * DataADataAT.at<double>(1, 0);
DataADataATi = cv::Mat(2, 2, CV_64FC1);
DataADataATi.at<double>(0, 0) = DataADataAT.at<double>(1, 1) / dt;
DataADataATi.at<double>(0, 1) = -DataADataAT.at<double>(0, 1) / dt;
DataADataATi.at<double>(1, 0) = -DataADataAT.at<double>(1, 0) / dt;
DataADataATi.at<double>(1, 1) = DataADataAT.at<double>(0, 0) / dt;
Pp = DataADataATi * DataA;
Bx = Pp * Mx;
By = Pp * My;
Ex = DataAT * Bx;
Ey = DataAT * By;
D = cv::Mat(2 * n, 3, CV_64FC1);
cv::Mat DT, DDT;
for (int i = 0; i < n; i++) {
D.at<double>(i, 0) = Mx.at<double>(i, 0) - Ex.at<double>(i, 0);
D.at<double>(i, 1) = Mx.at<double>(i, 1) - Ex.at<double>(i, 1);
D.at<double>(i, 2) = Mx.at<double>(i, 2) - Ex.at<double>(i, 2);
D.at<double>(i + n, 0) = My.at<double>(i, 0) - Ey.at<double>(i, 0);
D.at<double>(i + n, 1) = My.at<double>(i, 1) - Ey.at<double>(i, 1);
D.at<double>(i + n, 2) = My.at<double>(i, 2) - Ey.at<double>(i, 2);
}
cv::transpose(D, DT);
DDT = DT * D;
cv::Mat S, U, V, h12, h45;
double h3, h6;
cv::eigen(DDT, S, U);
cv::Mat h789(3, 1, CV_64FC1);
h789.at<double>(0, 0) = U.at<double>(2, 0);
h789.at<double>(1, 0) = U.at<double>(2, 1);
h789.at<double>(2, 0) = U.at<double>(2, 2);
h12 = -Bx * h789;
h45 = -By * h789;
h3 = -(mC1 * h789.at<double>(0, 0) + mC2 * h789.at<double>(1, 0));
h6 = -(mC3 * h789.at<double>(0, 0) + mC4 * h789.at<double>(1, 0));
H.at<double>(0, 0) = h12.at<double>(0, 0);
H.at<double>(0, 1) = h12.at<double>(1, 0);
H.at<double>(0, 2) = h3;
H.at<double>(1, 0) = h45.at<double>(0, 0);
H.at<double>(1, 1) = h45.at<double>(1, 0);
H.at<double>(1, 2) = h6;
H.at<double>(2, 0) = h789.at<double>(0, 0);
H.at<double>(2, 1) = h789.at<double>(1, 0);
H.at<double>(2, 2) = h789.at<double>(2, 0);
H = TB * H * TAi;
H = H / H.at<double>(2, 2);
}
void IPPE::PoseSolver::rotateVec2ZAxis(InputArray _a, OutputArray _Ra)
{
_Ra.create(3,3,CV_64FC1);
Mat Ra = _Ra.getMat();
double ax = _a.getMat().at<double>(0);
double ay = _a.getMat().at<double>(1);
double az = _a.getMat().at<double>(2);
double nrm = sqrt(ax*ax + ay*ay + az*az);
ax = ax/nrm;
ay = ay/nrm;
az = az/nrm;
double c = az;
if (abs(1.0+c)< std::numeric_limits<float>::epsilon())
{
Ra.setTo(0.0);
Ra.at<double>(0,0) = 1.0;
Ra.at<double>(1,1) = 1.0;
Ra.at<double>(2,2) = -1.0;
}
else
{
double d = 1.0/(1.0+c);
double ax2 = ax*ax;
double ay2 = ay*ay;
double axay = ax*ay;
Ra.at<double>(0,0) = - ax2*d + 1.0;
Ra.at<double>(0,1) = -axay*d;
Ra.at<double>(0,2) = -ax;
Ra.at<double>(1,0) = -axay*d;
Ra.at<double>(1,1) = - ay2*d + 1.0;
Ra.at<double>(1,2) = -ay;
Ra.at<double>(2,0) = ax;
Ra.at<double>(2,1) = ay;
Ra.at<double>(2,2) = 1.0 - (ax2 + ay2)*d;
}
}
bool IPPE::PoseSolver::computeObjextSpaceR3Pts(InputArray _objectPoints, OutputArray R)
{
bool ret; //return argument
double p1x,p1y,p1z;
double p2x,p2y,p2z;
double p3x,p3y,p3z;
cv::Mat objectPoints = _objectPoints.getMat();
// size_t n = objectPoints.rows*objectPoints.cols;
if (objectPoints.type() == CV_32FC3)
{
p1x = objectPoints.at<Vec3f>(0)[0];
p1y = objectPoints.at<Vec3f>(0)[1];
p1z = objectPoints.at<Vec3f>(0)[2];
p2x = objectPoints.at<Vec3f>(1)[0];
p2y = objectPoints.at<Vec3f>(1)[1];
p2z = objectPoints.at<Vec3f>(1)[2];
p3x = objectPoints.at<Vec3f>(2)[0];
p3y = objectPoints.at<Vec3f>(2)[1];
p3z = objectPoints.at<Vec3f>(2)[2];
}
else
{
p1x = objectPoints.at<Vec3d>(0)[0];
p1y = objectPoints.at<Vec3d>(0)[1];
p1z = objectPoints.at<Vec3d>(0)[2];
p2x = objectPoints.at<Vec3d>(1)[0];
p2y = objectPoints.at<Vec3d>(1)[1];
p2z = objectPoints.at<Vec3d>(1)[2];
p3x = objectPoints.at<Vec3d>(2)[0];
p3y = objectPoints.at<Vec3d>(2)[1];
p3z = objectPoints.at<Vec3d>(2)[2];
}
double nx = (p1y - p2y)*(p1z - p3z) - (p1y - p3y)*(p1z - p2z);
double ny = (p1x - p3x)*(p1z - p2z) - (p1x - p2x)*(p1z - p3z);
double nz = (p1x - p2x)*(p1y - p3y) - (p1x - p3x)*(p1y - p2y);
double nrm = sqrt(nx*nx+ ny*ny + nz*nz);
if (nrm>IPPE_SMALL)
{
nx = nx/nrm;
ny = ny/nrm;
nz = nz/nrm;
cv::Mat v(3,1,CV_64FC1);
v.at<double>(0) = nx;
v.at<double>(1) = ny;
v.at<double>(2) = nz;
rotateVec2ZAxis(v,R);
ret = true;
}
else
{
ret = false;
}
return ret;
}
bool IPPE::PoseSolver::computeObjextSpaceRSvD(InputArray _objectPointsZeroMean, OutputArray _R)
{
bool ret; //return argument
_R.create(3,3,CV_64FC1);
cv::Mat R = _R.getMat();
//we could not compute R with the first three points, so lets use the SVD
cv::SVD s;
cv::Mat W, U, VT;
s.compute(_objectPointsZeroMean.getMat() * _objectPointsZeroMean.getMat().t(), W, U, VT);
double s3 = W.at<double>(2);
double s2 = W.at<double>(1);
//check if points are coplanar:
assert(s3 / s2 < IPPE_SMALL);
R = U.t();
if (cv::determinant(R) < 0) { //this ensures R is a rotation matrix and not a general unitary matrix:
R.at<double>(2, 0) = -R.at<double>(2, 0);
R.at<double>(2, 1) = -R.at<double>(2, 1);
R.at<double>(2, 2) = -R.at<double>(2, 2);
}
ret = true;
return ret;
}
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