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) 2000, 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// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's 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#include "_cv.h" 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic const double eps = 1e-6; 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvStatus 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvFitLine2D_wods( CvPoint2D32f * points, int _count, float *weights, float *line ) 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = 0, y = 0, x2 = 0, y2 = 0, xy = 0, w = 0; 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx2, dy2, dxy; 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int count = _count; 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float t; 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calculating the average of x and y... */ 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( weights == 0 ) 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i += 1 ) 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x += points[i].x; 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y += points[i].y; 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x2 += points[i].x * points[i].x; 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y2 += points[i].y * points[i].y; 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xy += points[i].x * points[i].y; 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w = (float) count; 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i += 1 ) 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x += weights[i] * points[i].x; 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y += weights[i] * points[i].y; 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x2 += weights[i] * points[i].x * points[i].x; 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y2 += weights[i] * points[i].y * points[i].y; 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xy += weights[i] * points[i].x * points[i].y; 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w += weights[i]; 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x /= w; 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y /= w; 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x2 /= w; 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y2 /= w; 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xy /= w; 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dx2 = x2 - x * x; 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dy2 = y2 - y * y; 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dxy = xy - x * y; 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = (float) atan2( 2 * dxy, dx2 - dy2 ) / 2; 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn line[0] = (float) cos( t ); 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn line[1] = (float) sin( t ); 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn line[2] = (float) x; 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn line[3] = (float) y; 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NO_ERR; 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvStatus 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvFitLine3D_wods( CvPoint3D32f * points, int count, float *weights, float *line ) 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float w0 = 0; 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x0 = 0, y0 = 0, z0 = 0; 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x2 = 0, y2 = 0, z2 = 0, xy = 0, yz = 0, xz = 0; 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float dx2, dy2, dz2, dxy, dxz, dyz; 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *v; 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float n; 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float det[9], evc[9], evl[3]; 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( evl, 0, 3*sizeof(evl[0])); 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( evc, 0, 9*sizeof(evl[0])); 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( weights ) 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x = points[i].x; 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float y = points[i].y; 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float z = points[i].z; 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float w = weights[i]; 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x2 += x * x * w; 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xy += x * y * w; 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xz += x * z * w; 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y2 += y * y * w; 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yz += y * z * w; 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z2 += z * z * w; 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x0 += x * w; 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y0 += y * w; 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z0 += z * w; 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w0 += w; 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x = points[i].x; 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float y = points[i].y; 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float z = points[i].z; 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x2 += x * x; 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xy += x * y; 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xz += x * z; 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y2 += y * y; 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yz += y * z; 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z2 += z * z; 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x0 += x; 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y0 += y; 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z0 += z; 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w0 = (float) count; 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x2 /= w0; 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xy /= w0; 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xz /= w0; 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y2 /= w0; 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yz /= w0; 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z2 /= w0; 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x0 /= w0; 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y0 /= w0; 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z0 /= w0; 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dx2 = x2 - x0 * x0; 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dxy = xy - x0 * y0; 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dxz = xz - x0 * z0; 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dy2 = y2 - y0 * y0; 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dyz = yz - y0 * z0; 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dz2 = z2 - z0 * z0; 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det[0] = dz2 + dy2; 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det[1] = -dxy; 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det[2] = -dxz; 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det[3] = det[1]; 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det[4] = dx2 + dz2; 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det[5] = -dyz; 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det[6] = det[2]; 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det[7] = det[5]; 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det[8] = dy2 + dx2; 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Searching for a eigenvector of det corresponding to the minimal eigenvalue */ 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _det = cvMat( 3, 3, CV_32F, det ); 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _evc = cvMat( 3, 3, CV_32F, evc ); 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _evl = cvMat( 3, 1, CV_32F, evl ); 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvEigenVV( &_det, &_evc, &_evl, 0 ); 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i = evl[0] < evl[1] ? (evl[0] < evl[2] ? 0 : 2) : (evl[1] < evl[2] ? 1 : 2); 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#else 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _det = cvMat( 3, 3, CV_32F, det ); 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _evc = cvMat( 3, 3, CV_32F, evc ); 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _evl = cvMat( 1, 3, CV_32F, evl ); 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD( &_det, &_evl, &_evc, 0, CV_SVD_MODIFY_A+CV_SVD_U_T ); 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i = 2; 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v = &evc[i * 3]; 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n = (float) sqrt( (double)v[0] * v[0] + (double)v[1] * v[1] + (double)v[2] * v[2] ); 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n = (float)MAX(n, eps); 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn line[0] = v[0] / n; 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn line[1] = v[1] / n; 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn line[2] = v[2] / n; 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn line[3] = x0; 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn line[4] = y0; 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn line[5] = z0; 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NO_ERR; 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic double 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvCalcDist2D( CvPoint2D32f * points, int count, float *_line, float *dist ) 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int j; 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float px = _line[2], py = _line[3]; 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float nx = _line[1], ny = -_line[0]; 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sum_dist = 0.; 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < count; j++ ) 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x, y; 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = points[j].x - px; 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = points[j].y - py; 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist[j] = (float) fabs( nx * x + ny * y ); 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum_dist += dist[j]; 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return sum_dist; 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic double 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvCalcDist3D( CvPoint3D32f * points, int count, float *_line, float *dist ) 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int j; 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float px = _line[3], py = _line[4], pz = _line[5]; 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float vx = _line[0], vy = _line[1], vz = _line[2]; 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sum_dist = 0.; 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < count; j++ ) 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x, y, z; 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p1, p2, p3; 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = points[j].x - px; 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = points[j].y - py; 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z = points[j].z - pz; 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p1 = vy * z - vz * y; 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p2 = vz * x - vx * z; 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p3 = vx * y - vy * x; 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist[j] = (float) sqrt( p1*p1 + p2*p2 + p3*p3 ); 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum_dist += dist[j]; 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return sum_dist; 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvWeightL1( float *d, int count, float *w ) 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t = fabs( (double) d[i] ); 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[i] = (float)(1. / MAX(t, eps)); 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvWeightL12( float *d, int count, float *w ) 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[i] = 1.0f / (float) sqrt( 1 + (double) (d[i] * d[i] * 0.5) ); 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvWeightHuber( float *d, int count, float *w, float _c ) 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float c = _c <= 0 ? 1.345f : _c; 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d[i] < c ) 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[i] = 1.0f; 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[i] = c/d[i]; 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvWeightFair( float *d, int count, float *w, float _c ) 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float c = _c == 0 ? 1 / 1.3998f : 1 / _c; 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[i] = 1 / (1 + d[i] * c); 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvWeightWelsch( float *d, int count, float *w, float _c ) 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float c = _c == 0 ? 1 / 2.9846f : 1 / _c; 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[i] = (float) exp( -d[i] * d[i] * c * c ); 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Takes an array of 2D points, type of distance (including user-defined 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renndistance specified by callbacks, fills the array of four floats with line 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennparameters A, B, C, D, where (A, B) is the normalized direction vector, 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn(C, D) is the point that belongs to the line. */ 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvStatus icvFitLine2D( CvPoint2D32f * points, int count, int dist, 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float _param, float reps, float aeps, float *line ) 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double EPS = count*FLT_EPSILON; 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void (*calc_weights) (float *, int, float *) = 0; 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void (*calc_weights_param) (float *, int, float *, float) = 0; 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *w; /* weights */ 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *r; /* square distances */ 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, k; 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float _line[6], _lineprev[6]; 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float rdelta = reps != 0 ? reps : 1.0f; 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float adelta = aeps != 0 ? aeps : 0.01f; 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double min_err = DBL_MAX, err = 0; 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvRNG rng = cvRNG(-1); 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( line, 0, 4*sizeof(line[0]) ); 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn switch (dist) 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_DIST_L2: 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return icvFitLine2D_wods( points, count, 0, line ); 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_DIST_L1: 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights = icvWeightL1; 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_DIST_L12: 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights = icvWeightL12; 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_DIST_FAIR: 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights_param = icvWeightFair; 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_DIST_WELSCH: 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights_param = icvWeightWelsch; 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_DIST_HUBER: 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights_param = icvWeightHuber; 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*case CV_DIST_USER: 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights = (void ( * )(float *, int, float *)) _PFP.fp; 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break;*/ 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn default: 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADFACTOR_ERR; 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w = (float *) cvAlloc( count * sizeof( float )); 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r = (float *) cvAlloc( count * sizeof( float )); 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 20; k++ ) 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int first = 1; 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[i] = 0.f; 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < MIN(count,10); ) 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn j = cvRandInt(&rng) % count; 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( w[j] < FLT_EPSILON ) 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[j] = 1.f; 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i++; 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFitLine2D_wods( points, count, w, _line ); 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 30; i++ ) 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sum_w = 0; 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( first ) 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn first = 0; 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1]; 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = MAX(t,-1.); 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = MIN(t,1.); 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(acos(t)) < adelta ) 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x, y, d; 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = (float) fabs( _line[2] - _lineprev[2] ); 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = (float) fabs( _line[3] - _lineprev[3] ); 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = x > y ? x : y; 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d < rdelta ) 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* calculate distances */ 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err = icvCalcDist2D( points, count, _line, r ); 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( err < EPS ) 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* calculate weights */ 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( calc_weights ) 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights( r, count, w ); 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights_param( r, count, w, _param ); 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < count; j++ ) 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum_w += w[j]; 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(sum_w) > FLT_EPSILON ) 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum_w = 1./sum_w; 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < count; j++ ) 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[j] = (float)(w[j]*sum_w); 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < count; j++ ) 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[j] = 1.f; 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* save the line parameters */ 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( _lineprev, _line, 4 * sizeof( float )); 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Run again... */ 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFitLine2D_wods( points, count, w, _line ); 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( err < min_err ) 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn min_err = err; 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( line, _line, 4 * sizeof(line[0])); 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( err < EPS ) 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &w ); 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &r ); 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Takes an array of 3D points, type of distance (including user-defined 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renndistance specified by callbacks, fills the array of four floats with line 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennparameters A, B, C, D, E, F, where (A, B, C) is the normalized direction vector, 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn(D, E, F) is the point that belongs to the line. */ 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvStatus 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvFitLine3D( CvPoint3D32f * points, int count, int dist, 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float _param, float reps, float aeps, float *line ) 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double EPS = count*FLT_EPSILON; 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void (*calc_weights) (float *, int, float *) = 0; 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void (*calc_weights_param) (float *, int, float *, float) = 0; 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *w; /* weights */ 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *r; /* square distances */ 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, k; 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float _line[6], _lineprev[6]; 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float rdelta = reps != 0 ? reps : 1.0f; 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float adelta = aeps != 0 ? aeps : 0.01f; 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double min_err = DBL_MAX, err = 0; 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvRNG rng = cvRNG(-1); 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( line, 0, 6*sizeof(line[0]) ); 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn switch (dist) 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_DIST_L2: 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return icvFitLine3D_wods( points, count, 0, line ); 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_DIST_L1: 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights = icvWeightL1; 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_DIST_L12: 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights = icvWeightL12; 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_DIST_FAIR: 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights_param = icvWeightFair; 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_DIST_WELSCH: 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights_param = icvWeightWelsch; 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_DIST_HUBER: 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights_param = icvWeightHuber; 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*case CV_DIST_USER: 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _PFP.p = param; 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights = (void ( * )(float *, int, float *)) _PFP.fp; 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break;*/ 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn default: 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADFACTOR_ERR; 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w = (float *) cvAlloc( count * sizeof( float )); 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r = (float *) cvAlloc( count * sizeof( float )); 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 20; k++ ) 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int first = 1; 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[i] = 0.f; 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < MIN(count,10); ) 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn j = cvRandInt(&rng) % count; 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( w[j] < FLT_EPSILON ) 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[j] = 1.f; 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i++; 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFitLine3D_wods( points, count, w, _line ); 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 30; i++ ) 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sum_w = 0; 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( first ) 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn first = 0; 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1] + _line[2] * _lineprev[2]; 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = MAX(t,-1.); 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = MIN(t,1.); 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(acos(t)) < adelta ) 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x, y, z, ax, ay, az, dx, dy, dz, d; 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = _line[3] - _lineprev[3]; 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = _line[4] - _lineprev[4]; 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z = _line[5] - _lineprev[5]; 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ax = _line[0] - _lineprev[0]; 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ay = _line[1] - _lineprev[1]; 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn az = _line[2] - _lineprev[2]; 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dx = (float) fabs( y * az - z * ay ); 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dy = (float) fabs( z * ax - x * az ); 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dz = (float) fabs( x * ay - y * ax ); 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = dx > dy ? (dx > dz ? dx : dz) : (dy > dz ? dy : dz); 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d < rdelta ) 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* calculate distances */ 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( icvCalcDist3D( points, count, _line, r ) < FLT_EPSILON*count ) 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* calculate weights */ 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( calc_weights ) 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights( r, count, w ); 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_weights_param( r, count, w, _param ); 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < count; j++ ) 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum_w += w[j]; 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(sum_w) > FLT_EPSILON ) 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum_w = 1./sum_w; 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < count; j++ ) 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[j] = (float)(w[j]*sum_w); 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < count; j++ ) 6136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w[j] = 1.f; 6146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* save the line parameters */ 6176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( _lineprev, _line, 6 * sizeof( float )); 6186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Run again... */ 6206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFitLine3D_wods( points, count, w, _line ); 6216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( err < min_err ) 6246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn min_err = err; 6266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( line, _line, 6 * sizeof(line[0])); 6276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( err < EPS ) 6286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 6296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // Return... 6336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &w ); 6346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &r ); 6356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 6366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 6406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvFitLine( const CvArr* array, int dist, double param, 6416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double reps, double aeps, float *line ) 6426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn schar* buffer = 0; 6446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvFitLine" ); 6456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 6476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn schar* points = 0; 6496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn union { CvContour contour; CvSeq seq; } header; 6506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSeqBlock block; 6516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSeq* ptseq = (CvSeq*)array; 6526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int type; 6536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !line ) 6556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "NULL pointer to line parameters" ); 6566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_IS_SEQ(ptseq) ) 6586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn type = CV_SEQ_ELTYPE(ptseq); 6606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ptseq->total == 0 ) 6616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "The sequence has no points" ); 6626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (type!=CV_32FC2 && type!=CV_32FC3 && type!=CV_32SC2 && type!=CV_32SC3) || 6636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ELEM_SIZE(type) != ptseq->elem_size ) 6646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, 6656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Input sequence must consist of 2d points or 3d points" ); 6666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* mat = (CvMat*)array; 6706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn type = CV_MAT_TYPE(mat->type); 6716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(mat)) 6726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Input array is not a sequence nor matrix" ); 6736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT_CONT(mat->type) || 6756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (type!=CV_32FC2 && type!=CV_32FC3 && type!=CV_32SC2 && type!=CV_32SC3) || 6766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (mat->width != 1 && mat->height != 1)) 6776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, 6786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Input array must be 1d continuous array of 2d or 3d points" ); 6796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( ptseq = cvMakeSeqHeaderForArray( 6816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SEQ_KIND_GENERIC|type, sizeof(CvContour), CV_ELEM_SIZE(type), mat->data.ptr, 6826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mat->width + mat->height - 1, &header.seq, &block )); 6836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( reps < 0 || aeps < 0 ) 6866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Both reps and aeps must be non-negative" ); 6876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_DEPTH(type) == CV_32F && ptseq->first->next == ptseq->first ) 6896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* no need to copy data in this case */ 6916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points = ptseq->first->data; 6926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( buffer = points = (schar*)cvAlloc( ptseq->total*CV_ELEM_SIZE(type) )); 6966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvCvtSeqToArray( ptseq, points, CV_WHOLE_SEQ )); 6976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_DEPTH(type) != CV_32F ) 6996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, total = ptseq->total*CV_MAT_CN(type); 7016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( CV_MAT_DEPTH(type) == CV_32S ); 7026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < total; i++ ) 7046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((float*)points)[i] = (float)((int*)points)[i]; 7056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dist == CV_DIST_USER ) 7096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "User-defined distance is not allowed" ); 7106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_CN(type) == 2 ) 7126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IPPI_CALL( icvFitLine2D( (CvPoint2D32f*)points, ptseq->total, 7146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist, (float)param, (float)reps, (float)aeps, line )); 7156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IPPI_CALL( icvFitLine3D( (CvPoint3D32f*)points, ptseq->total, 7196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist, (float)param, (float)reps, (float)aeps, line )); 7206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 7236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &buffer ); 7256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 7266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file. */ 728