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 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 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 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 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 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: 136 CV_composeRT_Test() {} 137 ~CV_composeRT_Test() {} 138 protected: 139 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 214 TEST(Calib3d_ComposeRT, accuracy) { CV_composeRT_Test test; test.safe_run(); } 215
