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) 2015, OpenCV Foundation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
21 //
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
25 //
26 // * The name of the copyright holders may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41
42 #include "test_precomp.hpp"
43
44 namespace opencv_test { namespace {
45
46 const string STRUCTURED_LIGHT_DIR = "structured_light";
47 const string FOLDER_DATA = "data";
48
49 /****************************************************************************************\
50 * Plane test *
51 \****************************************************************************************/
52 class CV_PlaneTest : public cvtest::BaseTest
53 {
54 public:
55 CV_PlaneTest();
56 ~CV_PlaneTest();
57
58 //////////////////////////////////////////////////////////////////////////////////////////////////
59 // From rgbd module: since I needed the distance method of plane class, I copied the class from rgb module
60 // it will be made a pull request to make Plane class public
61
62 /** Structure defining a plane. The notations are from the second paper */
63 class PlaneBase
64 {
65 public:
PlaneBase(const Vec3f & m,const Vec3f & n_in,int index)66 PlaneBase(const Vec3f & m, const Vec3f &n_in, int index) :
67 index_(index),
68 n_(n_in),
69 m_sum_(Vec3f(0, 0, 0)),
70 m_(m),
71 Q_(Matx33f::zeros()),
72 mse_(0),
73 K_(0)
74 {
75 UpdateD();
76 }
77
~PlaneBase()78 virtual ~PlaneBase()
79 {
80 }
81
82 /** Compute the distance to the plane. This will be implemented by the children to take into account different
83 * sensor models
84 * @param p_j
85 * @return
86 */
87 virtual
88 float
89 distance(const Vec3f& p_j) const = 0;
90
91 /** The d coefficient in the plane equation ax+by+cz+d = 0
92 * @return
93 */
d() const94 inline float d() const
95 {
96 return d_;
97 }
98
99 /** The normal to the plane
100 * @return the normal to the plane
101 */
102 const Vec3f &
n() const103 n() const
104 {
105 return n_;
106 }
107
108 /** Update the different coefficients of the plane, based on the new statistics
109 */
UpdateParameters()110 void UpdateParameters()
111 {
112 if( empty() )
113 return;
114 m_ = m_sum_ / K_;
115 // Compute C
116 Matx33f C = Q_ - m_sum_ * m_.t();
117
118 // Compute n
119 SVD svd(C);
120 n_ = Vec3f(svd.vt.at<float>(2, 0), svd.vt.at<float>(2, 1), svd.vt.at<float>(2, 2));
121 mse_ = svd.w.at<float>(2) / K_;
122
123 UpdateD();
124 }
125
126 /** Update the different sum of point and sum of point*point.t()
127 */
UpdateStatistics(const Vec3f & point,const Matx33f & Q_local)128 void UpdateStatistics(const Vec3f & point, const Matx33f & Q_local)
129 {
130 m_sum_ += point;
131 Q_ += Q_local;
132 ++K_;
133 }
134
empty() const135 inline size_t empty() const
136 {
137 return K_ == 0;
138 }
139
K() const140 inline int K() const
141 {
142 return K_;
143 }
144 /** The index of the plane */
145 int index_;
146 protected:
147 /** The 4th coefficient in the plane equation ax+by+cz+d = 0 */
148 float d_;
149 /** Normal of the plane */
150 Vec3f n_;
151 private:
UpdateD()152 inline void UpdateD()
153 {
154 // Hessian form (d = nc . p_plane (centroid here) + p)
155 //d = -1 * n.dot (xyz_centroid);//d =-axP+byP+czP
156 d_ = -m_.dot(n_);
157 }
158 /** The sum of the points */
159 Vec3f m_sum_;
160 /** The mean of the points */
161 Vec3f m_;
162 /** The sum of pi * pi^\top */
163 Matx33f Q_;
164 /** The different matrices we need to update */
165 Matx33f C_;
166 float mse_;
167 /** the number of points that form the plane */
168 int K_;
169 };
170
171 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
172
173 /** Basic planar child, with no sensor error model
174 */
175 class Plane : public PlaneBase
176 {
177 public:
Plane(const Vec3f & m,const Vec3f & n_in,int index)178 Plane(const Vec3f & m, const Vec3f &n_in, int index) :
179 PlaneBase(m, n_in, index)
180 {
181 }
182
183 /** The computed distance is perfect in that case
184 * @param p_j the point to compute its distance to
185 * @return
186 */
distance(const Vec3f & p_j) const187 float distance(const Vec3f& p_j) const
188 {
189 return std::abs(float(p_j.dot(n_) + d_));
190 }
191 };
192 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
193
194 protected:
195 void run( int );
196
197 };
198
CV_PlaneTest()199 CV_PlaneTest::CV_PlaneTest(){}
200
~CV_PlaneTest()201 CV_PlaneTest::~CV_PlaneTest(){}
202
run(int)203 void CV_PlaneTest::run( int )
204 {
205 string folder = cvtest::TS::ptr()->get_data_path() + "/" + STRUCTURED_LIGHT_DIR + "/" + FOLDER_DATA + "/";
206 structured_light::GrayCodePattern::Params params;
207 params.width = 1280;
208 params.height = 800;
209 // Set up GraycodePattern with params
210 Ptr<structured_light::GrayCodePattern> graycode = structured_light::GrayCodePattern::create( params );
211 size_t numberOfPatternImages = graycode->getNumberOfPatternImages();
212
213
214 FileStorage fs( folder + "calibrationParameters.yml", FileStorage::READ );
215 if( !fs.isOpened() )
216 {
217 ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
218 }
219
220 FileStorage fs2( folder + "gt_plane.yml", FileStorage::READ );
221 if( !fs.isOpened() )
222 {
223 ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
224 }
225
226 // Loading ground truth plane parameters
227 Vec4f plane_coefficients;
228 Vec3f m;
229 fs2["plane_coefficients"] >> plane_coefficients;
230 fs2["m"] >> m;
231
232 // Loading calibration parameters
233 Mat cam1intrinsics, cam1distCoeffs, cam2intrinsics, cam2distCoeffs, R, T;
234
235 fs["cam1_intrinsics"] >> cam1intrinsics;
236 fs["cam2_intrinsics"] >> cam2intrinsics;
237 fs["cam1_distorsion"] >> cam1distCoeffs;
238 fs["cam2_distorsion"] >> cam2distCoeffs;
239 fs["R"] >> R;
240 fs["T"] >> T;
241
242 // Loading white and black images
243 vector<Mat> blackImages;
244 vector<Mat> whiteImages;
245
246 blackImages.resize( 2 );
247 whiteImages.resize( 2 );
248
249 whiteImages[0] = imread( folder + "pattern_cam1_im43.jpg", 0 );
250 whiteImages[1] = imread( folder + "pattern_cam2_im43.jpg", 0 );
251 blackImages[0] = imread( folder + "pattern_cam1_im44.jpg", 0 );
252 blackImages[1] = imread( folder + "pattern_cam2_im44.jpg", 0 );
253
254 Size imagesSize = whiteImages[0].size();
255
256 if( ( !cam1intrinsics.data ) || ( !cam2intrinsics.data ) || ( !cam1distCoeffs.data ) || ( !cam2distCoeffs.data ) || ( !R.data )
257 || ( !T.data ) || ( !whiteImages[0].data ) || ( !whiteImages[1].data ) || ( !blackImages[0].data )
258 || ( !blackImages[1].data ) )
259 {
260 ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
261 }
262
263 // Computing stereo rectify parameters
264 Mat R1, R2, P1, P2, Q;
265 Rect validRoi[2];
266 stereoRectify( cam1intrinsics, cam1distCoeffs, cam2intrinsics, cam2distCoeffs, imagesSize, R, T, R1, R2, P1, P2, Q, 0,
267 -1, imagesSize, &validRoi[0], &validRoi[1] );
268
269 Mat map1x, map1y, map2x, map2y;
270 initUndistortRectifyMap( cam1intrinsics, cam1distCoeffs, R1, P1, imagesSize, CV_32FC1, map1x, map1y );
271 initUndistortRectifyMap( cam2intrinsics, cam2distCoeffs, R2, P2, imagesSize, CV_32FC1, map2x, map2y );
272
273 vector<vector<Mat> > captured_pattern;
274 captured_pattern.resize( 2 );
275 captured_pattern[0].resize( numberOfPatternImages );
276 captured_pattern[1].resize( numberOfPatternImages );
277
278 // Loading and rectifying pattern images
279 for( size_t i = 0; i < numberOfPatternImages; i++ )
280 {
281 std::ostringstream name1;
282 name1 << "pattern_cam1_im" << i + 1 << ".jpg";
283 captured_pattern[0][i] = imread( folder + name1.str(), 0 );
284 std::ostringstream name2;
285 name2 << "pattern_cam2_im" << i + 1 << ".jpg";
286 captured_pattern[1][i] = imread( folder + name2.str(), 0 );
287
288 if( (!captured_pattern[0][i].data) || (!captured_pattern[1][i].data) )
289 {
290 ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
291 }
292
293 remap( captured_pattern[0][i], captured_pattern[0][i], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
294 remap( captured_pattern[1][i], captured_pattern[1][i], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
295 }
296
297 // Rectifying white and black images
298 remap( whiteImages[0], whiteImages[0], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
299 remap( whiteImages[1], whiteImages[1], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
300
301 remap( blackImages[0], blackImages[0], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
302 remap( blackImages[1], blackImages[1], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
303
304 // Setting up threshold parameters to reconstruct only the plane in foreground
305 graycode->setBlackThreshold( 55 );
306 graycode->setWhiteThreshold( 10 );
307
308 // Computing the disparity map
309 Mat disparityMap;
310 bool decoded = graycode->decode( captured_pattern, disparityMap, blackImages, whiteImages,
311 structured_light::DECODE_3D_UNDERWORLD );
312 EXPECT_TRUE( decoded );
313
314 // Computing the point cloud
315 Mat pointcloud;
316 disparityMap.convertTo( disparityMap, CV_32FC1 );
317 reprojectImageTo3D( disparityMap, pointcloud, Q, true, -1 );
318 // from mm (unit of calibration) to m
319 pointcloud = pointcloud / 1000;
320
321 // Setting up plane with ground truth plane values
322 Vec3f normal( plane_coefficients.val[0], plane_coefficients.val[1], plane_coefficients.val[2] );
323 Ptr<PlaneBase> plane = Ptr<PlaneBase>( new Plane( m, normal, 0 ) );
324
325 // Computing the distance of every point of the pointcloud from ground truth plane
326 float sum_d = 0;
327 int cont = 0;
328 for( int i = 0; i < disparityMap.rows; i++ )
329 {
330 for( int j = 0; j < disparityMap.cols; j++ )
331 {
332 float value = disparityMap.at<float>( i, j );
333 if( value != 0 )
334 {
335 Vec3f point = pointcloud.at<Vec3f>( i, j );
336 sum_d += plane->distance( point );
337 cont++;
338 }
339 }
340 }
341
342 sum_d /= cont;
343
344 // test pass if the mean of points distance from ground truth plane is lower than 3 mm
345 EXPECT_LE( sum_d, 0.003 );
346 }
347
348 /****************************************************************************************\
349 * Test registration *
350 \****************************************************************************************/
351
TEST(GrayCodePattern,plane_reconstruction)352 TEST( GrayCodePattern, plane_reconstruction )
353 {
354 CV_PlaneTest test;
355 test.safe_run();
356 }
357
358 }} // namespace
359