1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
|
// Copyright (c) 2009, V. Lepetit, EPFL
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright notice, this
// list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
// The views and conclusions contained in the software and documentation are those
// of the authors and should not be interpreted as representing official policies,
// either expressed or implied, of the FreeBSD Project.
#include "epnp.h"
#include "math.h"
#include "stdio.h"
#include "time.h"
const double uc = 320;
const double vc = 240;
const double fu = 800;
const double fv = 800;
// MtM takes more time than 12x12 opencv SVD with about 180 points and more:
const int n = 10;
const double noise = 10;
double epnp_rand(double min, double max) { return min + (max - min) * (double)(rand()) / RAND_MAX; }
void random_pose(double R[3][3], double t[3]) {
const double range = 1;
double phi = epnp_rand(0, range * 3.14159 * 2);
double theta = epnp_rand(0, range * 3.14159);
double psi = epnp_rand(0, range * 3.14159 * 2);
R[0][0] = cos(psi) * cos(phi) - cos(theta) * sin(phi) * sin(psi);
R[0][1] = cos(psi) * sin(phi) + cos(theta) * cos(phi) * sin(psi);
R[0][2] = sin(psi) * sin(theta);
R[1][0] = -sin(psi) * cos(phi) - cos(theta) * sin(phi) * cos(psi);
R[1][1] = -sin(psi) * sin(phi) + cos(theta) * cos(phi) * cos(psi);
R[1][2] = cos(psi) * sin(theta);
R[2][0] = sin(theta) * sin(phi);
R[2][1] = -sin(theta) * cos(phi);
R[2][2] = cos(theta);
t[0] = 0.0f;
t[1] = 0.0f;
t[2] = 6.0f;
}
void random_point(double *Xw, double *Yw, double *Zw) {
double theta = epnp_rand(0, 3.14159), phi = epnp_rand(0, 2 * 3.14159), R = epnp_rand(0, +2);
*Xw = sin(theta) * sin(phi) * R;
*Yw = -sin(theta) * cos(phi) * R;
*Zw = cos(theta) * R;
}
void project_with_noise(double R[3][3], double t[3], double Xw, double Yw, double Zw, double *u, double *v) {
double Xc = R[0][0] * Xw + R[0][1] * Yw + R[0][2] * Zw + t[0];
double Yc = R[1][0] * Xw + R[1][1] * Yw + R[1][2] * Zw + t[1];
double Zc = R[2][0] * Xw + R[2][1] * Yw + R[2][2] * Zw + t[2];
double nu = epnp_rand(-noise, +noise);
double nv = epnp_rand(-noise, +noise);
*u = uc + fu * Xc / Zc + nu;
*v = vc + fv * Yc / Zc + nv;
}
int main(int argc, char **argv) {
epnp PnP = {};
srand(0);
epnp_set_internal_parameters(&PnP, uc, vc, fu, fv);
epnp_set_maximum_number_of_correspondences(&PnP, n);
double R_true[3][3], t_true[3];
random_pose(R_true, t_true);
epnp_reset_correspondences(&PnP);
for (int i = 0; i < n; i++) {
double Xw, Yw, Zw, u, v;
random_point(&Xw, &Yw, &Zw);
printf("%f %f %f\n", Xw, Yw, Zw);
project_with_noise(R_true, t_true, Xw, Yw, Zw, &u, &v);
epnp_add_correspondence(&PnP, Xw, Yw, Zw, u, v);
}
printf("\n");
double R_est[3][3], t_est[3];
double err2 = epnp_compute_pose(&PnP, R_est, t_est);
double rot_err, transl_err;
relative_error(&rot_err, &transl_err, R_true, t_true, R_est, t_est);
printf("Reprojection error: %g\n", err2);
printf("rot_err: %g, %g \n", rot_err, transl_err);
printf("\n");
printf("'True reprojection error': %g\n\n", epnp_reprojection_error(&PnP, R_true, t_true));
printf("True pose:\n");
print_pose(R_true, t_true);
printf("\n");
printf("Found pose:\n");
print_pose(R_est, t_est);
return 0;
}
|
