16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2002, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistributions of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistributions in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cvaux.h" 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if _MSC_VER >= 1200 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#pragma warning(disable:4786) // Disable MSVC warnings in the standard library. 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#pragma warning(disable:4100) 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#pragma warning(disable:4512) 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <stdio.h> 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <map> 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <algorithm> 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if _MSC_VER >= 1200 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#pragma warning(default:4100) 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#pragma warning(default:4512) 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ARRAY_SIZEOF(a) (sizeof(a)/sizeof((a)[0])) 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void FillObjectPoints(CvPoint3D32f *obj_points, CvSize etalon_size, float square_size); 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void DrawEtalon(IplImage *img, CvPoint2D32f *corners, 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int corner_count, CvSize etalon_size, int draw_ordered); 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void MultMatrix(float rm[4][4], const float m1[4][4], const float m2[4][4]); 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void MultVectorMatrix(float rv[4], const float v[4], const float m[4][4]); 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvPoint3D32f ImageCStoWorldCS(const Cv3dTrackerCameraInfo &camera_info, CvPoint2D32f p); 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic bool intersection(CvPoint3D32f o1, CvPoint3D32f p1, 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f o2, CvPoint3D32f p2, 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f &r1, CvPoint3D32f &r2); 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn///////////////////////////////// 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// cv3dTrackerCalibrateCameras // 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn///////////////////////////////// 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL CvBool cv3dTrackerCalibrateCameras(int num_cameras, 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const Cv3dTrackerCameraIntrinsics camera_intrinsics[], // size is num_cameras 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize etalon_size, 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float square_size, 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *samples[], // size is num_cameras 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Cv3dTrackerCameraInfo camera_info[]) // size is num_cameras 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME("cv3dTrackerCalibrateCameras"); 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int num_points = etalon_size.width * etalon_size.height; 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int cameras_done = 0; // the number of cameras whose positions have been determined 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f *object_points = NULL; // real-world coordinates of checkerboard points 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f *points = NULL; // 2d coordinates of checkerboard points as seen by a camera 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *gray_img = NULL; // temporary image for color conversion 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *tmp_img = NULL; // temporary image used by FindChessboardCornerGuesses 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int c, i, j; 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (etalon_size.width < 3 || etalon_size.height < 3) 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsBadArg, "Chess board size is invalid"); 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (c = 0; c < num_cameras; c++) 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // CV_CHECK_IMAGE is not available in the cvaux library 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // so perform the checks inline. 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //CV_CALL(CV_CHECK_IMAGE(samples[c])); 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( samples[c] == NULL ) 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_HeaderIsNull, "Null image" ); 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( samples[c]->dataOrder != IPL_DATA_ORDER_PIXEL && samples[c]->nChannels > 1 ) 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_BadOrder, "Unsupported image format" ); 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( samples[c]->maskROI != 0 || samples[c]->tileInfo != 0 ) 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Unsupported image format" ); 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( samples[c]->imageData == 0 ) 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_BadDataPtr, "Null image data" ); 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( samples[c]->roi && 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((samples[c]->roi->xOffset | samples[c]->roi->yOffset 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn | samples[c]->roi->width | samples[c]->roi->height) < 0 || 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn samples[c]->roi->xOffset + samples[c]->roi->width > samples[c]->width || 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn samples[c]->roi->yOffset + samples[c]->roi->height > samples[c]->height || 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (unsigned) (samples[c]->roi->coi) > (unsigned) (samples[c]->nChannels))) 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_BadROISize, "Invalid ROI" ); 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // End of CV_CHECK_IMAGE inline expansion 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (samples[c]->depth != IPL_DEPTH_8U) 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_BadDepth, "Channel depth of source image must be 8"); 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (samples[c]->nChannels != 3 && samples[c]->nChannels != 1) 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_BadNumChannels, "Source image must have 1 or 3 channels"); 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(object_points = (CvPoint3D32f *)cvAlloc(num_points * sizeof(CvPoint3D32f))); 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(points = (CvPoint2D32f *)cvAlloc(num_points * sizeof(CvPoint2D32f))); 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // fill in the real-world coordinates of the checkerboard points 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FillObjectPoints(object_points, etalon_size, square_size); 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (c = 0; c < num_cameras; c++) 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize image_size = cvSize(samples[c]->width, samples[c]->height); 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *img; 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // The input samples are not required to all have the same size or color 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // format. If they have different sizes, the temporary images are 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // reallocated as necessary. 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (samples[c]->nChannels == 3) 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // convert to gray 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (gray_img == NULL || gray_img->width != samples[c]->width || 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn gray_img->height != samples[c]->height ) 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (gray_img != NULL) 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseImage(&gray_img); 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(gray_img = cvCreateImage(image_size, IPL_DEPTH_8U, 1)); 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(cvCvtColor(samples[c], gray_img, CV_BGR2GRAY)); 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn img = gray_img; 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // no color conversion required 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn img = samples[c]; 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (tmp_img == NULL || tmp_img->width != samples[c]->width || 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp_img->height != samples[c]->height ) 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (tmp_img != NULL) 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseImage(&tmp_img); 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(tmp_img = cvCreateImage(image_size, IPL_DEPTH_8U, 1)); 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int count = num_points; 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool found = cvFindChessBoardCornerGuesses(img, tmp_img, 0, 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn etalon_size, points, &count) != 0; 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (count == 0) 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // If found is true, it means all the points were found (count = num_points). 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // If found is false but count is non-zero, it means that not all points were found. 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFindCornerSubPix(img, points, count, cvSize(5,5), cvSize(-1,-1), 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10, 0.01f)); 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // If the image origin is BL (bottom-left), fix the y coordinates 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // so they are relative to the true top of the image. 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (samples[c]->origin == IPL_ORIGIN_BL) 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (i = 0; i < count; i++) 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points[i].y = samples[c]->height - 1 - points[i].y; 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (found) 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // Make sure x coordinates are increasing and y coordinates are decreasing. 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // (The y coordinate of point (0,0) should be the greatest, because the point 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // on the checkerboard that is the origin is nearest the bottom of the image.) 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // This is done after adjusting the y coordinates according to the image origin. 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (points[0].x > points[1].x) 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // reverse points in each row 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (j = 0; j < etalon_size.height; j++) 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f *row = &points[j*etalon_size.width]; 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (i = 0; i < etalon_size.width/2; i++) 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn std::swap(row[i], row[etalon_size.width-i-1]); 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (points[0].y < points[etalon_size.width].y) 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // reverse points in each column 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (i = 0; i < etalon_size.width; i++) 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (j = 0; j < etalon_size.height/2; j++) 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn std::swap(points[i+j*etalon_size.width], 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points[i+(etalon_size.height-j-1)*etalon_size.width]); 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn DrawEtalon(samples[c], points, count, etalon_size, found); 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (!found) 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float rotVect[3]; 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float rotMatr[9]; 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float transVect[3]; 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFindExtrinsicCameraParams(count, 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn image_size, 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points, 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn object_points, 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const_cast<float *>(camera_intrinsics[c].focal_length), 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camera_intrinsics[c].principal_point, 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const_cast<float *>(camera_intrinsics[c].distortion), 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotVect, 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect); 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // Check result against an arbitrary limit to eliminate impossible values. 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // (If the chess board were truly that far away, the camera wouldn't be able to 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // see the squares.) 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (transVect[0] > 1000*square_size 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn || transVect[1] > 1000*square_size 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn || transVect[2] > 1000*square_size) 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // ignore impossible results 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat rotMatrDescr = cvMat(3, 3, CV_32FC1, rotMatr); 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat rotVectDescr = cvMat(3, 1, CV_32FC1, rotVect); 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calc rotation matrix by Rodrigues Transform */ 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2( &rotVectDescr, &rotMatrDescr ); 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //combine the two transformations into one matrix 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //order is important! rotations are not commutative 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float tmat[4][4] = { { 1.f, 0.f, 0.f, 0.f }, 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 0.f, 1.f, 0.f, 0.f }, 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 0.f, 0.f, 1.f, 0.f }, 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { transVect[0], transVect[1], transVect[2], 1.f } }; 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float rmat[4][4] = { { rotMatr[0], rotMatr[1], rotMatr[2], 0.f }, 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { rotMatr[3], rotMatr[4], rotMatr[5], 0.f }, 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { rotMatr[6], rotMatr[7], rotMatr[8], 0.f }, 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 0.f, 0.f, 0.f, 1.f } }; 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MultMatrix(camera_info[c].mat, tmat, rmat); 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // change the transformation of the cameras to put them in the world coordinate 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // system we want to work with. 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // Start with an identity matrix; then fill in the values to accomplish 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // the desired transformation. 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float smat[4][4] = { { 1.f, 0.f, 0.f, 0.f }, 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 0.f, 1.f, 0.f, 0.f }, 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 0.f, 0.f, 1.f, 0.f }, 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 0.f, 0.f, 0.f, 1.f } }; 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // First, reflect through the origin by inverting all three axes. 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn smat[0][0] = -1.f; 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn smat[1][1] = -1.f; 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn smat[2][2] = -1.f; 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MultMatrix(tmat, camera_info[c].mat, smat); 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // Scale x and y coordinates by the focal length (allowing for non-square pixels 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // and/or non-symmetrical lenses). 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn smat[0][0] = 1.0f / camera_intrinsics[c].focal_length[0]; 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn smat[1][1] = 1.0f / camera_intrinsics[c].focal_length[1]; 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn smat[2][2] = 1.0f; 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MultMatrix(camera_info[c].mat, smat, tmat); 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camera_info[c].principal_point = camera_intrinsics[c].principal_point; 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camera_info[c].valid = true; 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cameras_done++; 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennexit: 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseImage(&gray_img); 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseImage(&tmp_img); 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&object_points); 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&points); 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return cameras_done == num_cameras; 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// fill in the real-world coordinates of the checkerboard points 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void FillObjectPoints(CvPoint3D32f *obj_points, CvSize etalon_size, float square_size) 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x, y, i; 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (y = 0, i = 0; y < etalon_size.height; y++) 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (x = 0; x < etalon_size.width; x++, i++) 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn obj_points[i].x = square_size * x; 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn obj_points[i].y = square_size * y; 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn obj_points[i].z = 0; 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Mark the points found on the input image 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// The marks are drawn multi-colored if all the points were found. 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void DrawEtalon(IplImage *img, CvPoint2D32f *corners, 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int corner_count, CvSize etalon_size, int draw_ordered) 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int r = 4; 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x, y; 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint prev_pt = { 0, 0 }; 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn static const CvScalar rgb_colors[] = { 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {{0,0,255}}, 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {{0,128,255}}, 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {{0,200,200}}, 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {{0,255,0}}, 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {{200,200,0}}, 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {{255,0,0}}, 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {{255,0,255}} }; 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn static const CvScalar gray_colors[] = { 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {{80}}, {{120}}, {{160}}, {{200}}, {{100}}, {{140}}, {{180}} 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvScalar* colors = img->nChannels == 3 ? rgb_colors : gray_colors; 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvScalar color = colors[0]; 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (y = 0, i = 0; y < etalon_size.height; y++) 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (draw_ordered) 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn color = colors[y % ARRAY_SIZEOF(rgb_colors)]; 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (x = 0; x < etalon_size.width && i < corner_count; x++, i++) 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint pt; 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt.x = cvRound(corners[i].x); 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt.y = cvRound(corners[i].y); 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (img->origin == IPL_ORIGIN_BL) 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt.y = img->height - 1 - pt.y; 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (draw_ordered) 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (i != 0) 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvLine(img, prev_pt, pt, color, 1, CV_AA); 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prev_pt = pt; 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvLine( img, cvPoint(pt.x - r, pt.y - r), 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvPoint(pt.x + r, pt.y + r), color, 1, CV_AA ); 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvLine( img, cvPoint(pt.x - r, pt.y + r), 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvPoint(pt.x + r, pt.y - r), color, 1, CV_AA ); 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCircle( img, pt, r+1, color, 1, CV_AA ); 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Find the midpoint of the line segment between two points. 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvPoint3D32f midpoint(const CvPoint3D32f &p1, const CvPoint3D32f &p2) 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return cvPoint3D32f((p1.x+p2.x)/2, (p1.y+p2.y)/2, (p1.z+p2.z)/2); 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void operator +=(CvPoint3D32f &p1, const CvPoint3D32f &p2) 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p1.x += p2.x; 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p1.y += p2.y; 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p1.z += p2.z; 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvPoint3D32f operator /(const CvPoint3D32f &p, int d) 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return cvPoint3D32f(p.x/d, p.y/d, p.z/d); 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic const Cv3dTracker2dTrackedObject *find(const Cv3dTracker2dTrackedObject v[], int num_objects, int id) 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (int i = 0; i < num_objects; i++) 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (v[i].id == id) 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return &v[i]; 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return NULL; 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define CAMERA_POS(c) (cvPoint3D32f((c).mat[3][0], (c).mat[3][1], (c).mat[3][2])) 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn////////////////////////////// 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// cv3dTrackerLocateObjects // 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn////////////////////////////// 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL int cv3dTrackerLocateObjects(int num_cameras, int num_objects, 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const Cv3dTrackerCameraInfo camera_info[], // size is num_cameras 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const Cv3dTracker2dTrackedObject tracking_info[], // size is num_objects*num_cameras 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Cv3dTrackerTrackedObject tracked_objects[]) // size is num_objects 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*CV_FUNCNAME("cv3dTrackerLocateObjects");*/ 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int found_objects = 0; 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // count how many cameras could see each object 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn std::map<int, int> count; 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (int c = 0; c < num_cameras; c++) 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (!camera_info[c].valid) 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (int i = 0; i < num_objects; i++) 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const Cv3dTracker2dTrackedObject *o = &tracking_info[c*num_objects+i]; 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (o->id != -1) 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count[o->id]++; 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // process each object that was seen by at least two cameras 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (std::map<int, int>::iterator i = count.begin(); i != count.end(); i++) 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (i->second < 2) 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; // ignore object seen by only one camera 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int id = i->first; 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // find an approximation of the objects location for each pair of cameras that 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // could see this object, and average them 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f total = cvPoint3D32f(0, 0, 0); 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int weight = 0; 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (int c1 = 0; c1 < num_cameras-1; c1++) 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (!camera_info[c1].valid) 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const Cv3dTracker2dTrackedObject *o1 = find(&tracking_info[c1*num_objects], 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn num_objects, id); 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (o1 == NULL) 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; // this camera didn't see this object 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f p1a = CAMERA_POS(camera_info[c1]); 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f p1b = ImageCStoWorldCS(camera_info[c1], o1->p); 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (int c2 = c1 + 1; c2 < num_cameras; c2++) 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (!camera_info[c2].valid) 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const Cv3dTracker2dTrackedObject *o2 = find(&tracking_info[c2*num_objects], 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn num_objects, id); 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (o2 == NULL) 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; // this camera didn't see this object 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f p2a = CAMERA_POS(camera_info[c2]); 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f p2b = ImageCStoWorldCS(camera_info[c2], o2->p); 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // these variables are initialized simply to avoid erroneous error messages 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // from the compiler 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f r1 = cvPoint3D32f(0, 0, 0); 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f r2 = cvPoint3D32f(0, 0, 0); 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // find the intersection of the two lines (or the points of closest 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // approach, if they don't intersect) 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (!intersection(p1a, p1b, p2a, p2b, r1, r2)) 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn total += midpoint(r1, r2); 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn weight++; 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f center = total/weight; 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tracked_objects[found_objects++] = cv3dTrackerTrackedObject(id, center); 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return found_objects; 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define EPS 1e-9 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Compute the determinant of the 3x3 matrix represented by 3 row vectors. 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic inline double det(CvPoint3D32f v1, CvPoint3D32f v2, CvPoint3D32f v3) 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return v1.x*v2.y*v3.z + v1.z*v2.x*v3.y + v1.y*v2.z*v3.x 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn - v1.z*v2.y*v3.x - v1.x*v2.z*v3.y - v1.y*v2.x*v3.z; 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvPoint3D32f operator +(CvPoint3D32f a, CvPoint3D32f b) 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return cvPoint3D32f(a.x + b.x, a.y + b.y, a.z + b.z); 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvPoint3D32f operator -(CvPoint3D32f a, CvPoint3D32f b) 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return cvPoint3D32f(a.x - b.x, a.y - b.y, a.z - b.z); 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvPoint3D32f operator *(CvPoint3D32f v, double f) 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return cvPoint3D32f(f*v.x, f*v.y, f*v.z); 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Find the intersection of two lines, or if they don't intersect, 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the points of closest approach. 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// The lines are defined by (o1,p1) and (o2, p2). 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If they intersect, r1 and r2 will be the same. 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Returns false on error. 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic bool intersection(CvPoint3D32f o1, CvPoint3D32f p1, 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f o2, CvPoint3D32f p2, 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f &r1, CvPoint3D32f &r2) 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f x = o2 - o1; 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f d1 = p1 - o1; 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f d2 = p2 - o2; 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f cross = cvPoint3D32f(d1.y*d2.z - d1.z*d2.y, 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1.z*d2.x - d1.x*d2.z, 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1.x*d2.y - d1.y*d2.x); 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double den = cross.x*cross.x + cross.y*cross.y + cross.z*cross.z; 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (den < EPS) 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return false; 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t1 = det(x, d2, cross) / den; 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t2 = det(x, d1, cross) / den; 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r1 = o1 + d1 * t1; 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r2 = o2 + d2 * t2; 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return true; 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Convert from image to camera space by transforming point p in 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the image plane by the camera matrix. 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvPoint3D32f ImageCStoWorldCS(const Cv3dTrackerCameraInfo &camera_info, CvPoint2D32f p) 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float tp[4]; 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tp[0] = (float)p.x - camera_info.principal_point.x; 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tp[1] = (float)p.y - camera_info.principal_point.y; 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tp[2] = 1.f; 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tp[3] = 1.f; 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float tr[4]; 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //multiply tp by mat to get tr 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MultVectorMatrix(tr, tp, camera_info.mat); 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return cvPoint3D32f(tr[0]/tr[3], tr[1]/tr[3], tr[2]/tr[3]); 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Multiply affine transformation m1 by the affine transformation m2 and 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// return the result in rm. 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void MultMatrix(float rm[4][4], const float m1[4][4], const float m2[4][4]) 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (int i=0; i<=3; i++) 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (int j=0; j<=3; j++) 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rm[i][j]= 0.0; 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (int k=0; k <= 3; k++) 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rm[i][j] += m1[i][k]*m2[k][j]; 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Multiply the vector v by the affine transformation matrix m and return the 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// result in rv. 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid MultVectorMatrix(float rv[4], const float v[4], const float m[4][4]) 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (int i=0; i<=3; i++) 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rv[i] = 0.f; 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (int j=0;j<=3;j++) 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rv[i] += v[j] * m[j][i]; 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 589