1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
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.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2002, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistributions of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
22 // * Redistributions 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.
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
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.
42 #include "precomp.hpp"
43 #include "opencv2/calib3d/calib3d_c.h"
49 #define ARRAY_SIZEOF(a) (sizeof(a)/sizeof((a)[0]))
51 static void FillObjectPoints(CvPoint3D32f *obj_points, CvSize etalon_size, float square_size);
52 static void DrawEtalon(IplImage *img, CvPoint2D32f *corners,
53 int corner_count, CvSize etalon_size, int draw_ordered);
54 static void MultMatrix(float rm[4][4], const float m1[4][4], const float m2[4][4]);
55 static void MultVectorMatrix(float rv[4], const float v[4], const float m[4][4]);
56 static CvPoint3D32f ImageCStoWorldCS(const Cv3dTrackerCameraInfo &camera_info, CvPoint2D32f p);
57 static bool intersection(CvPoint3D32f o1, CvPoint3D32f p1,
58 CvPoint3D32f o2, CvPoint3D32f p2,
59 CvPoint3D32f &r1, CvPoint3D32f &r2);
61 /////////////////////////////////
62 // cv3dTrackerCalibrateCameras //
63 /////////////////////////////////
64 CV_IMPL CvBool cv3dTrackerCalibrateCameras(int num_cameras,
65 const Cv3dTrackerCameraIntrinsics camera_intrinsics[], // size is num_cameras
68 IplImage *samples[], // size is num_cameras
69 Cv3dTrackerCameraInfo camera_info[]) // size is num_cameras
71 CV_FUNCNAME("cv3dTrackerCalibrateCameras");
72 const int num_points = etalon_size.width * etalon_size.height;
73 int cameras_done = 0; // the number of cameras whose positions have been determined
74 CvPoint3D32f *object_points = NULL; // real-world coordinates of checkerboard points
75 CvPoint2D32f *points = NULL; // 2d coordinates of checkerboard points as seen by a camera
76 IplImage *gray_img = NULL; // temporary image for color conversion
77 IplImage *tmp_img = NULL; // temporary image used by FindChessboardCornerGuesses
80 if (etalon_size.width < 3 || etalon_size.height < 3)
81 CV_ERROR(CV_StsBadArg, "Chess board size is invalid");
83 for (c = 0; c < num_cameras; c++)
85 // CV_CHECK_IMAGE is not available in the cvaux library
86 // so perform the checks inline.
88 //CV_CALL(CV_CHECK_IMAGE(samples[c]));
90 if( samples[c] == NULL )
91 CV_ERROR( CV_HeaderIsNull, "Null image" );
93 if( samples[c]->dataOrder != IPL_DATA_ORDER_PIXEL && samples[c]->nChannels > 1 )
94 CV_ERROR( CV_BadOrder, "Unsupported image format" );
96 if( samples[c]->maskROI != 0 || samples[c]->tileInfo != 0 )
97 CV_ERROR( CV_StsBadArg, "Unsupported image format" );
99 if( samples[c]->imageData == 0 )
100 CV_ERROR( CV_BadDataPtr, "Null image data" );
102 if( samples[c]->roi &&
103 ((samples[c]->roi->xOffset | samples[c]->roi->yOffset
104 | samples[c]->roi->width | samples[c]->roi->height) < 0 ||
105 samples[c]->roi->xOffset + samples[c]->roi->width > samples[c]->width ||
106 samples[c]->roi->yOffset + samples[c]->roi->height > samples[c]->height ||
107 (unsigned) (samples[c]->roi->coi) > (unsigned) (samples[c]->nChannels)))
108 CV_ERROR( CV_BadROISize, "Invalid ROI" );
110 // End of CV_CHECK_IMAGE inline expansion
112 if (samples[c]->depth != IPL_DEPTH_8U)
113 CV_ERROR(CV_BadDepth, "Channel depth of source image must be 8");
115 if (samples[c]->nChannels != 3 && samples[c]->nChannels != 1)
116 CV_ERROR(CV_BadNumChannels, "Source image must have 1 or 3 channels");
119 CV_CALL(object_points = (CvPoint3D32f *)cvAlloc(num_points * sizeof(CvPoint3D32f)));
120 CV_CALL(points = (CvPoint2D32f *)cvAlloc(num_points * sizeof(CvPoint2D32f)));
122 // fill in the real-world coordinates of the checkerboard points
123 FillObjectPoints(object_points, etalon_size, square_size);
125 for (c = 0; c < num_cameras; c++)
127 CvSize image_size = cvSize(samples[c]->width, samples[c]->height);
130 // The input samples are not required to all have the same size or color
131 // format. If they have different sizes, the temporary images are
132 // reallocated as necessary.
133 if (samples[c]->nChannels == 3)
136 if (gray_img == NULL || gray_img->width != samples[c]->width ||
137 gray_img->height != samples[c]->height )
139 if (gray_img != NULL)
140 cvReleaseImage(&gray_img);
141 CV_CALL(gray_img = cvCreateImage(image_size, IPL_DEPTH_8U, 1));
144 CV_CALL(cvCvtColor(samples[c], gray_img, CV_BGR2GRAY));
150 // no color conversion required
154 if (tmp_img == NULL || tmp_img->width != samples[c]->width ||
155 tmp_img->height != samples[c]->height )
158 cvReleaseImage(&tmp_img);
159 CV_CALL(tmp_img = cvCreateImage(image_size, IPL_DEPTH_8U, 1));
162 int count = num_points;
163 bool found = cvFindChessBoardCornerGuesses(img, tmp_img, 0,
164 etalon_size, points, &count) != 0;
168 // If found is true, it means all the points were found (count = num_points).
169 // If found is false but count is non-zero, it means that not all points were found.
171 cvFindCornerSubPix(img, points, count, cvSize(5,5), cvSize(-1,-1),
172 cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10, 0.01f));
174 // If the image origin is BL (bottom-left), fix the y coordinates
175 // so they are relative to the true top of the image.
176 if (samples[c]->origin == IPL_ORIGIN_BL)
178 for (i = 0; i < count; i++)
179 points[i].y = samples[c]->height - 1 - points[i].y;
184 // Make sure x coordinates are increasing and y coordinates are decreasing.
185 // (The y coordinate of point (0,0) should be the greatest, because the point
186 // on the checkerboard that is the origin is nearest the bottom of the image.)
187 // This is done after adjusting the y coordinates according to the image origin.
188 if (points[0].x > points[1].x)
190 // reverse points in each row
191 for (j = 0; j < etalon_size.height; j++)
193 CvPoint2D32f *row = &points[j*etalon_size.width];
194 for (i = 0; i < etalon_size.width/2; i++)
195 std::swap(row[i], row[etalon_size.width-i-1]);
199 if (points[0].y < points[etalon_size.width].y)
201 // reverse points in each column
202 for (i = 0; i < etalon_size.width; i++)
204 for (j = 0; j < etalon_size.height/2; j++)
205 std::swap(points[i+j*etalon_size.width],
206 points[i+(etalon_size.height-j-1)*etalon_size.width]);
211 DrawEtalon(samples[c], points, count, etalon_size, found);
220 cvFindExtrinsicCameraParams(count,
224 const_cast<float *>(camera_intrinsics[c].focal_length),
225 camera_intrinsics[c].principal_point,
226 const_cast<float *>(camera_intrinsics[c].distortion),
230 // Check result against an arbitrary limit to eliminate impossible values.
231 // (If the chess board were truly that far away, the camera wouldn't be able to
233 if (transVect[0] > 1000*square_size
234 || transVect[1] > 1000*square_size
235 || transVect[2] > 1000*square_size)
237 // ignore impossible results
241 CvMat rotMatrDescr = cvMat(3, 3, CV_32FC1, rotMatr);
242 CvMat rotVectDescr = cvMat(3, 1, CV_32FC1, rotVect);
244 /* Calc rotation matrix by Rodrigues Transform */
245 cvRodrigues2( &rotVectDescr, &rotMatrDescr );
247 //combine the two transformations into one matrix
248 //order is important! rotations are not commutative
249 float tmat[4][4] = { { 1.f, 0.f, 0.f, 0.f },
250 { 0.f, 1.f, 0.f, 0.f },
251 { 0.f, 0.f, 1.f, 0.f },
252 { transVect[0], transVect[1], transVect[2], 1.f } };
254 float rmat[4][4] = { { rotMatr[0], rotMatr[1], rotMatr[2], 0.f },
255 { rotMatr[3], rotMatr[4], rotMatr[5], 0.f },
256 { rotMatr[6], rotMatr[7], rotMatr[8], 0.f },
257 { 0.f, 0.f, 0.f, 1.f } };
260 MultMatrix(camera_info[c].mat, tmat, rmat);
262 // change the transformation of the cameras to put them in the world coordinate
263 // system we want to work with.
265 // Start with an identity matrix; then fill in the values to accomplish
266 // the desired transformation.
267 float smat[4][4] = { { 1.f, 0.f, 0.f, 0.f },
268 { 0.f, 1.f, 0.f, 0.f },
269 { 0.f, 0.f, 1.f, 0.f },
270 { 0.f, 0.f, 0.f, 1.f } };
272 // First, reflect through the origin by inverting all three axes.
276 MultMatrix(tmat, camera_info[c].mat, smat);
278 // Scale x and y coordinates by the focal length (allowing for non-square pixels
279 // and/or non-symmetrical lenses).
280 smat[0][0] = 1.0f / camera_intrinsics[c].focal_length[0];
281 smat[1][1] = 1.0f / camera_intrinsics[c].focal_length[1];
283 MultMatrix(camera_info[c].mat, smat, tmat);
285 camera_info[c].principal_point = camera_intrinsics[c].principal_point;
286 camera_info[c].valid = true;
292 cvReleaseImage(&gray_img);
293 cvReleaseImage(&tmp_img);
294 cvFree(&object_points);
297 return cameras_done == num_cameras;
300 // fill in the real-world coordinates of the checkerboard points
301 static void FillObjectPoints(CvPoint3D32f *obj_points, CvSize etalon_size, float square_size)
305 for (y = 0, i = 0; y < etalon_size.height; y++)
307 for (x = 0; x < etalon_size.width; x++, i++)
309 obj_points[i].x = square_size * x;
310 obj_points[i].y = square_size * y;
317 // Mark the points found on the input image
318 // The marks are drawn multi-colored if all the points were found.
319 static void DrawEtalon(IplImage *img, CvPoint2D32f *corners,
320 int corner_count, CvSize etalon_size, int draw_ordered)
326 static const CvScalar rgb_colors[] =
336 static const CvScalar gray_colors[] = {
337 CvScalar(80), CvScalar(120), CvScalar(160), CvScalar(200), CvScalar(100), CvScalar(140), CvScalar(180)
339 const CvScalar* colors = img->nChannels == 3 ? rgb_colors : gray_colors;
341 CvScalar color = colors[0];
342 for (y = 0, i = 0; y < etalon_size.height; y++)
345 color = colors[y % ARRAY_SIZEOF(rgb_colors)];
347 for (x = 0; x < etalon_size.width && i < corner_count; x++, i++)
350 pt.x = cvRound(corners[i].x);
351 pt.y = cvRound(corners[i].y);
352 if (img->origin == IPL_ORIGIN_BL)
353 pt.y = img->height - 1 - pt.y;
358 cvLine(img, prev_pt, pt, color, 1, CV_AA);
362 cvLine( img, cvPoint(pt.x - r, pt.y - r),
363 cvPoint(pt.x + r, pt.y + r), color, 1, CV_AA );
364 cvLine( img, cvPoint(pt.x - r, pt.y + r),
365 cvPoint(pt.x + r, pt.y - r), color, 1, CV_AA );
366 cvCircle( img, pt, r+1, color, 1, CV_AA );
371 // Find the midpoint of the line segment between two points.
372 static CvPoint3D32f midpoint(const CvPoint3D32f &p1, const CvPoint3D32f &p2)
374 return cvPoint3D32f((p1.x+p2.x)/2, (p1.y+p2.y)/2, (p1.z+p2.z)/2);
377 static void operator +=(CvPoint3D32f &p1, const CvPoint3D32f &p2)
384 static CvPoint3D32f operator /(const CvPoint3D32f &p, int d)
386 return cvPoint3D32f(p.x/d, p.y/d, p.z/d);
389 static const Cv3dTracker2dTrackedObject *find(const Cv3dTracker2dTrackedObject v[], int num_objects, int id)
391 for (int i = 0; i < num_objects; i++)
399 #define CAMERA_POS(c) (cvPoint3D32f((c).mat[3][0], (c).mat[3][1], (c).mat[3][2]))
401 //////////////////////////////
402 // cv3dTrackerLocateObjects //
403 //////////////////////////////
404 CV_IMPL int cv3dTrackerLocateObjects(int num_cameras, int num_objects,
405 const Cv3dTrackerCameraInfo camera_info[], // size is num_cameras
406 const Cv3dTracker2dTrackedObject tracking_info[], // size is num_objects*num_cameras
407 Cv3dTrackerTrackedObject tracked_objects[]) // size is num_objects
409 /*CV_FUNCNAME("cv3dTrackerLocateObjects");*/
410 int found_objects = 0;
412 // count how many cameras could see each object
413 std::map<int, int> count;
414 for (int c = 0; c < num_cameras; c++)
416 if (!camera_info[c].valid)
419 for (int i = 0; i < num_objects; i++)
421 const Cv3dTracker2dTrackedObject *o = &tracking_info[c*num_objects+i];
427 // process each object that was seen by at least two cameras
428 for (std::map<int, int>::iterator i = count.begin(); i != count.end(); i++)
431 continue; // ignore object seen by only one camera
434 // find an approximation of the objects location for each pair of cameras that
435 // could see this object, and average them
436 CvPoint3D32f total = cvPoint3D32f(0, 0, 0);
439 for (int c1 = 0; c1 < num_cameras-1; c1++)
441 if (!camera_info[c1].valid)
444 const Cv3dTracker2dTrackedObject *o1 = find(&tracking_info[c1*num_objects],
447 continue; // this camera didn't see this object
449 CvPoint3D32f p1a = CAMERA_POS(camera_info[c1]);
450 CvPoint3D32f p1b = ImageCStoWorldCS(camera_info[c1], o1->p);
452 for (int c2 = c1 + 1; c2 < num_cameras; c2++)
454 if (!camera_info[c2].valid)
457 const Cv3dTracker2dTrackedObject *o2 = find(&tracking_info[c2*num_objects],
460 continue; // this camera didn't see this object
462 CvPoint3D32f p2a = CAMERA_POS(camera_info[c2]);
463 CvPoint3D32f p2b = ImageCStoWorldCS(camera_info[c2], o2->p);
465 // these variables are initialized simply to avoid erroneous error messages
467 CvPoint3D32f r1 = cvPoint3D32f(0, 0, 0);
468 CvPoint3D32f r2 = cvPoint3D32f(0, 0, 0);
470 // find the intersection of the two lines (or the points of closest
471 // approach, if they don't intersect)
472 if (!intersection(p1a, p1b, p2a, p2b, r1, r2))
475 total += midpoint(r1, r2);
480 CvPoint3D32f center = total/weight;
481 tracked_objects[found_objects++] = cv3dTrackerTrackedObject(id, center);
484 return found_objects;
489 // Compute the determinant of the 3x3 matrix represented by 3 row vectors.
490 static inline double det(CvPoint3D32f v1, CvPoint3D32f v2, CvPoint3D32f v3)
492 return v1.x*v2.y*v3.z + v1.z*v2.x*v3.y + v1.y*v2.z*v3.x
493 - v1.z*v2.y*v3.x - v1.x*v2.z*v3.y - v1.y*v2.x*v3.z;
496 static CvPoint3D32f operator +(CvPoint3D32f a, CvPoint3D32f b)
498 return cvPoint3D32f(a.x + b.x, a.y + b.y, a.z + b.z);
501 static CvPoint3D32f operator -(CvPoint3D32f a, CvPoint3D32f b)
503 return cvPoint3D32f(a.x - b.x, a.y - b.y, a.z - b.z);
506 static CvPoint3D32f operator *(CvPoint3D32f v, double f)
508 return cvPoint3D32f(f*v.x, f*v.y, f*v.z);
512 // Find the intersection of two lines, or if they don't intersect,
513 // the points of closest approach.
514 // The lines are defined by (o1,p1) and (o2, p2).
515 // If they intersect, r1 and r2 will be the same.
516 // Returns false on error.
517 static bool intersection(CvPoint3D32f o1, CvPoint3D32f p1,
518 CvPoint3D32f o2, CvPoint3D32f p2,
519 CvPoint3D32f &r1, CvPoint3D32f &r2)
521 CvPoint3D32f x = o2 - o1;
522 CvPoint3D32f d1 = p1 - o1;
523 CvPoint3D32f d2 = p2 - o2;
525 CvPoint3D32f cross = cvPoint3D32f(d1.y*d2.z - d1.z*d2.y,
526 d1.z*d2.x - d1.x*d2.z,
527 d1.x*d2.y - d1.y*d2.x);
528 double den = cross.x*cross.x + cross.y*cross.y + cross.z*cross.z;
533 double t1 = det(x, d2, cross) / den;
534 double t2 = det(x, d1, cross) / den;
542 // Convert from image to camera space by transforming point p in
543 // the image plane by the camera matrix.
544 static CvPoint3D32f ImageCStoWorldCS(const Cv3dTrackerCameraInfo &camera_info, CvPoint2D32f p)
547 tp[0] = (float)p.x - camera_info.principal_point.x;
548 tp[1] = (float)p.y - camera_info.principal_point.y;
553 //multiply tp by mat to get tr
554 MultVectorMatrix(tr, tp, camera_info.mat);
556 return cvPoint3D32f(tr[0]/tr[3], tr[1]/tr[3], tr[2]/tr[3]);
559 // Multiply affine transformation m1 by the affine transformation m2 and
560 // return the result in rm.
561 static void MultMatrix(float rm[4][4], const float m1[4][4], const float m2[4][4])
563 for (int i=0; i<=3; i++)
564 for (int j=0; j<=3; j++)
567 for (int k=0; k <= 3; k++)
568 rm[i][j] += m1[i][k]*m2[k][j];
572 // Multiply the vector v by the affine transformation matrix m and return the
574 void MultVectorMatrix(float rv[4], const float v[4], const float m[4][4])
576 for (int i=0; i<=3; i++)
579 for (int j=0;j<=3;j++)
580 rv[i] += v[j] * m[j][i];