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

Warning fixes continued

Browse Source
This commit is contained in:
Andrey Kamaev
2012-06-09 15:00:04 +00:00
parent f6b451c607
commit f2d3b9b4a1
127 changed files with 6298 additions and 6277 deletions
Download Patch File Download Diff File Expand all files Collapse all files

166
modules/calib3d/src/epnp.cpp
View File

@@ -8,26 +8,26 @@ epnp::epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& i
if (cameraMatrix.depth() == CV_32F)
init_camera_parameters<float>(cameraMatrix);
else
init_camera_parameters<double>(cameraMatrix);
init_camera_parameters<double>(cameraMatrix);
number_of_correspondences = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
pws.resize(3 * number_of_correspondences);
us.resize(2 * number_of_correspondences);
us.resize(2 * number_of_correspondences);
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
init_points<cv::Point3f,cv::Point2f>(opoints, ipoints);
else
init_points<cv::Point3d,cv::Point2d>(opoints, ipoints);
if (opoints.depth() == CV_32F)
init_points<cv::Point3f,cv::Point2f>(opoints, ipoints);
else
init_points<cv::Point3d,cv::Point2d>(opoints, ipoints);
}
else if (opoints.depth() == CV_32F)
init_points<cv::Point3f,cv::Point2d>(opoints, ipoints);
init_points<cv::Point3f,cv::Point2d>(opoints, ipoints);
else
init_points<cv::Point3d,cv::Point2f>(opoints, ipoints);
init_points<cv::Point3d,cv::Point2f>(opoints, ipoints);
alphas.resize(4 * number_of_correspondences);
alphas.resize(4 * number_of_correspondences);
pcs.resize(3 * number_of_correspondences);
max_nr = 0;
@@ -97,15 +97,15 @@ void epnp::compute_barycentric_coordinates(void)
for(int j = 0; j < 3; j++)
a[1 + j] =
ci[3 * j ] * (pi[0] - cws[0][0]) +
ci[3 * j + 1] * (pi[1] - cws[0][1]) +
ci[3 * j + 2] * (pi[2] - cws[0][2]);
ci[3 * j ] * (pi[0] - cws[0][0]) +
ci[3 * j + 1] * (pi[1] - cws[0][1]) +
ci[3 * j + 2] * (pi[2] - cws[0][2]);
a[0] = 1.0f - a[1] - a[2] - a[3];
}
}
void epnp::fill_M(CvMat * M,
const int row, const double * as, const double u, const double v)
const int row, const double * as, const double u, const double v)
{
double * M1 = M->data.db + row * 12;
double * M2 = M1 + 12;
@@ -130,7 +130,7 @@ void epnp::compute_ccs(const double * betas, const double * ut)
const double * v = ut + 12 * (11 - i);
for(int j = 0; j < 4; j++)
for(int k = 0; k < 3; k++)
ccs[j][k] += betas[i] * v[3 * j + k];
ccs[j][k] += betas[i] * v[3 * j + k];
}
}
@@ -195,7 +195,7 @@ void epnp::compute_pose(cv::Mat& R, cv::Mat& t)
}
void epnp::copy_R_and_t(const double R_src[3][3], const double t_src[3],
double R_dst[3][3], double t_dst[3])
double R_dst[3][3], double t_dst[3])
{
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++)
@@ -282,7 +282,7 @@ void epnp::solve_for_sign(void)
if (pcs[2] < 0.0) {
for(int i = 0; i < 4; i++)
for(int j = 0; j < 3; j++)
ccs[i][j] = -ccs[i][j];
ccs[i][j] = -ccs[i][j];
for(int i = 0; i < number_of_correspondences; i++) {
pcs[3 * i ] = -pcs[3 * i];
@@ -293,7 +293,7 @@ void epnp::solve_for_sign(void)
}
double epnp::compute_R_and_t(const double * ut, const double * betas,
double R[3][3], double t[3])
double R[3][3], double t[3])
{
compute_ccs(betas, ut);
compute_pcs();
@@ -322,13 +322,13 @@ double epnp::reprojection_error(const double R[3][3], const double t[3])
}
return sum2 / number_of_correspondences;
}
}
// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
// betas_approx_1 = [B11 B12 B13 B14]
void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
double * betas)
{
double l_6x4[6 * 4], b4[4];
CvMat L_6x4 = cvMat(6, 4, CV_64F, l_6x4);
@@ -360,7 +360,7 @@ void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,
// betas_approx_2 = [B11 B12 B22 ]
void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
double * betas)
{
double l_6x3[6 * 3], b3[3];
CvMat L_6x3 = cvMat(6, 3, CV_64F, l_6x3);
@@ -392,7 +392,7 @@ void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,
// betas_approx_3 = [B11 B12 B22 B13 B23 ]
void epnp::find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
double * betas)
{
double l_6x5[6 * 5], b5[5];
CvMat L_6x5 = cvMat(6, 5, CV_64F, l_6x5);
@@ -440,8 +440,8 @@ void epnp::compute_L_6x10(const double * ut, double * l_6x10)
b++;
if (b > 3) {
a++;
b = a + 1;
a++;
b = a + 1;
}
}
}
@@ -473,7 +473,7 @@ void epnp::compute_rho(double * rho)
}
void epnp::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
const double betas[4], CvMat * A, CvMat * b)
const double betas[4], CvMat * A, CvMat * b)
{
for(int i = 0; i < 6; i++) {
const double * rowL = l_6x10 + i * 10;
@@ -485,23 +485,22 @@ void epnp::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rh
rowA[3] = rowL[6] * betas[0] + rowL[7] * betas[1] + rowL[8] * betas[2] + 2 * rowL[9] * betas[3];
cvmSet(b, i, 0, rho[i] -
(
rowL[0] * betas[0] * betas[0] +
rowL[1] * betas[0] * betas[1] +
rowL[2] * betas[1] * betas[1] +
rowL[3] * betas[0] * betas[2] +
rowL[4] * betas[1] * betas[2] +
rowL[5] * betas[2] * betas[2] +
rowL[6] * betas[0] * betas[3] +
rowL[7] * betas[1] * betas[3] +
rowL[8] * betas[2] * betas[3] +
rowL[9] * betas[3] * betas[3]
));
(
rowL[0] * betas[0] * betas[0] +
rowL[1] * betas[0] * betas[1] +
rowL[2] * betas[1] * betas[1] +
rowL[3] * betas[0] * betas[2] +
rowL[4] * betas[1] * betas[2] +
rowL[5] * betas[2] * betas[2] +
rowL[6] * betas[0] * betas[3] +
rowL[7] * betas[1] * betas[3] +
rowL[8] * betas[2] * betas[3] +
rowL[9] * betas[3] * betas[3]
));
}
}
void epnp::gauss_newton(const CvMat * L_6x10, const CvMat * Rho,
double betas[4])
void epnp::gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double betas[4])
{
const int iterations_number = 5;
@@ -510,12 +509,13 @@ void epnp::gauss_newton(const CvMat * L_6x10, const CvMat * Rho,
CvMat B = cvMat(6, 1, CV_64F, b);
CvMat X = cvMat(4, 1, CV_64F, x);
for(int k = 0; k < iterations_number; k++) {
for(int k = 0; k < iterations_number; k++)
{
compute_A_and_b_gauss_newton(L_6x10->data.db, Rho->data.db,
betas, &A, &B);
betas, &A, &B);
qr_solve(&A, &B, &X);
for(int i = 0; i < 4; i++)
betas[i] += x[i];
betas[i] += x[i];
}
}
@@ -524,53 +524,64 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
const int nr = A->rows;
const int nc = A->cols;
if (max_nr != 0 && max_nr < nr) {
if (max_nr != 0 && max_nr < nr)
{
delete [] A1;
delete [] A2;
}
if (max_nr < nr) {
if (max_nr < nr)
{
max_nr = nr;
A1 = new double[nr];
A2 = new double[nr];
}
double * pA = A->data.db, * ppAkk = pA;
for(int k = 0; k < nc; k++) {
double * ppAik = ppAkk, eta = fabs(*ppAik);
for(int i = k + 1; i < nr; i++) {
double elt = fabs(*ppAik);
for(int k = 0; k < nc; k++)
{
double * ppAik1 = ppAkk, eta = fabs(*ppAik1);
for(int i = k + 1; i < nr; i++)
{
double elt = fabs(*ppAik1);
if (eta < elt) eta = elt;
ppAik += nc;
ppAik1 += nc;
}
if (eta == 0) {
if (eta == 0)
{
A1[k] = A2[k] = 0.0;
//cerr << "God damnit, A is singular, this shouldn't happen." << endl;
return;
} else {
double * ppAik = ppAkk, sum = 0.0, inv_eta = 1. / eta;
for(int i = k; i < nr; i++) {
*ppAik *= inv_eta;
sum += *ppAik * *ppAik;
ppAik += nc;
}
else
{
double * ppAik2 = ppAkk, sum2 = 0.0, inv_eta = 1. / eta;
for(int i = k; i < nr; i++)
{
*ppAik2 *= inv_eta;
sum2 += *ppAik2 * *ppAik2;
ppAik2 += nc;
}
double sigma = sqrt(sum);
double sigma = sqrt(sum2);
if (*ppAkk < 0)
sigma = -sigma;
sigma = -sigma;
*ppAkk += sigma;
A1[k] = sigma * *ppAkk;
A2[k] = -eta * sigma;
for(int j = k + 1; j < nc; j++) {
double * ppAik = ppAkk, sum = 0;
for(int i = k; i < nr; i++) {
sum += *ppAik * ppAik[j - k];
ppAik += nc;
}
double tau = sum / A1[k];
ppAik = ppAkk;
for(int i = k; i < nr; i++) {
ppAik[j - k] -= tau * *ppAik;
ppAik += nc;
}
for(int j = k + 1; j < nc; j++)
{
double * ppAik = ppAkk, sum = 0;
for(int i = k; i < nr; i++)
{
sum += *ppAik * ppAik[j - k];
ppAik += nc;
}
double tau = sum / A1[k];
ppAik = ppAkk;
for(int i = k; i < nr; i++)
{
ppAik[j - k] -= tau * *ppAik;
ppAik += nc;
}
}
}
ppAkk += nc + 1;
@@ -578,15 +589,18 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
// b <- Qt b
double * ppAjj = pA, * pb = b->data.db;
for(int j = 0; j < nc; j++) {
for(int j = 0; j < nc; j++)
{
double * ppAij = ppAjj, tau = 0;
for(int i = j; i < nr; i++) {
for(int i = j; i < nr; i++)
{
tau += *ppAij * pb[i];
ppAij += nc;
}
tau /= A1[j];
ppAij = ppAjj;
for(int i = j; i < nr; i++) {
for(int i = j; i < nr; i++)
{
pb[i] -= tau * *ppAij;
ppAij += nc;
}
@@ -596,10 +610,12 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
// X = R-1 b
double * pX = X->data.db;
pX[nc - 1] = pb[nc - 1] / A2[nc - 1];
for(int i = nc - 2; i >= 0; i--) {
for(int i = nc - 2; i >= 0; i--)
{
double * ppAij = pA + i * nc + (i + 1), sum = 0;
for(int j = i + 1; j < nc; j++) {
for(int j = i + 1; j < nc; j++)
{
sum += *ppAij * pX[j];
ppAij++;
}