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 namespace opencv_test { namespace {
46
47 class Differential
48 {
49 public:
50 typedef Mat_<double> mat_t;
51
Differential(double eps_,const mat_t & rv1_,const mat_t & tv1_,const mat_t & rv2_,const mat_t & tv2_)52 Differential(double eps_, const mat_t& rv1_, const mat_t& tv1_, const mat_t& rv2_, const mat_t& tv2_)
53 : rv1(rv1_), tv1(tv1_), rv2(rv2_), tv2(tv2_), eps(eps_), ev(3, 1) {}
54
dRv1(mat_t & dr3_dr1,mat_t & dt3_dr1)55 void dRv1(mat_t& dr3_dr1, mat_t& dt3_dr1)
56 {
57 dr3_dr1.create(3, 3); dt3_dr1.create(3, 3);
58
59 for(int i = 0; i < 3; ++i)
60 {
61 ev.setTo(Scalar(0)); ev(i, 0) = eps;
62
63 composeRT( rv1 + ev, tv1, rv2, tv2, rv3_p, tv3_p);
64 composeRT( rv1 - ev, tv1, rv2, tv2, rv3_m, tv3_m);
65
66 dr3_dr1.col(i) = rv3_p - rv3_m;
67 dt3_dr1.col(i) = tv3_p - tv3_m;
68 }
69 dr3_dr1 /= 2 * eps; dt3_dr1 /= 2 * eps;
70 }
71
dRv2(mat_t & dr3_dr2,mat_t & dt3_dr2)72 void dRv2(mat_t& dr3_dr2, mat_t& dt3_dr2)
73 {
74 dr3_dr2.create(3, 3); dt3_dr2.create(3, 3);
75
76 for(int i = 0; i < 3; ++i)
77 {
78 ev.setTo(Scalar(0)); ev(i, 0) = eps;
79
80 composeRT( rv1, tv1, rv2 + ev, tv2, rv3_p, tv3_p);
81 composeRT( rv1, tv1, rv2 - ev, tv2, rv3_m, tv3_m);
82
83 dr3_dr2.col(i) = rv3_p - rv3_m;
84 dt3_dr2.col(i) = tv3_p - tv3_m;
85 }
86 dr3_dr2 /= 2 * eps; dt3_dr2 /= 2 * eps;
87 }
88
dTv1(mat_t & drt3_dt1,mat_t & dt3_dt1)89 void dTv1(mat_t& drt3_dt1, mat_t& dt3_dt1)
90 {
91 drt3_dt1.create(3, 3); dt3_dt1.create(3, 3);
92
93 for(int i = 0; i < 3; ++i)
94 {
95 ev.setTo(Scalar(0)); ev(i, 0) = eps;
96
97 composeRT( rv1, tv1 + ev, rv2, tv2, rv3_p, tv3_p);
98 composeRT( rv1, tv1 - ev, rv2, tv2, rv3_m, tv3_m);
99
100 drt3_dt1.col(i) = rv3_p - rv3_m;
101 dt3_dt1.col(i) = tv3_p - tv3_m;
102 }
103 drt3_dt1 /= 2 * eps; dt3_dt1 /= 2 * eps;
104 }
105
dTv2(mat_t & dr3_dt2,mat_t & dt3_dt2)106 void dTv2(mat_t& dr3_dt2, mat_t& dt3_dt2)
107 {
108 dr3_dt2.create(3, 3); dt3_dt2.create(3, 3);
109
110 for(int i = 0; i < 3; ++i)
111 {
112 ev.setTo(Scalar(0)); ev(i, 0) = eps;
113
114 composeRT( rv1, tv1, rv2, tv2 + ev, rv3_p, tv3_p);
115 composeRT( rv1, tv1, rv2, tv2 - ev, rv3_m, tv3_m);
116
117 dr3_dt2.col(i) = rv3_p - rv3_m;
118 dt3_dt2.col(i) = tv3_p - tv3_m;
119 }
120 dr3_dt2 /= 2 * eps; dt3_dt2 /= 2 * eps;
121 }
122
123 private:
124 const mat_t& rv1, tv1, rv2, tv2;
125 double eps;
126 Mat_<double> ev;
127
128 Differential& operator=(const Differential&);
129 Mat rv3_m, tv3_m, rv3_p, tv3_p;
130 };
131
132 class CV_composeRT_Test : public cvtest::BaseTest
133 {
134 public:
CV_composeRT_Test()135 CV_composeRT_Test() {}
~CV_composeRT_Test()136 ~CV_composeRT_Test() {}
137 protected:
138
run(int)139 void run(int)
140 {
141 ts->set_failed_test_info(cvtest::TS::OK);
142
143 Mat_<double> rvec1(3, 1), tvec1(3, 1), rvec2(3, 1), tvec2(3, 1);
144
145 randu(rvec1, Scalar(0), Scalar(6.29));
146 randu(rvec2, Scalar(0), Scalar(6.29));
147
148 randu(tvec1, Scalar(-2), Scalar(2));
149 randu(tvec2, Scalar(-2), Scalar(2));
150
151 Mat rvec3, tvec3;
152 composeRT(rvec1, tvec1, rvec2, tvec2, rvec3, tvec3);
153
154 Mat rvec3_exp, tvec3_exp;
155
156 Mat rmat1, rmat2;
157 cv::Rodrigues(rvec1, rmat1); // TODO cvtest
158 cv::Rodrigues(rvec2, rmat2); // TODO cvtest
159 cv::Rodrigues(rmat2 * rmat1, rvec3_exp); // TODO cvtest
160
161 tvec3_exp = rmat2 * tvec1 + tvec2;
162
163 const double thres = 1e-5;
164 if (cv::norm(rvec3_exp, rvec3) > thres || cv::norm(tvec3_exp, tvec3) > thres)
165 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
166
167 const double eps = 1e-3;
168 Differential diff(eps, rvec1, tvec1, rvec2, tvec2);
169
170 Mat dr3dr1, dr3dt1, dr3dr2, dr3dt2, dt3dr1, dt3dt1, dt3dr2, dt3dt2;
171
172 composeRT(rvec1, tvec1, rvec2, tvec2, rvec3, tvec3,
173 dr3dr1, dr3dt1, dr3dr2, dr3dt2, dt3dr1, dt3dt1, dt3dr2, dt3dt2);
174
175 Mat_<double> dr3_dr1, dt3_dr1;
176 diff.dRv1(dr3_dr1, dt3_dr1);
177
178 if (cv::norm(dr3_dr1, dr3dr1) > thres || cv::norm(dt3_dr1, dt3dr1) > thres)
179 {
180 ts->printf( cvtest::TS::LOG, "Invalid derivates by r1\n" );
181 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
182 }
183
184 Mat_<double> dr3_dr2, dt3_dr2;
185 diff.dRv2(dr3_dr2, dt3_dr2);
186
187 if (cv::norm(dr3_dr2, dr3dr2) > thres || cv::norm(dt3_dr2, dt3dr2) > thres)
188 {
189 ts->printf( cvtest::TS::LOG, "Invalid derivates by r2\n" );
190 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
191 }
192
193 Mat_<double> dr3_dt1, dt3_dt1;
194 diff.dTv1(dr3_dt1, dt3_dt1);
195
196 if (cv::norm(dr3_dt1, dr3dt1) > thres || cv::norm(dt3_dt1, dt3dt1) > thres)
197 {
198 ts->printf( cvtest::TS::LOG, "Invalid derivates by t1\n" );
199 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
200 }
201
202 Mat_<double> dr3_dt2, dt3_dt2;
203 diff.dTv2(dr3_dt2, dt3_dt2);
204
205 if (cv::norm(dr3_dt2, dr3dt2) > thres || cv::norm(dt3_dt2, dt3dt2) > thres)
206 {
207 ts->printf( cvtest::TS::LOG, "Invalid derivates by t2\n" );
208 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
209 }
210 }
211 };
212
TEST(Calib3d_ComposeRT,accuracy)213 TEST(Calib3d_ComposeRT, accuracy) { CV_composeRT_Test test; test.safe_run(); }
214
215 }} // namespace
216