generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Updating for N=2

Browse Source
This commit is contained in:
edgarriba
2014-10-06 10:28:04 +02:00
parent dc76ca5fc9
commit d6bf209bb3
2 changed files with 355 additions and 289 deletions
Download Patch File Download Diff File Expand all files Collapse all files

128
modules/calib3d/src/upnp.cpp
View File

@@ -2,17 +2,6 @@
#include "upnp.h"
#include <limits>
void printMat(cv::Mat &mat)
{
cout << mat.rows << "x" << mat.cols << endl;
for (int i = 0; i < mat.rows; ++i) {
cout << "[";
for (int j = 0; j < mat.cols; ++j) {
cout << mat.at<double>(i,j) << ",";
}
cout << "];" << endl;
}
}
upnp::upnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints)
{
if (cameraMatrix.depth() == CV_32F)
@@ -81,25 +70,25 @@ double upnp::compute_pose(cv::Mat& R, cv::Mat& t)
compute_L_6x12(ut, l_6x12);
compute_rho(rho);
double Betas[4][4], Efs[4][1], rep_errors[4];
double Rs[4][3][3], ts[4][3];
double Betas[3][4], Efs[3][1], rep_errors[3];
double Rs[3][3][3], ts[3][3];
find_betas_and_focal_approx_1(&Ut, &Rho, Betas[1], Efs[1]);
gauss_newton(&L_6x12, &Rho, Betas[1], Efs[1]);
rep_errors[1] = compute_R_and_t(ut, Betas[1], Efs[1], Rs[1], ts[1]);
rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]);
find_betas_and_focal_approx_2(&Ut, &Rho, Betas[2], Efs[2]);
//gauss_newton(&L_6x12, &Rho, Betas[2], Efs[2]);
//rep_errors[2] = compute_R_and_t(ut, Betas[2], Efs[2], Rs[2], ts[2]);
gauss_newton(&L_6x12, &Rho, Betas[2], Efs[2]);
rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]);
int N = 1;
//if (rep_errors[2] < rep_errors[1]) N = 2;
//if (rep_errors[3] < rep_errors[N]) N = 3;
if (rep_errors[2] < rep_errors[1]) N = 2;
cv::Mat(3, 1, CV_64F, ts[N]).copyTo(t);
cv::Mat(3, 3, CV_64F, Rs[N]).copyTo(R);
fu = fv = Efs[N][0];
return Efs[N][0];
return fu;
}
void upnp::copy_R_and_t(const double R_src[3][3], const double t_src[3],
@@ -187,10 +176,10 @@ void upnp::solve_for_sign(void)
}
}
double upnp::compute_R_and_t(const double * ut, const double * betas, const double * efs,
double upnp::compute_R_and_t(const double * ut, const double * betas,
double R[3][3], double t[3])
{
compute_ccs(betas, efs, ut);
compute_ccs(betas, ut);
compute_pcs();
solve_for_sign();
@@ -229,8 +218,8 @@ void upnp::choose_control_points()
void upnp::compute_alphas()
{
cv::Mat CC = cv::Mat(4, 3, CV_64F, &cws);
cv::Mat PC = cv::Mat(number_of_correspondences, 3, CV_64F, &pws.front());
cv::Mat ALPHAS = cv::Mat(number_of_correspondences, 4, CV_64F, &alphas.front());
cv::Mat PC = cv::Mat(number_of_correspondences, 3, CV_64F, &pws[0]);
cv::Mat ALPHAS = cv::Mat(number_of_correspondences, 4, CV_64F, &alphas[0]);
cv::Mat CC_ = CC.clone().t();
cv::Mat PC_ = PC.clone().t();
@@ -260,7 +249,7 @@ void upnp::fill_M(CvMat * M, const int row, const double * as, const double u, c
}
}
void upnp::compute_ccs(const double * betas, const double * f, const double * ut)
void upnp::compute_ccs(const double * betas, const double * ut)
{
for(int i = 0; i < 4; ++i)
ccs[i][0] = ccs[i][1] = ccs[i][2] = 0.0f;
@@ -270,10 +259,10 @@ void upnp::compute_ccs(const double * betas, const double * f, const double * ut
const double * v = ut + 12 * (9 + i);
for(int j = 0; j < 4; ++j)
for(int k = 0; k < 3; ++k)
ccs[j][k] += betas[i] * v[3 * j + k]; // be careful with that
ccs[j][k] += betas[i] * v[3 * j + k];
}
for (int i = 0; i < 4; ++i) ccs[i][2] *= f[0];
for (int i = 0; i < 4; ++i) ccs[i][2] *= fu;
}
void upnp::compute_pcs(void)
@@ -314,18 +303,44 @@ void upnp::find_betas_and_focal_approx_2(const CvMat * Ut, const CvMat * Rho, do
cv::Mat Kmf2 = cv::Mat(12, 1, CV_64F, Ut->data.db + 11 * 12);
cv::Mat dsq = cv::Mat(6, 1, CV_64F, Rho->data.db);
cv::Mat D = compute_constraint_distance_3param_6eq_6unk_f_unk( Kmf1, Kmf2 ); // OKAY
cv::Mat D = compute_constraint_distance_3param_6eq_6unk_f_unk( Kmf1, Kmf2 );
cv::Mat A = D;
cv::Mat b = dsq;
cv::Mat x = cv::Mat(6, 1, CV_64F);
double x[6];
cv::Mat X = cv::Mat(6, 1, CV_64F, x);
cv::solve(A, b, x, cv::DECOMP_QR);
cv::solve(A, b, X, cv::DECOMP_QR);
betas[0] = std::sqrt( std::abs( x.at<double>(0) ) );
betas[1] = std::sqrt( std::abs( x.at<double>(2) ) * ( x.at<double>(2) < 0 ) ? -1 : ( x.at<double>(2) > 0 ) ? 1 : 0 );
double solutions[18][3];
generate_all_possible_solutions_for_f_unk(x, solutions);
// find solution with minimum reprojection error
double min_error = std::numeric_limits<double>::max();
int min_sol = 0;
for (int i = 0; i < 18; ++i) {
betas[3] = solutions[i][0];
betas[2] = solutions[i][1];
betas[1] = betas[0] = 0;
fu = fv = solutions[i][2];
double Rs[3][3], ts[3];
double error_i = compute_R_and_t( Ut->data.db, betas, Rs, ts);
if( error_i < min_error)
{
min_error = error_i;
min_sol = i;
}
}
betas[0] = solutions[min_sol][0];
betas[1] = solutions[min_sol][1];
betas[2] = betas[3] = 0;
efs[0] = solutions[min_sol][2];
}
cv::Mat upnp::compute_constraint_distance_2param_6eq_2unk_f_unk(const cv::Mat& M1)
@@ -457,9 +472,51 @@ cv::Mat upnp::compute_constraint_distance_3param_6eq_6unk_f_unk(const cv::Mat& M
return P;
}
void upnp::generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3])
{
int matrix_to_resolve[18][9] = {
{ 2, 0, 0, 1, 1, 0, 2, 0, 2 }, { 2, 0, 0, 1, 1, 0, 1, 1, 2 },
{ 2, 0, 0, 1, 1, 0, 0, 2, 2 }, { 2, 0, 0, 0, 2, 0, 2, 0, 2 },
{ 2, 0, 0, 0, 2, 0, 1, 1, 2 }, { 2, 0, 0, 0, 2, 0, 0, 2, 2 },
{ 2, 0, 0, 2, 0, 2, 1, 1, 2 }, { 2, 0, 0, 2, 0, 2, 0, 2, 2 },
{ 2, 0, 0, 1, 1, 2, 0, 2, 2 }, { 1, 1, 0, 0, 2, 0, 2, 0, 2 },
{ 1, 1, 0, 0, 2, 0, 1, 1, 2 }, { 1, 1, 0, 2, 0, 2, 0, 2, 2 },
{ 1, 1, 0, 2, 0, 2, 1, 1, 2 }, { 1, 1, 0, 2, 0, 2, 0, 2, 2 },
{ 1, 1, 0, 1, 1, 2, 0, 2, 2 }, { 0, 2, 0, 2, 0, 2, 1, 1, 2 },
{ 0, 2, 0, 2, 0, 2, 0, 2, 2 }, { 0, 2, 0, 1, 1, 2, 0, 2, 2 }
};
int combination[18][3] = {
{ 1, 2, 4 }, { 1, 2, 5 }, { 1, 2, 6 }, { 1, 3, 4 },
{ 1, 3, 5 }, { 1, 3, 6 }, { 1, 4, 5 }, { 1, 4, 6 },
{ 1, 5, 6 }, { 2, 3, 4 }, { 2, 3, 5 }, { 2, 3, 6 },
{ 2, 4, 5 }, { 2, 4, 6 }, { 2, 5, 6 }, { 3, 4, 5 },
{ 3, 4, 6 }, { 3, 5, 6 }
};
for (int i = 0; i < 18; ++i) {
double matrix[9], independent_term[3];
cv::Mat M = cv::Mat(3, 3, CV_64F, matrix);
cv::Mat I = cv::Mat(3, 1, CV_64F, independent_term);
cv::Mat S = cv::Mat(1, 3, CV_64F);
for (int j = 0; j < 9; ++j) matrix[j] = (double)matrix_to_resolve[i][j];
independent_term[0] = std::log( std::abs( betas[ combination[i][0]-1 ] ) );
independent_term[1] = std::log( std::abs( betas[ combination[i][1]-1 ] ) );
independent_term[2] = std::log( std::abs( betas[ combination[i][2]-1 ] ) );
cv::exp( cv::Mat(M.inv() * I), S);
solutions[i][0] = S.at<double>(0);
solutions[i][1] = S.at<double>(1) * sign( betas[1] );
solutions[i][2] = std::abs( S.at<double>(2) );
}
}
void upnp::gauss_newton(const CvMat * L_6x12, const CvMat * Rho, double betas[4], double * f)
{
const int iterations_number = 100;
const int iterations_number = 50;
double a[6*4], b[6], x[4];
CvMat A = cvMat(6, 4, CV_64F, a);
@@ -476,6 +533,8 @@ void upnp::gauss_newton(const CvMat * L_6x12, const CvMat * Rho, double betas[4]
}
if (f[0] < 0) f[0] = -f[0];
fu = fv = f[0];
}
void upnp::compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho,
@@ -587,6 +646,11 @@ double upnp::dotZ(const double * v1, const double * v2)
return v1[2] * v2[2];
}
double upnp::sign(const double v)
{
return ( v < 0 ) ? -1. : ( v > 0 ) ? 1. : 0.;
}
void upnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
{
const int nr = A->rows;

6
modules/calib3d/src/upnp.h
View File

@@ -41,7 +41,7 @@ private:
void choose_control_points();
void compute_alphas();
void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v);
void compute_ccs(const double * betas, const double * f, const double * ut);
void compute_ccs(const double * betas, const double * ut);
void compute_pcs(void);
void solve_for_sign(void);
@@ -52,7 +52,9 @@ private:
cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk(const cv::Mat& M1);
cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk(const cv::Mat& M1, const cv::Mat& M2);
void generate_all_possible_solutions_for_f_unk(const double betas_[5], double solutions[18][3]);
double sign(const double v);
double dot(const double * v1, const double * v2);
double dotXY(const double * v1, const double * v2);
double dotZ(const double * v1, const double * v2);
@@ -65,7 +67,7 @@ private:
void compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho,
const double cb[4], CvMat * A, CvMat * b, double const f);
double compute_R_and_t(const double * ut, const double * betas, const double * efs,
double compute_R_and_t(const double * ut, const double * betas,
double R[3][3], double t[3]);
void estimate_R_and_t(double R[3][3], double t[3]);