1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
15 // Third party copyrights are property of their respective owners.
16 //
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
19 //
20 // * Redistribution's of source code must retain the above copyright notice,
21 // this list of conditions and the following disclaimer.
22 //
23 // * Redistribution's in binary form must reproduce the above copyright notice,
24 // this list of conditions and the following disclaimer in the documentation
25 // and/or other materials provided with the distribution.
26 //
27 // * The name of the copyright holders may not be used to endorse or promote products
28 // derived from this software without specific prior written permission.
29 //
30 // This software is provided by the copyright holders and contributors "as is" and
31 // any express or implied warranties, including, but not limited to, the implied
32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
33 // In no event shall the Intel Corporation or contributors be liable for any direct,
34 // indirect, incidental, special, exemplary, or consequential damages
35 // (including, but not limited to, procurement of substitute goods or services;
36 // loss of use, data, or profits; or business interruption) however caused
37 // and on any theory of liability, whether in contract, strict liability,
38 // or tort (including negligence or otherwise) arising in any way out of
39 // the use of this software, even if advised of the possibility of such damage.
40 //
41 //M*/
42
43 #include "test_precomp.hpp"
44
45 using namespace cv;
46 using namespace std;
47
48 class Differential
49 {
50 public:
51 typedef Mat_<double> mat_t;
52
Differential(double eps_,const mat_t & rv1_,const mat_t & tv1_,const mat_t & rv2_,const mat_t & tv2_)53 Differential(double eps_, const mat_t& rv1_, const mat_t& tv1_, const mat_t& rv2_, const mat_t& tv2_)
54 : rv1(rv1_), tv1(tv1_), rv2(rv2_), tv2(tv2_), eps(eps_), ev(3, 1) {}
55
dRv1(mat_t & dr3_dr1,mat_t & dt3_dr1)56 void dRv1(mat_t& dr3_dr1, mat_t& dt3_dr1)
57 {
58 dr3_dr1.create(3, 3); dt3_dr1.create(3, 3);
59
60 for(int i = 0; i < 3; ++i)
61 {
62 ev.setTo(Scalar(0)); ev(i, 0) = eps;
63
64 composeRT( rv1 + ev, tv1, rv2, tv2, rv3_p, tv3_p);
65 composeRT( rv1 - ev, tv1, rv2, tv2, rv3_m, tv3_m);
66
67 dr3_dr1.col(i) = rv3_p - rv3_m;
68 dt3_dr1.col(i) = tv3_p - tv3_m;
69 }
70 dr3_dr1 /= 2 * eps; dt3_dr1 /= 2 * eps;
71 }
72
dRv2(mat_t & dr3_dr2,mat_t & dt3_dr2)73 void dRv2(mat_t& dr3_dr2, mat_t& dt3_dr2)
74 {
75 dr3_dr2.create(3, 3); dt3_dr2.create(3, 3);
76
77 for(int i = 0; i < 3; ++i)
78 {
79 ev.setTo(Scalar(0)); ev(i, 0) = eps;
80
81 composeRT( rv1, tv1, rv2 + ev, tv2, rv3_p, tv3_p);
82 composeRT( rv1, tv1, rv2 - ev, tv2, rv3_m, tv3_m);
83
84 dr3_dr2.col(i) = rv3_p - rv3_m;
85 dt3_dr2.col(i) = tv3_p - tv3_m;
86 }
87 dr3_dr2 /= 2 * eps; dt3_dr2 /= 2 * eps;
88 }
89
dTv1(mat_t & drt3_dt1,mat_t & dt3_dt1)90 void dTv1(mat_t& drt3_dt1, mat_t& dt3_dt1)
91 {
92 drt3_dt1.create(3, 3); dt3_dt1.create(3, 3);
93
94 for(int i = 0; i < 3; ++i)
95 {
96 ev.setTo(Scalar(0)); ev(i, 0) = eps;
97
98 composeRT( rv1, tv1 + ev, rv2, tv2, rv3_p, tv3_p);
99 composeRT( rv1, tv1 - ev, rv2, tv2, rv3_m, tv3_m);
100
101 drt3_dt1.col(i) = rv3_p - rv3_m;
102 dt3_dt1.col(i) = tv3_p - tv3_m;
103 }
104 drt3_dt1 /= 2 * eps; dt3_dt1 /= 2 * eps;
105 }
106
dTv2(mat_t & dr3_dt2,mat_t & dt3_dt2)107 void dTv2(mat_t& dr3_dt2, mat_t& dt3_dt2)
108 {
109 dr3_dt2.create(3, 3); dt3_dt2.create(3, 3);
110
111 for(int i = 0; i < 3; ++i)
112 {
113 ev.setTo(Scalar(0)); ev(i, 0) = eps;
114
115 composeRT( rv1, tv1, rv2, tv2 + ev, rv3_p, tv3_p);
116 composeRT( rv1, tv1, rv2, tv2 - ev, rv3_m, tv3_m);
117
118 dr3_dt2.col(i) = rv3_p - rv3_m;
119 dt3_dt2.col(i) = tv3_p - tv3_m;
120 }
121 dr3_dt2 /= 2 * eps; dt3_dt2 /= 2 * eps;
122 }
123
124 private:
125 const mat_t& rv1, tv1, rv2, tv2;
126 double eps;
127 Mat_<double> ev;
128
129 Differential& operator=(const Differential&);
130 Mat rv3_m, tv3_m, rv3_p, tv3_p;
131 };
132
133 class CV_composeRT_Test : public cvtest::BaseTest
134 {
135 public:
CV_composeRT_Test()136 CV_composeRT_Test() {}
~CV_composeRT_Test()137 ~CV_composeRT_Test() {}
138 protected:
139
run(int)140 void run(int)
141 {
142 ts->set_failed_test_info(cvtest::TS::OK);
143
144 Mat_<double> rvec1(3, 1), tvec1(3, 1), rvec2(3, 1), tvec2(3, 1);
145
146 randu(rvec1, Scalar(0), Scalar(6.29));
147 randu(rvec2, Scalar(0), Scalar(6.29));
148
149 randu(tvec1, Scalar(-2), Scalar(2));
150 randu(tvec2, Scalar(-2), Scalar(2));
151
152 Mat rvec3, tvec3;
153 composeRT(rvec1, tvec1, rvec2, tvec2, rvec3, tvec3);
154
155 Mat rvec3_exp, tvec3_exp;
156
157 Mat rmat1, rmat2;
158 Rodrigues(rvec1, rmat1);
159 Rodrigues(rvec2, rmat2);
160 Rodrigues(rmat2 * rmat1, rvec3_exp);
161
162 tvec3_exp = rmat2 * tvec1 + tvec2;
163
164 const double thres = 1e-5;
165 if (norm(rvec3_exp, rvec3) > thres || norm(tvec3_exp, tvec3) > thres)
166 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
167
168 const double eps = 1e-3;
169 Differential diff(eps, rvec1, tvec1, rvec2, tvec2);
170
171 Mat dr3dr1, dr3dt1, dr3dr2, dr3dt2, dt3dr1, dt3dt1, dt3dr2, dt3dt2;
172
173 composeRT(rvec1, tvec1, rvec2, tvec2, rvec3, tvec3,
174 dr3dr1, dr3dt1, dr3dr2, dr3dt2, dt3dr1, dt3dt1, dt3dr2, dt3dt2);
175
176 Mat_<double> dr3_dr1, dt3_dr1;
177 diff.dRv1(dr3_dr1, dt3_dr1);
178
179 if (norm(dr3_dr1, dr3dr1) > thres || norm(dt3_dr1, dt3dr1) > thres)
180 {
181 ts->printf( cvtest::TS::LOG, "Invalid derivates by r1\n" );
182 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
183 }
184
185 Mat_<double> dr3_dr2, dt3_dr2;
186 diff.dRv2(dr3_dr2, dt3_dr2);
187
188 if (norm(dr3_dr2, dr3dr2) > thres || norm(dt3_dr2, dt3dr2) > thres)
189 {
190 ts->printf( cvtest::TS::LOG, "Invalid derivates by r2\n" );
191 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
192 }
193
194 Mat_<double> dr3_dt1, dt3_dt1;
195 diff.dTv1(dr3_dt1, dt3_dt1);
196
197 if (norm(dr3_dt1, dr3dt1) > thres || norm(dt3_dt1, dt3dt1) > thres)
198 {
199 ts->printf( cvtest::TS::LOG, "Invalid derivates by t1\n" );
200 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
201 }
202
203 Mat_<double> dr3_dt2, dt3_dt2;
204 diff.dTv2(dr3_dt2, dt3_dt2);
205
206 if (norm(dr3_dt2, dr3dt2) > thres || norm(dt3_dt2, dt3dt2) > thres)
207 {
208 ts->printf( cvtest::TS::LOG, "Invalid derivates by t2\n" );
209 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
210 }
211 }
212 };
213
TEST(Calib3d_ComposeRT,accuracy)214 TEST(Calib3d_ComposeRT, accuracy) { CV_composeRT_Test test; test.safe_run(); }
215