calibfilter.cpp

Go to the documentation of this file.

/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
 
#include "calibfilter.h"
 
#include <stdio.h>
 
#include <opencv2/calib3d/calib3d_c.h>
#include <opencv2/core/core_c.h>
 
#undef quad
 
CvCalibFilter::CvCalibFilter()
{
    /* etalon data */
    etalonType = CV_CALIB_ETALON_USER;
    etalonParamCount = 0;
    etalonParams = 0;
    etalonPointCount = 0;
    etalonPoints = 0;
 
    /* camera data */
    cameraCount = 1;
 
    memset(points, 0, sizeof(points));
    memset(undistMap, 0, sizeof(undistMap));
    undistImg = 0;
    memset(latestCounts, 0, sizeof(latestCounts));
    memset(latestPoints, 0, sizeof(latestPoints));
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wclass-memaccess"
    memset(&stereo, 0, sizeof(stereo));
#pragma GCC diagnostic pop
    maxPoints = 0;
    framesTotal = 15;
    framesAccepted = 0;
    isCalibrated = false;
 
    imgSize = cvSize(0, 0);
    grayImg = 0;
    tempImg = 0;
    storage = 0;
 
    memset(rectMap, 0, sizeof(rectMap));
}
 
CvCalibFilter::~CvCalibFilter()
{
    SetCameraCount(0);
    cvFree(&etalonParams);
    cvFree(&etalonPoints);
    cvReleaseMat(&grayImg);
    cvReleaseMat(&tempImg);
    cvReleaseMat(&undistImg);
    cvReleaseMemStorage(&storage);
}
 
bool
CvCalibFilter::SetEtalon(CvCalibEtalonType type,
                         double* params,
                         int pointCount,
                         CvPoint2D32f* _points)
{
    int i, arrSize;
 
    Stop();
 
    for (i = 0; i < MAX_CAMERAS; i++)
    {
        cvFree(latestPoints + i);
    }
 
    if (type == CV_CALIB_ETALON_USER || type != etalonType)
    {
        if (etalonParams != NULL)
        {
            cvFree(&etalonParams);
        }
    }
 
    etalonType = type;
 
    switch (etalonType)
    {
        case CV_CALIB_ETALON_CHESSBOARD:
            etalonParamCount = 3;
            if (!params || cvRound(params[0]) != params[0] || params[0] < 3 ||
                cvRound(params[1]) != params[1] || params[1] < 3 || params[2] <= 0)
            {
                assert(0);
                return false;
            }
 
            pointCount = cvRound((params[0] - 1) * (params[1] - 1));
            break;
 
        case CV_CALIB_ETALON_USER:
            etalonParamCount = 0;
 
            if (!_points || pointCount < 4)
            {
                assert(0);
                return false;
            }
            break;
 
        default:
            assert(0);
            return false;
    }
 
    if (etalonParamCount > 0)
    {
        arrSize = etalonParamCount * sizeof(etalonParams[0]);
        etalonParams = (double*)cvAlloc(arrSize);
    }
 
    arrSize = pointCount * sizeof(etalonPoints[0]);
 
    if (etalonPointCount != pointCount)
    {
        if (etalonPoints != NULL)
        {
            cvFree(&etalonPoints);
        }
        etalonPointCount = pointCount;
        etalonPoints = (CvPoint2D32f*)cvAlloc(arrSize);
    }
 
    switch (etalonType)
    {
        case CV_CALIB_ETALON_CHESSBOARD:
        {
            int etalonWidth = cvRound(params[0]) - 1;
            int etalonHeight = cvRound(params[1]) - 1;
            int x, y, k = 0;
 
            etalonParams[0] = etalonWidth;
            etalonParams[1] = etalonHeight;
            etalonParams[2] = params[2];
 
            for (y = 0; y < etalonHeight; y++)
                for (x = 0; x < etalonWidth; x++)
                {
                    etalonPoints[k++] =
                        cvPoint2D32f((etalonWidth - 1 - x) * params[2], y * params[2]);
                }
        }
        break;
 
        case CV_CALIB_ETALON_USER:
            if (params != NULL)
            {
                memcpy(etalonParams, params, arrSize);
            }
            if (_points != NULL)
            {
                memcpy(etalonPoints, _points, arrSize);
            }
            break;
 
        default:
            assert(0);
            return false;
    }
 
    return true;
}
 
CvCalibEtalonType
CvCalibFilter::GetEtalon(int* paramCount,
                         const double** params,
                         int* pointCount,
                         const CvPoint2D32f** _points) const
{
    if (paramCount)
    {
        *paramCount = etalonParamCount;
    }
 
    if (params)
    {
        *params = etalonParams;
    }
 
    if (pointCount)
    {
        *pointCount = etalonPointCount;
    }
 
    if (_points)
    {
        *_points = etalonPoints;
    }
 
    return etalonType;
}
 
void
CvCalibFilter::SetCameraCount(int count)
{
    Stop();
 
    if (count != cameraCount)
    {
        for (int i = 0; i < cameraCount; i++)
        {
            cvFree(points + i);
            cvFree(latestPoints + i);
            cvReleaseMat(&undistMap[i][0]);
            cvReleaseMat(&undistMap[i][1]);
            cvReleaseMat(&rectMap[i][0]);
            cvReleaseMat(&rectMap[i][1]);
        }
 
        memset(latestCounts, 0, sizeof(latestCounts));
        maxPoints = 0;
        cameraCount = count;
    }
}
 
bool
CvCalibFilter::SetFrames(int frames)
{
    if (frames < 5)
    {
        assert(0);
        return false;
    }
 
    framesTotal = frames;
    return true;
}
 
/* Calibrates camera using multiple views of calibration pattern */
static void
cvCalibrateCamera(int image_count,
                  int* _point_counts,
                  cv::Size image_size,
                  cv::Point2f* _image_points,
                  cv::Point2f* _object_points,
                  float* _distortion_coeffs,
                  float* _camera_matrix,
                  float* _translation_vectors,
                  float* _rotation_matrices,
                  int flags)
{
    int i, total = 0;
    cv::Mat point_counts = cv::Mat(image_count, 1, CV_32SC1, _point_counts);
    std::vector<std::vector<cv::Point2f>> image_points, object_points;
    cv::Mat dist_coeffs = cv::Mat(4, 1, CV_32FC1, _distortion_coeffs);
    cv::Mat camera_matrix = cv::Mat(3, 3, CV_32FC1, _camera_matrix);
    std::vector<cv::Mat> rotation_matrices;
    std::vector<cv::Mat> translation_vectors;
 
    for (i = 0; i < image_count; i++)
    {
        total += _point_counts[i];
        for (int j = 0; j < _point_counts[i]; j++)
        {
            std::vector<cv::Point2f> img, obj;
            img.insert(img.begin(), _image_points, _image_points + _point_counts[i]);
            obj.insert(obj.begin(), _object_points, _object_points + _point_counts[i]);
            image_points.push_back(img);
            object_points.push_back(obj);
            _image_points += _point_counts[i];
            _object_points += _point_counts[i];
        }
 
        rotation_matrices.emplace_back(1, 9, CV_32FC1, _rotation_matrices);
        translation_vectors.emplace_back(1, 3, CV_32FC1, _translation_vectors);
        _rotation_matrices += 9;
        _translation_vectors += 3;
    }
 
 
    cv::calibrateCamera(object_points,
                        image_points,
                        std::move(image_size),
                        camera_matrix,
                        dist_coeffs,
                        rotation_matrices,
                        translation_vectors,
                        flags);
}
 
#define icvCheckVector_32f(ptr, len)
#define icvCheckVector_64f(ptr, len)
 
CV_INLINE void
icvMulMatrix_32f(const float* src1, int w1, int h1, const float* src2, int w2, int h2, float* dst)
{
    int i, j, k;
 
    if (w1 != h2)
    {
        assert(0);
        return;
    }
 
    for (i = 0; i < h1; i++, src1 += w1, dst += w2)
        for (j = 0; j < w2; j++)
        {
            double s = 0;
            for (k = 0; k < w1; k++)
            {
                s += src1[k] * src2[j + k * w2];
            }
            dst[j] = (float)s;
        }
 
    icvCheckVector_32f(dst, h1 * w2);
}
 
CV_INLINE void
icvSubVector_64d(const double* src1, const double* src2, double* dst, int len)
{
    int i;
    for (i = 0; i < len; i++)
    {
        dst[i] = src1[i] - src2[i];
    }
 
    icvCheckVector_64f(dst, len);
}
 
CV_INLINE void
icvMulMatrix_64d(const double* src1,
                 int w1,
                 int h1,
                 const double* src2,
                 int w2,
                 int h2,
                 double* dst)
{
    int i, j, k;
 
    if (w1 != h2)
    {
        assert(0);
        return;
    }
 
    for (i = 0; i < h1; i++, src1 += w1, dst += w2)
        for (j = 0; j < w2; j++)
        {
            double s = 0;
            for (k = 0; k < w1; k++)
            {
                s += src1[k] * src2[j + k * w2];
            }
            dst[j] = s;
        }
 
    icvCheckVector_64f(dst, h1 * w2);
}
 
CV_INLINE void
icvInvertMatrix_64d(double* A, int n, double* invA)
{
    CvMat Am = cvMat(n, n, CV_64F, A);
    CvMat invAm = cvMat(n, n, CV_64F, invA);
 
    cvInvert(&Am, &invAm, CV_SVD);
}
 
CV_INLINE void
icvAddVector_64d(const double* src1, const double* src2, double* dst, int len)
{
    int i;
    for (i = 0; i < len; i++)
    {
        dst[i] = src1[i] + src2[i];
    }
 
    icvCheckVector_64f(dst, len);
}
 
CV_INLINE void
icvTransposeMatrix_64d(const double* src, int w, int h, double* dst)
{
    int i, j;
 
    for (i = 0; i < w; i++)
        for (j = 0; j < h; j++)
        {
            *dst++ = src[j * w + i];
        }
 
    icvCheckVector_64f(dst, w * h);
}
 
double
cvPseudoInverse(const CvArr* src, CvArr* dst)
{
    return cvInvert(src, dst, CV_SVD);
}
 
// BEGIN
/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
 
 
#include <float.h>
#include <limits.h>
 
#include <utility>
 
/* IPP-compatible return codes */
typedef enum CvStatus
{
    CV_BADMEMBLOCK_ERR = -113,
    CV_INPLACE_NOT_SUPPORTED_ERR = -112,
    CV_UNMATCHED_ROI_ERR = -111,
    CV_NOTFOUND_ERR = -110,
    CV_BADCONVERGENCE_ERR = -109,
 
    CV_BADDEPTH_ERR = -107,
    CV_BADROI_ERR = -106,
    CV_BADHEADER_ERR = -105,
    CV_UNMATCHED_FORMATS_ERR = -104,
    CV_UNSUPPORTED_COI_ERR = -103,
    CV_UNSUPPORTED_CHANNELS_ERR = -102,
    CV_UNSUPPORTED_DEPTH_ERR = -101,
    CV_UNSUPPORTED_FORMAT_ERR = -100,
 
    CV_BADARG_ERR = -49, //ipp comp
    CV_NOTDEFINED_ERR = -48, //ipp comp
 
    CV_BADCHANNELS_ERR = -47, //ipp comp
    CV_BADRANGE_ERR = -44, //ipp comp
    CV_BADSTEP_ERR = -29, //ipp comp
 
    CV_BADFLAG_ERR = -12,
    CV_DIV_BY_ZERO_ERR = -11, //ipp comp
    CV_BADCOEF_ERR = -10,
 
    CV_BADFACTOR_ERR = -7,
    CV_BADPOINT_ERR = -6,
    CV_BADSCALE_ERR = -4,
    CV_OUTOFMEM_ERR = -3,
    CV_NULLPTR_ERR = -2,
    CV_BADSIZE_ERR = -1,
    CV_NO_ERR = 0,
    CV_OK = CV_NO_ERR
} CvStatus;
 
/* Valery Mosyagin */
 
#undef quad
 
#define EPS64D 1e-9
 
using CvMatr32f = float*;
using CvPoint2D64d = CvPoint2D64f;
using CvPoint3D64d = CvPoint3D64f;
using CvVect64d = double*;
using CvMatr64d = double*;
using CvVect32f = float*;
 
#define cvmMul(src1, src2, dst) cvMatMulAdd(src1, src2, 0, dst)
 
enum
{
    CV_MAT32F = CV_32FC1,
    CV_MAT3x1_32F = CV_32FC1,
    CV_MAT4x1_32F = CV_32FC1,
    CV_MAT3x3_32F = CV_32FC1,
    CV_MAT4x4_32F = CV_32FC1,
 
    CV_MAT64D = CV_64FC1,
    CV_MAT3x1_64D = CV_64FC1,
    CV_MAT4x1_64D = CV_64FC1,
    CV_MAT3x3_64D = CV_64FC1,
    CV_MAT4x4_64D = CV_64FC1
};
 
int cvComputeEssentialMatrix(CvMatr32f rotMatr, CvMatr32f transVect, CvMatr32f essMatr);
 
int cvConvertEssential2Fundamental(CvMatr32f essMatr,
                                   CvMatr32f fundMatr,
                                   CvMatr32f cameraMatr1,
                                   CvMatr32f cameraMatr2);
 
int
cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr, CvPoint3D32f* epipole1, CvPoint3D32f* epipole2);
 
void icvTestPoint(CvPoint2D64d testPoint,
                  CvVect64d line1,
                  CvVect64d line2,
                  CvPoint2D64d basePoint,
                  int* result);
 
 
void icvGetPieceLength3D(CvPoint3D64d point1, CvPoint3D64d point2, double* dist);
 
int
icvGetSymPoint3D(CvPoint3D64d pointCorner,
                 CvPoint3D64d point1,
                 CvPoint3D64d point2,
                 CvPoint3D64d* pointSym2)
{
    double len1, len2;
    double alpha;
    icvGetPieceLength3D(pointCorner, point1, &len1);
    if (len1 < EPS64D)
    {
        return CV_BADARG_ERR;
    }
    icvGetPieceLength3D(pointCorner, point2, &len2);
    alpha = len2 / len1;
 
    pointSym2->x = pointCorner.x + alpha * (point1.x - pointCorner.x);
    pointSym2->y = pointCorner.y + alpha * (point1.y - pointCorner.y);
    pointSym2->z = pointCorner.z + alpha * (point1.z - pointCorner.z);
    return CV_NO_ERR;
}
 
/*  author Valery Mosyagin */
 
/* Compute 3D point for scanline and alpha betta */
int
icvCompute3DPoint(double alpha, double betta, CvStereoLineCoeff* coeffs, CvPoint3D64d* point)
{
 
    double partX;
    double partY;
    double partZ;
    double partAll;
    double invPartAll;
 
    double alphabetta = alpha * betta;
 
    partAll = alpha - betta;
    if (fabs(partAll) > 0.00001) /* alpha must be > betta */
    {
 
        partX = coeffs->Xcoef + coeffs->XcoefA * alpha + coeffs->XcoefB * betta +
                coeffs->XcoefAB * alphabetta;
 
        partY = coeffs->Ycoef + coeffs->YcoefA * alpha + coeffs->YcoefB * betta +
                coeffs->YcoefAB * alphabetta;
 
        partZ = coeffs->Zcoef + coeffs->ZcoefA * alpha + coeffs->ZcoefB * betta +
                coeffs->ZcoefAB * alphabetta;
 
        invPartAll = 1.0 / partAll;
 
        point->x = partX * invPartAll;
        point->y = partY * invPartAll;
        point->z = partZ * invPartAll;
        return CV_NO_ERR;
    }
    else
    {
        return CV_BADFACTOR_ERR;
    }
}
 
/*--------------------------------------------------------------------------------------*/
 
/* Compute rotate matrix and trans vector for change system */
int
icvCreateConvertMatrVect(CvMatr64d rotMatr1,
                         CvMatr64d transVect1,
                         CvMatr64d rotMatr2,
                         CvMatr64d transVect2,
                         CvMatr64d convRotMatr,
                         CvMatr64d convTransVect)
{
    double invRotMatr2[9];
    double tmpVect[3];
 
 
    icvInvertMatrix_64d(rotMatr2, 3, invRotMatr2);
    /* Test for error */
 
    icvMulMatrix_64d(rotMatr1, 3, 3, invRotMatr2, 3, 3, convRotMatr);
 
    icvMulMatrix_64d(convRotMatr, 3, 3, transVect2, 1, 3, tmpVect);
 
    icvSubVector_64d(transVect1, tmpVect, convTransVect, 3);
 
 
    return CV_NO_ERR;
}
 
/*--------------------------------------------------------------------------------------*/
 
/* Compute point coordinates in other system */
int
icvConvertPointSystem(CvPoint3D64d M2, CvPoint3D64d* M1, CvMatr64d rotMatr, CvMatr64d transVect)
{
    double tmpVect[3];
 
    icvMulMatrix_64d(rotMatr, 3, 3, (double*)&M2, 1, 3, tmpVect);
 
    icvAddVector_64d(tmpVect, transVect, (double*)M1, 3);
 
    return CV_NO_ERR;
}
 
/*--------------------------------------------------------------------------------------*/
 
int icvComCoeffForLine(CvPoint2D64d point1,
                       CvPoint2D64d point2,
                       CvPoint2D64d point3,
                       CvPoint2D64d point4,
                       CvMatr64d camMatr1,
                       CvMatr64d rotMatr1,
                       CvMatr64d transVect1,
                       CvMatr64d camMatr2,
                       CvMatr64d rotMatr2,
                       CvMatr64d transVect2,
                       CvStereoLineCoeff* coeffs,
                       int* needSwapCamera);
 
static int
icvComputeCoeffForStereoV3(double quad1[4][2],
                           double quad2[4][2],
                           int numScanlines,
                           CvMatr64d camMatr1,
                           CvMatr64d rotMatr1,
                           CvMatr64d transVect1,
                           CvMatr64d camMatr2,
                           CvMatr64d rotMatr2,
                           CvMatr64d transVect2,
                           CvStereoLineCoeff* startCoeffs,
                           int* needSwapCamera)
{
    /* For each pair */
    /* In this function we must define position of cameras */
 
    CvPoint2D64d point1;
    CvPoint2D64d point2;
    CvPoint2D64d point3;
    CvPoint2D64d point4;
 
    int currLine;
    *needSwapCamera = 0;
    for (currLine = 0; currLine < numScanlines; currLine++)
    {
        /* Compute points */
        double alpha = ((double)currLine) / ((double)(numScanlines)); /* maybe - 1 */
 
        point1.x = (1.0 - alpha) * quad1[0][0] + alpha * quad1[3][0];
        point1.y = (1.0 - alpha) * quad1[0][1] + alpha * quad1[3][1];
 
        point2.x = (1.0 - alpha) * quad1[1][0] + alpha * quad1[2][0];
        point2.y = (1.0 - alpha) * quad1[1][1] + alpha * quad1[2][1];
 
        point3.x = (1.0 - alpha) * quad2[0][0] + alpha * quad2[3][0];
        point3.y = (1.0 - alpha) * quad2[0][1] + alpha * quad2[3][1];
 
        point4.x = (1.0 - alpha) * quad2[1][0] + alpha * quad2[2][0];
        point4.y = (1.0 - alpha) * quad2[1][1] + alpha * quad2[2][1];
 
        /* We can compute coeffs for this line */
        icvComCoeffForLine(point1,
                           point2,
                           point3,
                           point4,
                           camMatr1,
                           rotMatr1,
                           transVect1,
                           camMatr2,
                           rotMatr2,
                           transVect2,
                           &startCoeffs[currLine],
                           needSwapCamera);
    }
    return CV_NO_ERR;
}
 
/*--------------------------------------------------------------------------------------*/
 
static int icvCvt_32f_64d(float* src, double* dst, int size);
 
static int
icvComputeCoeffForStereoNew(double quad1[4][2],
                            double quad2[4][2],
                            int numScanlines,
                            CvMatr32f camMatr1,
                            CvMatr32f rotMatr1,
                            CvMatr32f transVect1,
                            CvMatr32f camMatr2,
                            CvStereoLineCoeff* startCoeffs,
                            int* needSwapCamera)
{
    /* Convert data */
 
    double camMatr1_64d[9];
    double camMatr2_64d[9];
 
    double rotMatr1_64d[9];
    double transVect1_64d[3];
 
    double rotMatr2_64d[9];
    double transVect2_64d[3];
 
    icvCvt_32f_64d(camMatr1, camMatr1_64d, 9);
    icvCvt_32f_64d(camMatr2, camMatr2_64d, 9);
 
    icvCvt_32f_64d(rotMatr1, rotMatr1_64d, 9);
    icvCvt_32f_64d(transVect1, transVect1_64d, 3);
 
    rotMatr2_64d[0] = 1;
    rotMatr2_64d[1] = 0;
    rotMatr2_64d[2] = 0;
    rotMatr2_64d[3] = 0;
    rotMatr2_64d[4] = 1;
    rotMatr2_64d[5] = 0;
    rotMatr2_64d[6] = 0;
    rotMatr2_64d[7] = 0;
    rotMatr2_64d[8] = 1;
 
    transVect2_64d[0] = 0;
    transVect2_64d[1] = 0;
    transVect2_64d[2] = 0;
 
    int status = icvComputeCoeffForStereoV3(quad1,
                                            quad2,
                                            numScanlines,
                                            camMatr1_64d,
                                            rotMatr1_64d,
                                            transVect1_64d,
                                            camMatr2_64d,
                                            rotMatr2_64d,
                                            transVect2_64d,
                                            startCoeffs,
                                            needSwapCamera);
 
 
    return status;
}
 
/*--------------------------------------------------------------------------------------*/
int
icvComputeCoeffForStereo(CvStereoCamera* stereoCamera)
{
    double quad1[4][2];
    double quad2[4][2];
 
    int i;
    for (i = 0; i < 4; i++)
    {
        quad1[i][0] = stereoCamera->quad[0][i].x;
        quad1[i][1] = stereoCamera->quad[0][i].y;
 
        quad2[i][0] = stereoCamera->quad[1][i].x;
        quad2[i][1] = stereoCamera->quad[1][i].y;
    }
 
    icvComputeCoeffForStereoNew(quad1,
                                quad2,
                                stereoCamera->warpSize.height,
                                stereoCamera->camera[0]->matrix,
                                stereoCamera->rotMatrix,
                                stereoCamera->transVector,
                                stereoCamera->camera[1]->matrix,
                                stereoCamera->lineCoeffs,
                                &(stereoCamera->needSwapCameras));
    return CV_OK;
}
 
int icvComputeStereoLineCoeffs(CvPoint3D64d pointA,
                               CvPoint3D64d pointB,
                               CvPoint3D64d pointCam1,
                               double gamma,
                               CvStereoLineCoeff* coeffs);
 
int icvGetDirectionForPoint(CvPoint2D64d point, CvMatr64d camMatr, CvPoint3D64d* direct);
 
int icvGetCrossLines(CvPoint3D64d point11,
                     CvPoint3D64d point12,
                     CvPoint3D64d point21,
                     CvPoint3D64d point22,
                     CvPoint3D64d* midPoint);
 
/*--------------------------------------------------------------------------------------*/
int
icvComCoeffForLine(CvPoint2D64d point1,
                   CvPoint2D64d point2,
                   CvPoint2D64d point3,
                   CvPoint2D64d point4,
                   CvMatr64d camMatr1,
                   CvMatr64d rotMatr1,
                   CvMatr64d transVect1,
                   CvMatr64d camMatr2,
                   CvMatr64d rotMatr2,
                   CvMatr64d transVect2,
                   CvStereoLineCoeff* coeffs,
                   int* needSwapCamera)
{
    /* Get direction for all points */
    /* Direction for camera 1 */
 
    CvPoint3D64f direct1;
    CvPoint3D64f direct2;
    CvPoint3D64f camPoint1;
 
    CvPoint3D64f directS3;
    CvPoint3D64f directS4;
    CvPoint3D64f direct3;
    CvPoint3D64f direct4;
    CvPoint3D64f camPoint2;
 
    icvGetDirectionForPoint(point1, camMatr1, &direct1);
 
    icvGetDirectionForPoint(point2, camMatr1, &direct2);
 
    /* Direction for camera 2 */
 
    icvGetDirectionForPoint(point3, camMatr2, &directS3);
 
    icvGetDirectionForPoint(point4, camMatr2, &directS4);
 
    /* Create conversion for camera 2: two direction and camera point */
 
    double convRotMatr[9];
    double convTransVect[3];
 
    icvCreateConvertMatrVect(
        rotMatr1, transVect1, rotMatr2, transVect2, convRotMatr, convTransVect);
 
    CvPoint3D64f zeroVect;
    zeroVect.x = zeroVect.y = zeroVect.z = 0.0;
    camPoint1.x = camPoint1.y = camPoint1.z = 0.0;
 
    icvConvertPointSystem(directS3, &direct3, convRotMatr, convTransVect);
    icvConvertPointSystem(directS4, &direct4, convRotMatr, convTransVect);
    icvConvertPointSystem(zeroVect, &camPoint2, convRotMatr, convTransVect);
 
    CvPoint3D64f pointB;
 
    int postype = 0;
 
    /* Changed order */
    /* Compute point B: xB,yB,zB */
    icvGetCrossLines(camPoint1, direct2, camPoint2, direct3, &pointB);
 
    if (pointB.z < 0) /* If negative use other lines for cross */
    {
        postype = 1;
        icvGetCrossLines(camPoint1, direct1, camPoint2, direct4, &pointB);
    }
 
    CvPoint3D64d pointNewA;
    CvPoint3D64d pointNewC;
 
    pointNewA.x = pointNewA.y = pointNewA.z = 0;
    pointNewC.x = pointNewC.y = pointNewC.z = 0;
 
    if (postype == 0)
    {
        icvGetSymPoint3D(camPoint1, direct1, pointB, &pointNewA);
 
        icvGetSymPoint3D(camPoint2, direct4, pointB, &pointNewC);
    }
    else
    {
        /* In this case we must change cameras */
        *needSwapCamera = 1;
        icvGetSymPoint3D(camPoint2, direct3, pointB, &pointNewA);
 
        icvGetSymPoint3D(camPoint1, direct2, pointB, &pointNewC);
    }
 
 
    double gamma;
 
    double xA, yA, zA;
    double xB, yB, zB;
    double xC, yC, zC;
 
    xA = pointNewA.x;
    yA = pointNewA.y;
    zA = pointNewA.z;
 
    xB = pointB.x;
    yB = pointB.y;
    zB = pointB.z;
 
    xC = pointNewC.x;
    yC = pointNewC.y;
    zC = pointNewC.z;
 
    double len1, len2;
    len1 = sqrt((xA - xB) * (xA - xB) + (yA - yB) * (yA - yB) + (zA - zB) * (zA - zB));
    len2 = sqrt((xB - xC) * (xB - xC) + (yB - yC) * (yB - yC) + (zB - zC) * (zB - zC));
    gamma = len2 / len1;
 
    icvComputeStereoLineCoeffs(pointNewA, pointB, camPoint1, gamma, coeffs);
 
    return CV_NO_ERR;
}
 
/*--------------------------------------------------------------------------------------*/
 
int
icvGetDirectionForPoint(CvPoint2D64d point, CvMatr64d camMatr, CvPoint3D64d* direct)
{
    /*  */
    double invMatr[9];
 
    /* Invert matrix */
 
    icvInvertMatrix_64d(camMatr, 3, invMatr);
    /* TEST FOR ERRORS */
 
    double vect[3];
    vect[0] = point.x;
    vect[1] = point.y;
    vect[2] = 1;
 
    /* Mul matr */
    icvMulMatrix_64d(invMatr, 3, 3, vect, 1, 3, (double*)direct);
 
    return CV_NO_ERR;
}
 
/*--------------------------------------------------------------------------------------*/
 
int
icvGetCrossLines(CvPoint3D64d point11,
                 CvPoint3D64d point12,
                 CvPoint3D64d point21,
                 CvPoint3D64d point22,
                 CvPoint3D64d* midPoint)
{
    double xM, yM, zM;
    double xN, yN, zN;
 
    double xA, yA, zA;
    double xB, yB, zB;
 
    double xC, yC, zC;
    double xD, yD, zD;
 
    xA = point11.x;
    yA = point11.y;
    zA = point11.z;
 
    xB = point12.x;
    yB = point12.y;
    zB = point12.z;
 
    xC = point21.x;
    yC = point21.y;
    zC = point21.z;
 
    xD = point22.x;
    yD = point22.y;
    zD = point22.z;
 
    double a11, a12, a21, a22;
    double b1, b2;
 
    a11 = (xB - xA) * (xB - xA) + (yB - yA) * (yB - yA) + (zB - zA) * (zB - zA);
    a12 = -(xD - xC) * (xB - xA) - (yD - yC) * (yB - yA) - (zD - zC) * (zB - zA);
    a21 = (xB - xA) * (xD - xC) + (yB - yA) * (yD - yC) + (zB - zA) * (zD - zC);
    a22 = -(xD - xC) * (xD - xC) - (yD - yC) * (yD - yC) - (zD - zC) * (zD - zC);
    b1 = -((xA - xC) * (xB - xA) + (yA - yC) * (yB - yA) + (zA - zC) * (zB - zA));
    b2 = -((xA - xC) * (xD - xC) + (yA - yC) * (yD - yC) + (zA - zC) * (zD - zC));
 
    double delta;
    double deltaA, deltaB;
    double alpha, betta;
 
    delta = a11 * a22 - a12 * a21;
 
    if (fabs(delta) < EPS64D)
    {
        /*return ERROR;*/
    }
 
    deltaA = b1 * a22 - b2 * a12;
    deltaB = a11 * b2 - b1 * a21;
 
    alpha = deltaA / delta;
    betta = deltaB / delta;
 
    xM = xA + alpha * (xB - xA);
    yM = yA + alpha * (yB - yA);
    zM = zA + alpha * (zB - zA);
 
    xN = xC + betta * (xD - xC);
    yN = yC + betta * (yD - yC);
    zN = zC + betta * (zD - zC);
 
    /* Compute middle point */
    midPoint->x = (xM + xN) * 0.5;
    midPoint->y = (yM + yN) * 0.5;
    midPoint->z = (zM + zN) * 0.5;
 
    return CV_NO_ERR;
}
 
/*--------------------------------------------------------------------------------------*/
 
int
icvComputeStereoLineCoeffs(CvPoint3D64d pointA,
                           CvPoint3D64d pointB,
                           CvPoint3D64d pointCam1,
                           double gamma,
                           CvStereoLineCoeff* coeffs)
{
    double x1, y1, z1;
 
    x1 = pointCam1.x;
    y1 = pointCam1.y;
    z1 = pointCam1.z;
 
    double xA, yA, zA;
    double xB, yB, zB;
 
    xA = pointA.x;
    yA = pointA.y;
    zA = pointA.z;
 
    xB = pointB.x;
    yB = pointB.y;
    zB = pointB.z;
 
    if (gamma > 0)
    {
        coeffs->Xcoef = -x1 + xA;
        coeffs->XcoefA = xB + x1 - xA;
        coeffs->XcoefB = -xA - gamma * x1 + gamma * xA;
        coeffs->XcoefAB = -xB + xA + gamma * xB - gamma * xA;
 
        coeffs->Ycoef = -y1 + yA;
        coeffs->YcoefA = yB + y1 - yA;
        coeffs->YcoefB = -yA - gamma * y1 + gamma * yA;
        coeffs->YcoefAB = -yB + yA + gamma * yB - gamma * yA;
 
        coeffs->Zcoef = -z1 + zA;
        coeffs->ZcoefA = zB + z1 - zA;
        coeffs->ZcoefB = -zA - gamma * z1 + gamma * zA;
        coeffs->ZcoefAB = -zB + zA + gamma * zB - gamma * zA;
    }
    else
    {
        gamma = -gamma;
        coeffs->Xcoef = -(-x1 + xA);
        coeffs->XcoefB = -(xB + x1 - xA);
        coeffs->XcoefA = -(-xA - gamma * x1 + gamma * xA);
        coeffs->XcoefAB = -(-xB + xA + gamma * xB - gamma * xA);
 
        coeffs->Ycoef = -(-y1 + yA);
        coeffs->YcoefB = -(yB + y1 - yA);
        coeffs->YcoefA = -(-yA - gamma * y1 + gamma * yA);
        coeffs->YcoefAB = -(-yB + yA + gamma * yB - gamma * yA);
 
        coeffs->Zcoef = -(-z1 + zA);
        coeffs->ZcoefB = -(zB + z1 - zA);
        coeffs->ZcoefA = -(-zA - gamma * z1 + gamma * zA);
        coeffs->ZcoefAB = -(-zB + zA + gamma * zB - gamma * zA);
    }
 
 
    return CV_NO_ERR;
}
 
/*--------------------------------------------------------------------------------------*/
 
 
/*---------------------------------------------------------------------------------------*/
 
/* This function get minimum angle started at point which contains rect */
int
icvGetAngleLine(CvPoint2D64d startPoint,
                CvSize imageSize,
                CvPoint2D64d* point1,
                CvPoint2D64d* point2)
{
    /* Get crosslines with image corners */
 
    /* Find four lines */
 
    CvPoint2D64d pa, pb, pc, pd;
 
    pa.x = 0;
    pa.y = 0;
 
    pb.x = imageSize.width - 1;
    pb.y = 0;
 
    pd.x = imageSize.width - 1;
    pd.y = imageSize.height - 1;
 
    pc.x = 0;
    pc.y = imageSize.height - 1;
 
    /* We can compute points for angle */
    /* Test for place section */
 
    if (startPoint.x < 0)
    {
        /* 1,4,7 */
        if (startPoint.y < 0)
        {
            /* 1 */
            *point1 = pb;
            *point2 = pc;
        }
        else if (startPoint.y > imageSize.height - 1)
        {
            /* 7 */
            *point1 = pa;
            *point2 = pd;
        }
        else
        {
            /* 4 */
            *point1 = pa;
            *point2 = pc;
        }
    }
    else if (startPoint.x > imageSize.width - 1)
    {
        /* 3,6,9 */
        if (startPoint.y < 0)
        {
            /* 3 */
            *point1 = pa;
            *point2 = pd;
        }
        else if (startPoint.y > imageSize.height - 1)
        {
            /* 9 */
            *point1 = pb;
            *point2 = pc;
        }
        else
        {
            /* 6 */
            *point1 = pb;
            *point2 = pd;
        }
    }
    else
    {
        /* 2,5,8 */
        if (startPoint.y < 0)
        {
            /* 2 */
            if (startPoint.x < imageSize.width / 2)
            {
                *point1 = pb;
                *point2 = pa;
            }
            else
            {
                *point1 = pa;
                *point2 = pb;
            }
        }
        else if (startPoint.y > imageSize.height - 1)
        {
            /* 8 */
            if (startPoint.x < imageSize.width / 2)
            {
                *point1 = pc;
                *point2 = pd;
            }
            else
            {
                *point1 = pd;
                *point2 = pc;
            }
        }
        else
        {
            /* 5 - point in the image */
            return 2;
        }
    }
    return 0;
} /* GetAngleLine */
 
/*---------------------------------------------------------------------------------------*/
 
void
icvGetCoefForPiece(CvPoint2D64d p_start,
                   CvPoint2D64d p_end,
                   double* a,
                   double* b,
                   double* c,
                   int* result)
{
    double det;
    double detA, detB, detC;
 
    det = p_start.x * p_end.y + p_end.x + p_start.y - p_end.y - p_start.y * p_end.x - p_start.x;
    if (fabs(det) < EPS64D) /* Error */
    {
        *result = 0;
        return;
    }
 
    detA = p_start.y - p_end.y;
    detB = p_end.x - p_start.x;
    detC = p_start.x * p_end.y - p_end.x * p_start.y;
 
    double invDet = 1.0 / det;
    *a = detA * invDet;
    *b = detB * invDet;
    *c = detC * invDet;
 
    *result = 1;
    return;
}
 
/*---------------------------------------------------------------------------------------*/
 
#define icvTransformVector_64d(matr, src, dst, w, h) icvMulMatrix_64d(matr, w, h, src, 1, w, dst)
 
void icvGetCrossRectDirect(CvSize imageSize,
                           double a,
                           double b,
                           double c,
                           CvPoint2D64d* start,
                           CvPoint2D64d* end,
                           int* result);
 
/* Get common area of rectifying */
static void
icvGetCommonArea(CvSize imageSize,
                 CvPoint3D64d epipole1,
                 CvPoint3D64d epipole2,
                 CvMatr64d fundMatr,
                 CvVect64d coeff11,
                 CvVect64d coeff12,
                 CvVect64d coeff21,
                 CvVect64d coeff22,
                 int* result)
{
    int res = 0;
    CvPoint2D64d point11;
    CvPoint2D64d point12;
    CvPoint2D64d point21;
    CvPoint2D64d point22;
 
    double corr11[3];
    double corr12[3];
    double corr21[3];
    double corr22[3];
 
    double pointW11[3];
    double pointW12[3];
    double pointW21[3];
    double pointW22[3];
 
    double transFundMatr[3 * 3];
    /* Compute transpose of fundamental matrix */
    icvTransposeMatrix_64d(fundMatr, 3, 3, transFundMatr);
 
    CvPoint2D64d epipole1_2d;
    CvPoint2D64d epipole2_2d;
 
    if (fabs(epipole1.z) < 1e-8)
    {
        /* epipole1 in infinity */
        *result = 0;
        return;
    }
    epipole1_2d.x = epipole1.x / epipole1.z;
    epipole1_2d.y = epipole1.y / epipole1.z;
 
    if (fabs(epipole2.z) < 1e-8)
    {
        /* epipole2 in infinity */
        *result = 0;
        return;
    }
    epipole2_2d.x = epipole2.x / epipole2.z;
    epipole2_2d.y = epipole2.y / epipole2.z;
 
    int stat = icvGetAngleLine(epipole1_2d, imageSize, &point11, &point12);
    if (stat == 2)
    {
        /* No angle */
        *result = 0;
        return;
    }
 
    stat = icvGetAngleLine(epipole2_2d, imageSize, &point21, &point22);
    if (stat == 2)
    {
        /* No angle */
        *result = 0;
        return;
    }
 
    /* ============= Computation for line 1 ================ */
    /* Find correspondence line for angle points11 */
    /* corr21 = Fund'*p1 */
 
    pointW11[0] = point11.x;
    pointW11[1] = point11.y;
    pointW11[2] = 1.0;
 
    icvTransformVector_64d(transFundMatr, /* !!! Modified from not transposed */
                           pointW11,
                           corr21,
                           3,
                           3);
 
    /* Find crossing of line with image 2 */
    CvPoint2D64d start;
    CvPoint2D64d end;
    icvGetCrossRectDirect(imageSize, corr21[0], corr21[1], corr21[2], &start, &end, &res);
 
    if (res == 0)
    {
        /* We have not cross */
        /* We must define new angle */
 
        pointW21[0] = point21.x;
        pointW21[1] = point21.y;
        pointW21[2] = 1.0;
 
        /* Find correspondence line for this angle points */
        /* We know point and try to get corr line */
        /* For point21 */
        /* corr11 = Fund * p21 */
 
        icvTransformVector_64d(fundMatr, /* !!! Modified */
                               pointW21,
                               corr11,
                               3,
                               3);
 
        /* We have cross. And it's result cross for up line. Set result coefs */
 
        /* Set coefs for line 1 image 1 */
        coeff11[0] = corr11[0];
        coeff11[1] = corr11[1];
        coeff11[2] = corr11[2];
 
        /* Set coefs for line 1 image 2 */
        icvGetCoefForPiece(epipole2_2d, point21, &coeff21[0], &coeff21[1], &coeff21[2], &res);
        if (res == 0)
        {
            *result = 0;
            return; /* Error */
        }
    }
    else
    {
        /* Line 1 cross image 2 */
        /* Set coefs for line 1 image 1 */
        icvGetCoefForPiece(epipole1_2d, point11, &coeff11[0], &coeff11[1], &coeff11[2], &res);
        if (res == 0)
        {
            *result = 0;
            return; /* Error */
        }
 
        /* Set coefs for line 1 image 2 */
        coeff21[0] = corr21[0];
        coeff21[1] = corr21[1];
        coeff21[2] = corr21[2];
    }
 
    /* ============= Computation for line 2 ================ */
    /* Find correspondence line for angle points11 */
    /* corr22 = Fund*p2 */
 
    pointW12[0] = point12.x;
    pointW12[1] = point12.y;
    pointW12[2] = 1.0;
 
    icvTransformVector_64d(transFundMatr, pointW12, corr22, 3, 3);
 
    /* Find crossing of line with image 2 */
    icvGetCrossRectDirect(imageSize, corr22[0], corr22[1], corr22[2], &start, &end, &res);
 
    if (res == 0)
    {
        /* We have not cross */
        /* We must define new angle */
 
        pointW22[0] = point22.x;
        pointW22[1] = point22.y;
        pointW22[2] = 1.0;
 
        /* Find correspondence line for this angle points */
        /* We know point and try to get corr line */
        /* For point21 */
        /* corr2 = Fund' * p1 */
 
        icvTransformVector_64d(fundMatr, pointW22, corr12, 3, 3);
 
 
        /* We have cross. And it's result cross for down line. Set result coefs */
 
        /* Set coefs for line 2 image 1 */
        coeff12[0] = corr12[0];
        coeff12[1] = corr12[1];
        coeff12[2] = corr12[2];
 
        /* Set coefs for line 1 image 2 */
        icvGetCoefForPiece(epipole2_2d, point22, &coeff22[0], &coeff22[1], &coeff22[2], &res);
        if (res == 0)
        {
            *result = 0;
            return; /* Error */
        }
    }
    else
    {
        /* Line 2 cross image 2 */
        /* Set coefs for line 2 image 1 */
        icvGetCoefForPiece(epipole1_2d, point12, &coeff12[0], &coeff12[1], &coeff12[2], &res);
        if (res == 0)
        {
            *result = 0;
            return; /* Error */
        }
 
        /* Set coefs for line 1 image 2 */
        coeff22[0] = corr22[0];
        coeff22[1] = corr22[1];
        coeff22[2] = corr22[2];
    }
 
    /* Now we know common area */
 
    return;
 
} /* GetCommonArea */
 
/*---------------------------------------------------------------------------------------*/
 
/* Get cross for direction1 and direction2 */
/*  Result = 1 - cross */
/*  Result = 2 - parallel and not equal */
/*  Result = 3 - parallel and equal */
 
void
icvGetCrossDirectDirect(CvVect64d direct1, CvVect64d direct2, CvPoint2D64d* cross, int* result)
{
    double det = direct1[0] * direct2[1] - direct2[0] * direct1[1];
    double detx = -direct1[2] * direct2[1] + direct1[1] * direct2[2];
 
    if (fabs(det) > EPS64D)
    {
        /* Have cross */
        cross->x = detx / det;
        cross->y = (-direct1[0] * direct2[2] + direct2[0] * direct1[2]) / det;
        *result = 1;
    }
    else
    {
        /* may be parallel */
        if (fabs(detx) > EPS64D)
        {
            /* parallel and not equal */
            *result = 2;
        }
        else
        {
            /* equals */
            *result = 3;
        }
    }
 
    return;
}
 
/*---------------------------------------------------------------------------------------*/
 
/* Get cross for piece p1,p2 and direction a,b,c */
/*  Result = 0 - no cross */
/*  Result = 1 - cross */
/*  Result = 2 - parallel and not equal */
/*  Result = 3 - parallel and equal */
 
void
icvGetCrossPieceDirect(CvPoint2D64d p_start,
                       CvPoint2D64d p_end,
                       double a,
                       double b,
                       double c,
                       CvPoint2D64d* cross,
                       int* result)
{
 
    if ((a * p_start.x + b * p_start.y + c) * (a * p_end.x + b * p_end.y + c) <= 0)
    {
        /* Have cross */
        double det;
        double detxc, detyc;
 
        det = a * (p_end.x - p_start.x) + b * (p_end.y - p_start.y);
 
        if (fabs(det) < EPS64D)
        {
            /* lines are parallel and may be equal or line is point */
            if (fabs(a * p_start.x + b * p_start.y + c) < EPS64D)
            {
                /* line is point or not diff */
                *result = 3;
                return;
            }
            else
            {
                *result = 2;
            }
            return;
        }
 
        detxc = b * (p_end.y * p_start.x - p_start.y * p_end.x) + c * (p_start.x - p_end.x);
        detyc = a * (p_end.x * p_start.y - p_start.x * p_end.y) + c * (p_start.y - p_end.y);
 
        cross->x = detxc / det;
        cross->y = detyc / det;
        *result = 1;
    }
    else
    {
        *result = 0;
    }
    return;
}
 
/*--------------------------------------------------------------------------------------*/
 
void
icvGetCrossPiecePiece(CvPoint2D64d p1_start,
                      CvPoint2D64d p1_end,
                      CvPoint2D64d p2_start,
                      CvPoint2D64d p2_end,
                      CvPoint2D64d* cross,
                      int* result)
{
    double ex1, ey1, ex2, ey2;
    double px1, py1, px2, py2;
    double del;
    double delA, delB, delX, delY;
    double alpha, betta;
 
    ex1 = p1_start.x;
    ey1 = p1_start.y;
    ex2 = p1_end.x;
    ey2 = p1_end.y;
 
    px1 = p2_start.x;
    py1 = p2_start.y;
    px2 = p2_end.x;
    py2 = p2_end.y;
 
    del = (py1 - py2) * (ex1 - ex2) - (px1 - px2) * (ey1 - ey2);
    if (fabs(del) <= EPS64D)
    {
        /* May be they are parallel !!! */
        *result = 0;
        return;
    }
 
    delA = (ey1 - ey2) * (ex1 - px1) + (ex1 - ex2) * (py1 - ey1);
    delB = (py1 - py2) * (ex1 - px1) + (px1 - px2) * (py1 - ey1);
 
    alpha = delA / del;
    betta = delB / del;
 
    if (alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
    {
        *result = 0;
        return;
    }
 
    delX = (px1 - px2) * (ey1 * (ex1 - ex2) - ex1 * (ey1 - ey2)) +
           (ex1 - ex2) * (px1 * (py1 - py2) - py1 * (px1 - px2));
 
    delY = (py1 - py2) * (ey1 * (ex1 - ex2) - ex1 * (ey1 - ey2)) +
           (ey1 - ey2) * (px1 * (py1 - py2) - py1 * (px1 - px2));
 
    cross->x = delX / del;
    cross->y = delY / del;
 
    *result = 1;
    return;
}
 
/*---------------------------------------------------------------------------------------*/
 
void
icvGetPieceLength(CvPoint2D64d point1, CvPoint2D64d point2, double* dist)
{
    double dx = point2.x - point1.x;
    double dy = point2.y - point1.y;
    *dist = sqrt(dx * dx + dy * dy);
    return;
}
 
/*---------------------------------------------------------------------------------------*/
 
void
icvGetPieceLength3D(CvPoint3D64d point1, CvPoint3D64d point2, double* dist)
{
    double dx = point2.x - point1.x;
    double dy = point2.y - point1.y;
    double dz = point2.z - point1.z;
    *dist = sqrt(dx * dx + dy * dy + dz * dz);
    return;
}
 
/*---------------------------------------------------------------------------------------*/
 
/* Find line from epipole which cross image rect */
/* Find points of cross 0 or 1 or 2. Return number of points in cross */
void
icvGetCrossRectDirect(CvSize imageSize,
                      double a,
                      double b,
                      double c,
                      CvPoint2D64d* start,
                      CvPoint2D64d* end,
                      int* result)
{
    CvPoint2D64d frameBeg;
    CvPoint2D64d frameEnd;
    CvPoint2D64d cross[4];
    int haveCross[4];
 
    haveCross[0] = 0;
    haveCross[1] = 0;
    haveCross[2] = 0;
    haveCross[3] = 0;
 
    frameBeg.x = 0;
    frameBeg.y = 0;
    frameEnd.x = imageSize.width;
    frameEnd.y = 0;
 
    icvGetCrossPieceDirect(frameBeg, frameEnd, a, b, c, &cross[0], &haveCross[0]);
 
    frameBeg.x = imageSize.width;
    frameBeg.y = 0;
    frameEnd.x = imageSize.width;
    frameEnd.y = imageSize.height;
    icvGetCrossPieceDirect(frameBeg, frameEnd, a, b, c, &cross[1], &haveCross[1]);
 
    frameBeg.x = imageSize.width;
    frameBeg.y = imageSize.height;
    frameEnd.x = 0;
    frameEnd.y = imageSize.height;
    icvGetCrossPieceDirect(frameBeg, frameEnd, a, b, c, &cross[2], &haveCross[2]);
 
    frameBeg.x = 0;
    frameBeg.y = imageSize.height;
    frameEnd.x = 0;
    frameEnd.y = 0;
    icvGetCrossPieceDirect(frameBeg, frameEnd, a, b, c, &cross[3], &haveCross[3]);
 
    double maxDist;
 
    int maxI = 0, maxJ = 0;
 
 
    int i, j;
 
    maxDist = -1.0;
 
    double distance;
 
    for (i = 0; i < 3; i++)
    {
        if (haveCross[i] == 1)
        {
            for (j = i + 1; j < 4; j++)
            {
                if (haveCross[j] == 1)
                {
                    /* Compute dist */
                    icvGetPieceLength(cross[i], cross[j], &distance);
                    if (distance > maxDist)
                    {
                        maxI = i;
                        maxJ = j;
                        maxDist = distance;
                    }
                }
            }
        }
    }
 
    if (maxDist >= 0)
    {
        /* We have cross */
        *start = cross[maxI];
        *result = 1;
        if (maxDist > 0)
        {
            *end = cross[maxJ];
            *result = 2;
        }
    }
    else
    {
        *result = 0;
    }
 
    return;
} /* GetCrossRectDirect */
 
/*---------------------------------------------------------------------------------------*/
void
icvProjectPointToImage(CvPoint3D64d point,
                       CvMatr64d camMatr,
                       CvMatr64d rotMatr,
                       CvVect64d transVect,
                       CvPoint2D64d* projPoint)
{
 
    double tmpVect1[3];
    double tmpVect2[3];
 
    icvMulMatrix_64d(rotMatr, 3, 3, (double*)&point, 1, 3, tmpVect1);
 
    icvAddVector_64d(tmpVect1, transVect, tmpVect2, 3);
 
    icvMulMatrix_64d(camMatr, 3, 3, tmpVect2, 1, 3, tmpVect1);
 
    projPoint->x = tmpVect1[0] / tmpVect1[2];
    projPoint->y = tmpVect1[1] / tmpVect1[2];
 
    return;
}
 
static int icvCvt_64d_32f(double* src, float* dst, int size);
 
void icvGetCutPiece(CvVect64d areaLineCoef1,
                    CvVect64d areaLineCoef2,
                    CvPoint2D64d epipole,
                    CvSize imageSize,
                    CvPoint2D64d* point11,
                    CvPoint2D64d* point12,
                    CvPoint2D64d* point21,
                    CvPoint2D64d* point22,
                    int* result);
 
void icvComputeeInfiniteProject1(CvMatr64d rotMatr,
                                 CvMatr64d camMatr1,
                                 CvMatr64d camMatr2,
                                 CvPoint2D32f point1,
                                 CvPoint2D32f* point2);
 
void icvComputeeInfiniteProject2(CvMatr64d rotMatr,
                                 CvMatr64d camMatr1,
                                 CvMatr64d camMatr2,
                                 CvPoint2D32f* point1,
                                 CvPoint2D32f point2);
 
/*---------------------------------------------------------------------------------------*/
/* Get quads for transform images */
void
icvGetQuadsTransform(CvSize imageSize,
                     CvMatr64d camMatr1,
                     CvMatr64d rotMatr1,
                     CvVect64d transVect1,
                     CvMatr64d camMatr2,
                     CvMatr64d rotMatr2,
                     CvVect64d transVect2,
                     CvSize* warpSize,
                     double quad1[4][2],
                     double quad2[4][2],
                     CvMatr64d fundMatr,
                     CvPoint3D64d* epipole1,
                     CvPoint3D64d* epipole2)
{
    /* First compute fundamental matrix and epipoles */
    int res;
 
 
    /* Compute epipoles and fundamental matrix using new functions */
    {
        double convRotMatr[9];
        double convTransVect[3];
 
        icvCreateConvertMatrVect(
            rotMatr1, transVect1, rotMatr2, transVect2, convRotMatr, convTransVect);
        float convRotMatr_32f[9];
        float convTransVect_32f[3];
 
        icvCvt_64d_32f(convRotMatr, convRotMatr_32f, 9);
        icvCvt_64d_32f(convTransVect, convTransVect_32f, 3);
 
        /* We know R and t */
        /* Compute essential matrix */
        float essMatr[9];
        float fundMatr_32f[9];
 
        float camMatr1_32f[9];
        float camMatr2_32f[9];
 
        icvCvt_64d_32f(camMatr1, camMatr1_32f, 9);
        icvCvt_64d_32f(camMatr2, camMatr2_32f, 9);
 
        cvComputeEssentialMatrix(convRotMatr_32f, convTransVect_32f, essMatr);
 
        cvConvertEssential2Fundamental(essMatr, fundMatr_32f, camMatr1_32f, camMatr2_32f);
 
        CvPoint3D32f epipole1_32f;
        CvPoint3D32f epipole2_32f;
 
        cvComputeEpipolesFromFundMatrix(fundMatr_32f, &epipole1_32f, &epipole2_32f);
        /* copy to 64d epipoles */
        epipole1->x = epipole1_32f.x;
        epipole1->y = epipole1_32f.y;
        epipole1->z = epipole1_32f.z;
 
        epipole2->x = epipole2_32f.x;
        epipole2->y = epipole2_32f.y;
        epipole2->z = epipole2_32f.z;
 
        /* Convert fundamental matrix */
        icvCvt_32f_64d(fundMatr_32f, fundMatr, 9);
    }
 
    double coeff11[3];
    double coeff12[3];
    double coeff21[3];
    double coeff22[3];
 
    icvGetCommonArea(
        imageSize, *epipole1, *epipole2, fundMatr, coeff11, coeff12, coeff21, coeff22, &res);
 
    CvPoint2D64d point11, point12, point21, point22;
    double width1, width2;
    double height1, height2;
    double tmpHeight1, tmpHeight2;
 
    CvPoint2D64d epipole1_2d;
    CvPoint2D64d epipole2_2d;
 
    /* ----- Image 1 ----- */
    if (fabs(epipole1->z) < 1e-8)
    {
        return;
    }
    epipole1_2d.x = epipole1->x / epipole1->z;
    epipole1_2d.y = epipole1->y / epipole1->z;
 
    icvGetCutPiece(
        coeff11, coeff12, epipole1_2d, imageSize, &point11, &point12, &point21, &point22, &res);
 
    /* Compute distance */
    icvGetPieceLength(point11, point21, &width1);
    icvGetPieceLength(point11, point12, &tmpHeight1);
    icvGetPieceLength(point21, point22, &tmpHeight2);
    height1 = MAX(tmpHeight1, tmpHeight2);
 
    quad1[0][0] = point11.x;
    quad1[0][1] = point11.y;
 
    quad1[1][0] = point21.x;
    quad1[1][1] = point21.y;
 
    quad1[2][0] = point22.x;
    quad1[2][1] = point22.y;
 
    quad1[3][0] = point12.x;
    quad1[3][1] = point12.y;
 
    /* ----- Image 2 ----- */
    if (fabs(epipole2->z) < 1e-8)
    {
        return;
    }
    epipole2_2d.x = epipole2->x / epipole2->z;
    epipole2_2d.y = epipole2->y / epipole2->z;
 
    icvGetCutPiece(
        coeff21, coeff22, epipole2_2d, imageSize, &point11, &point12, &point21, &point22, &res);
 
    /* Compute distance */
    icvGetPieceLength(point11, point21, &width2);
    icvGetPieceLength(point11, point12, &tmpHeight1);
    icvGetPieceLength(point21, point22, &tmpHeight2);
    height2 = MAX(tmpHeight1, tmpHeight2);
 
    quad2[0][0] = point11.x;
    quad2[0][1] = point11.y;
 
    quad2[1][0] = point21.x;
    quad2[1][1] = point21.y;
 
    quad2[2][0] = point22.x;
    quad2[2][1] = point22.y;
 
    quad2[3][0] = point12.x;
    quad2[3][1] = point12.y;
 
 
    /*=======================================================*/
    /* This is a new additional way to compute quads. */
    /* We must correct quads */
    {
        double convRotMatr[9];
        double convTransVect[3];
 
        double newQuad1[4][2];
        double newQuad2[4][2];
 
 
        icvCreateConvertMatrVect(
            rotMatr1, transVect1, rotMatr2, transVect2, convRotMatr, convTransVect);
 
        /* -------------Compute for first image-------------- */
        CvPoint2D32f pointb1;
        CvPoint2D32f pointe1;
 
        CvPoint2D32f pointb2;
        CvPoint2D32f pointe2;
 
        pointb1.x = (float)quad1[0][0];
        pointb1.y = (float)quad1[0][1];
 
        pointe1.x = (float)quad1[3][0];
        pointe1.y = (float)quad1[3][1];
 
        icvComputeeInfiniteProject1(convRotMatr, camMatr1, camMatr2, pointb1, &pointb2);
 
        icvComputeeInfiniteProject1(convRotMatr, camMatr1, camMatr2, pointe1, &pointe2);
 
        /*  JUST TEST FOR POINT */
 
        /* Compute distances */
        double dxOld, dyOld;
        double dxNew, dyNew;
        double distOld, distNew;
 
        dxOld = quad2[1][0] - quad2[0][0];
        dyOld = quad2[1][1] - quad2[0][1];
        distOld = dxOld * dxOld + dyOld * dyOld;
 
        dxNew = quad2[1][0] - pointb2.x;
        dyNew = quad2[1][1] - pointb2.y;
        distNew = dxNew * dxNew + dyNew * dyNew;
 
        if (distNew > distOld)
        {
            /* Get new points for second quad */
            newQuad2[0][0] = pointb2.x;
            newQuad2[0][1] = pointb2.y;
            newQuad2[3][0] = pointe2.x;
            newQuad2[3][1] = pointe2.y;
            newQuad1[0][0] = quad1[0][0];
            newQuad1[0][1] = quad1[0][1];
            newQuad1[3][0] = quad1[3][0];
            newQuad1[3][1] = quad1[3][1];
        }
        else
        {
            /* Get new points for first quad */
 
            pointb2.x = (float)quad2[0][0];
            pointb2.y = (float)quad2[0][1];
 
            pointe2.x = (float)quad2[3][0];
            pointe2.y = (float)quad2[3][1];
 
            icvComputeeInfiniteProject2(convRotMatr, camMatr1, camMatr2, &pointb1, pointb2);
 
            icvComputeeInfiniteProject2(convRotMatr, camMatr1, camMatr2, &pointe1, pointe2);
 
 
            /*  JUST TEST FOR POINT */
 
            newQuad2[0][0] = quad2[0][0];
            newQuad2[0][1] = quad2[0][1];
            newQuad2[3][0] = quad2[3][0];
            newQuad2[3][1] = quad2[3][1];
 
            newQuad1[0][0] = pointb1.x;
            newQuad1[0][1] = pointb1.y;
            newQuad1[3][0] = pointe1.x;
            newQuad1[3][1] = pointe1.y;
        }
 
        /* -------------Compute for second image-------------- */
        pointb1.x = (float)quad1[1][0];
        pointb1.y = (float)quad1[1][1];
 
        pointe1.x = (float)quad1[2][0];
        pointe1.y = (float)quad1[2][1];
 
        icvComputeeInfiniteProject1(convRotMatr, camMatr1, camMatr2, pointb1, &pointb2);
 
        icvComputeeInfiniteProject1(convRotMatr, camMatr1, camMatr2, pointe1, &pointe2);
 
        /* Compute distances */
 
        dxOld = quad2[0][0] - quad2[1][0];
        dyOld = quad2[0][1] - quad2[1][1];
        distOld = dxOld * dxOld + dyOld * dyOld;
 
        dxNew = quad2[0][0] - pointb2.x;
        dyNew = quad2[0][1] - pointb2.y;
        distNew = dxNew * dxNew + dyNew * dyNew;
 
        if (distNew > distOld)
        {
            /* Get new points for second quad */
            newQuad2[1][0] = pointb2.x;
            newQuad2[1][1] = pointb2.y;
            newQuad2[2][0] = pointe2.x;
            newQuad2[2][1] = pointe2.y;
            newQuad1[1][0] = quad1[1][0];
            newQuad1[1][1] = quad1[1][1];
            newQuad1[2][0] = quad1[2][0];
            newQuad1[2][1] = quad1[2][1];
        }
        else
        {
            /* Get new points for first quad */
 
            pointb2.x = (float)quad2[1][0];
            pointb2.y = (float)quad2[1][1];
 
            pointe2.x = (float)quad2[2][0];
            pointe2.y = (float)quad2[2][1];
 
            icvComputeeInfiniteProject2(convRotMatr, camMatr1, camMatr2, &pointb1, pointb2);
 
            icvComputeeInfiniteProject2(convRotMatr, camMatr1, camMatr2, &pointe1, pointe2);
 
            newQuad2[1][0] = quad2[1][0];
            newQuad2[1][1] = quad2[1][1];
            newQuad2[2][0] = quad2[2][0];
            newQuad2[2][1] = quad2[2][1];
 
            newQuad1[1][0] = pointb1.x;
            newQuad1[1][1] = pointb1.y;
            newQuad1[2][0] = pointe1.x;
            newQuad1[2][1] = pointe1.y;
        }
 
 
        /*-------------------------------------------------------------------------------*/
 
        /* Copy new quads to old quad */
        int i;
        for (i = 0; i < 4; i++)
        {
            {
                quad1[i][0] = newQuad1[i][0];
                quad1[i][1] = newQuad1[i][1];
                quad2[i][0] = newQuad2[i][0];
                quad2[i][1] = newQuad2[i][1];
            }
        }
    }
    /*=======================================================*/
 
    double warpWidth, warpHeight;
 
    warpWidth = MAX(width1, width2);
    warpHeight = MAX(height1, height2);
 
    warpSize->width = (int)warpWidth;
    warpSize->height = (int)warpHeight;
 
    warpSize->width = cvRound(warpWidth - 1);
    warpSize->height = cvRound(warpHeight - 1);
 
    /* !!! by Valery Mosyagin. this lines added just for test no warp */
    warpSize->width = imageSize.width;
    warpSize->height = imageSize.height;
 
    return;
}
 
/*---------------------------------------------------------------------------------------*/
 
static void
icvGetQuadsTransformNew(CvSize imageSize,
                        CvMatr32f camMatr1,
                        CvMatr32f camMatr2,
                        CvMatr32f rotMatr1,
                        CvVect32f transVect1,
                        CvSize* warpSize,
                        double quad1[4][2],
                        double quad2[4][2],
                        CvMatr32f fundMatr,
                        CvPoint3D32f* epipole1,
                        CvPoint3D32f* epipole2)
{
    /* Convert data */
    /* Convert camera matrix */
    double camMatr1_64d[9];
    double camMatr2_64d[9];
    double rotMatr1_64d[9];
    double transVect1_64d[3];
    double rotMatr2_64d[9];
    double transVect2_64d[3];
    double fundMatr_64d[9];
    CvPoint3D64d epipole1_64d;
    CvPoint3D64d epipole2_64d;
 
    icvCvt_32f_64d(camMatr1, camMatr1_64d, 9);
    icvCvt_32f_64d(camMatr2, camMatr2_64d, 9);
    icvCvt_32f_64d(rotMatr1, rotMatr1_64d, 9);
    icvCvt_32f_64d(transVect1, transVect1_64d, 3);
 
    /* Create vector and matrix */
 
    rotMatr2_64d[0] = 1;
    rotMatr2_64d[1] = 0;
    rotMatr2_64d[2] = 0;
    rotMatr2_64d[3] = 0;
    rotMatr2_64d[4] = 1;
    rotMatr2_64d[5] = 0;
    rotMatr2_64d[6] = 0;
    rotMatr2_64d[7] = 0;
    rotMatr2_64d[8] = 1;
 
    transVect2_64d[0] = 0;
    transVect2_64d[1] = 0;
    transVect2_64d[2] = 0;
 
    icvGetQuadsTransform(imageSize,
                         camMatr1_64d,
                         rotMatr1_64d,
                         transVect1_64d,
                         camMatr2_64d,
                         rotMatr2_64d,
                         transVect2_64d,
                         warpSize,
                         quad1,
                         quad2,
                         fundMatr_64d,
                         &epipole1_64d,
                         &epipole2_64d);
 
    /* Convert epipoles */
    epipole1->x = (float)(epipole1_64d.x);
    epipole1->y = (float)(epipole1_64d.y);
    epipole1->z = (float)(epipole1_64d.z);
 
    epipole2->x = (float)(epipole2_64d.x);
    epipole2->y = (float)(epipole2_64d.y);
    epipole2->z = (float)(epipole2_64d.z);
 
    /* Convert fundamental matrix */
    icvCvt_64d_32f(fundMatr_64d, fundMatr, 9);
 
    return;
}
 
/*---------------------------------------------------------------------------------------*/
void
icvGetQuadsTransformStruct(CvStereoCamera* stereoCamera)
{
    /* Wrapper for icvGetQuadsTransformNew */
 
 
    double quad1[4][2];
    double quad2[4][2];
 
    icvGetQuadsTransformNew(cvSize(cvRound(stereoCamera->camera[0]->imgSize[0]),
                                   cvRound(stereoCamera->camera[0]->imgSize[1])),
                            stereoCamera->camera[0]->matrix,
                            stereoCamera->camera[1]->matrix,
                            stereoCamera->rotMatrix,
                            stereoCamera->transVector,
                            &(stereoCamera->warpSize),
                            quad1,
                            quad2,
                            stereoCamera->fundMatr,
                            &(stereoCamera->epipole[0]),
                            &(stereoCamera->epipole[1]));
 
    int i;
    for (i = 0; i < 4; i++)
    {
        stereoCamera->quad[0][i] = cvPoint2D32f(quad1[i][0], quad1[i][1]);
        stereoCamera->quad[1][i] = cvPoint2D32f(quad2[i][0], quad2[i][1]);
    }
 
    return;
}
 
int icvComputeRestStereoParams(CvStereoCamera* stereoparams);
 
/*---------------------------------------------------------------------------------------*/
void
icvComputeStereoParamsForCameras(CvStereoCamera* stereoCamera)
{
    /* For given intrinsic and extrinsic parameters computes rest parameters
    **   such as fundamental matrix. warping coeffs, epipoles, ...
    */
 
 
    /* compute rotate matrix and translate vector */
    double rotMatr1[9];
    double rotMatr2[9];
 
    double transVect1[3];
    double transVect2[3];
 
    double convRotMatr[9];
    double convTransVect[3];
 
    /* fill matrices */
    icvCvt_32f_64d(stereoCamera->camera[0]->rotMatr, rotMatr1, 9);
    icvCvt_32f_64d(stereoCamera->camera[1]->rotMatr, rotMatr2, 9);
 
    icvCvt_32f_64d(stereoCamera->camera[0]->transVect, transVect1, 3);
    icvCvt_32f_64d(stereoCamera->camera[1]->transVect, transVect2, 3);
 
    icvCreateConvertMatrVect(
        rotMatr1, transVect1, rotMatr2, transVect2, convRotMatr, convTransVect);
 
    /* copy to stereo camera params */
    icvCvt_64d_32f(convRotMatr, stereoCamera->rotMatrix, 9);
    icvCvt_64d_32f(convTransVect, stereoCamera->transVector, 3);
 
 
    icvGetQuadsTransformStruct(stereoCamera);
    icvComputeRestStereoParams(stereoCamera);
}
 
void icvGetMiddleAnglePoint(CvPoint2D64d basePoint,
                            CvPoint2D64d point1,
                            CvPoint2D64d point2,
                            CvPoint2D64d* midPoint);
 
void icvProjectPointToDirect(CvPoint2D64d point, CvVect64d lineCoeff, CvPoint2D64d* projectPoint);
 
/* Get normal direct to direct in line */
void
icvGetNormalDirect(CvVect64d direct, CvPoint2D64d point, CvVect64d normDirect)
{
    normDirect[0] = direct[1];
    normDirect[1] = -direct[0];
    normDirect[2] = -(normDirect[0] * point.x + normDirect[1] * point.y);
    return;
}
 
/*---------------------------------------------------------------------------------------*/
 
/* Get cut line for one image */
void
icvGetCutPiece(CvVect64d areaLineCoef1,
               CvVect64d areaLineCoef2,
               CvPoint2D64d epipole,
               CvSize imageSize,
               CvPoint2D64d* point11,
               CvPoint2D64d* point12,
               CvPoint2D64d* point21,
               CvPoint2D64d* point22,
               int* result)
{
    /* Compute nearest cut line to epipole */
    /* Get corners inside sector */
    /* Collect all candidate point */
 
    CvPoint2D64d candPoints[8];
    CvPoint2D64d midPoint = {0, 0};
    int numPoints = 0;
    int res;
    int i;
 
    double cutLine1[3];
    double cutLine2[3];
 
    /* Find middle line of sector */
    double midLine[3] = {0, 0, 0};
 
 
    /* Different way  */
    CvPoint2D64d pointOnLine1;
    pointOnLine1.x = pointOnLine1.y = 0;
    CvPoint2D64d pointOnLine2;
    pointOnLine2.x = pointOnLine2.y = 0;
 
    CvPoint2D64d start1, end1;
 
    icvGetCrossRectDirect(
        imageSize, areaLineCoef1[0], areaLineCoef1[1], areaLineCoef1[2], &start1, &end1, &res);
    if (res > 0)
    {
        pointOnLine1 = start1;
    }
 
    icvGetCrossRectDirect(
        imageSize, areaLineCoef2[0], areaLineCoef2[1], areaLineCoef2[2], &start1, &end1, &res);
    if (res > 0)
    {
        pointOnLine2 = start1;
    }
 
    icvGetMiddleAnglePoint(epipole, pointOnLine1, pointOnLine2, &midPoint);
 
    icvGetCoefForPiece(epipole, midPoint, &midLine[0], &midLine[1], &midLine[2], &res);
 
    /* Test corner points */
    CvPoint2D64d cornerPoint;
    CvPoint2D64d tmpPoints[2];
 
    cornerPoint.x = 0;
    cornerPoint.y = 0;
    icvTestPoint(cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
    if (res == 1)
    {
        /* Add point */
        candPoints[numPoints] = cornerPoint;
        numPoints++;
    }
 
    cornerPoint.x = imageSize.width;
    cornerPoint.y = 0;
    icvTestPoint(cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
    if (res == 1)
    {
        /* Add point */
        candPoints[numPoints] = cornerPoint;
        numPoints++;
    }
 
    cornerPoint.x = imageSize.width;
    cornerPoint.y = imageSize.height;
    icvTestPoint(cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
    if (res == 1)
    {
        /* Add point */
        candPoints[numPoints] = cornerPoint;
        numPoints++;
    }
 
    cornerPoint.x = 0;
    cornerPoint.y = imageSize.height;
    icvTestPoint(cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
    if (res == 1)
    {
        /* Add point */
        candPoints[numPoints] = cornerPoint;
        numPoints++;
    }
 
    /* Find cross line 1 with image border */
    icvGetCrossRectDirect(imageSize,
                          areaLineCoef1[0],
                          areaLineCoef1[1],
                          areaLineCoef1[2],
                          &tmpPoints[0],
                          &tmpPoints[1],
                          &res);
    for (i = 0; i < res; i++)
    {
        candPoints[numPoints++] = tmpPoints[i];
    }
 
    /* Find cross line 2 with image border */
    icvGetCrossRectDirect(imageSize,
                          areaLineCoef2[0],
                          areaLineCoef2[1],
                          areaLineCoef2[2],
                          &tmpPoints[0],
                          &tmpPoints[1],
                          &res);
 
    for (i = 0; i < res; i++)
    {
        candPoints[numPoints++] = tmpPoints[i];
    }
 
    if (numPoints < 2)
    {
        *result = 0;
        return; /* Error. Not enough points */
    }
    /* Project all points to middle line and get max and min */
 
    CvPoint2D64d projPoint;
    CvPoint2D64d minPoint;
    minPoint.x = minPoint.y = FLT_MAX;
    CvPoint2D64d maxPoint;
    maxPoint.x = maxPoint.y = -FLT_MAX;
 
 
    double dist;
    double maxDist = 0;
    double minDist = 10000000;
 
 
    for (i = 0; i < numPoints; i++)
    {
        icvProjectPointToDirect(candPoints[i], midLine, &projPoint);
        icvGetPieceLength(epipole, projPoint, &dist);
        if (dist < minDist)
        {
            minDist = dist;
            minPoint = projPoint;
        }
 
        if (dist > maxDist)
        {
            maxDist = dist;
            maxPoint = projPoint;
        }
    }
 
    /* We know maximum and minimum points. Now we can compute cut lines */
 
    icvGetNormalDirect(midLine, minPoint, cutLine1);
    icvGetNormalDirect(midLine, maxPoint, cutLine2);
 
    /* Test for begin of line. */
    CvPoint2D64d tmpPoint2;
 
    /* Get cross with */
    icvGetCrossDirectDirect(areaLineCoef1, cutLine1, point11, &res);
    icvGetCrossDirectDirect(areaLineCoef2, cutLine1, point12, &res);
 
    icvGetCrossDirectDirect(areaLineCoef1, cutLine2, point21, &res);
    icvGetCrossDirectDirect(areaLineCoef2, cutLine2, point22, &res);
 
    if (epipole.x > imageSize.width * 0.5)
    {
        /* Need to change points */
        tmpPoint2 = *point11;
        *point11 = *point21;
        *point21 = tmpPoint2;
 
        tmpPoint2 = *point12;
        *point12 = *point22;
        *point22 = tmpPoint2;
    }
 
    return;
}
 
/*---------------------------------------------------------------------------------------*/
/* Get middle angle */
void
icvGetMiddleAnglePoint(CvPoint2D64d basePoint,
                       CvPoint2D64d point1,
                       CvPoint2D64d point2,
                       CvPoint2D64d* midPoint)
{
    /* !!! May be need to return error */
 
    double dist1;
    double dist2;
    icvGetPieceLength(basePoint, point1, &dist1);
    icvGetPieceLength(basePoint, point2, &dist2);
    CvPoint2D64d pointNew1;
    CvPoint2D64d pointNew2;
    double alpha = dist2 / dist1;
 
    pointNew1.x = basePoint.x + (1.0 / alpha) * (point2.x - basePoint.x);
    pointNew1.y = basePoint.y + (1.0 / alpha) * (point2.y - basePoint.y);
 
    pointNew2.x = basePoint.x + alpha * (point1.x - basePoint.x);
    pointNew2.y = basePoint.y + alpha * (point1.y - basePoint.y);
 
    int res;
    icvGetCrossPiecePiece(point1, point2, pointNew1, pointNew2, midPoint, &res);
 
    return;
}
 
/*---------------------------------------------------------------------------------------*/
 
 
/*---------------------------------------------------------------------------------------*/
CV_IMPL double
icvGetVect(CvPoint2D64d basePoint, CvPoint2D64d point1, CvPoint2D64d point2)
{
    return (point1.x - basePoint.x) * (point2.y - basePoint.y) -
           (point2.x - basePoint.x) * (point1.y - basePoint.y);
}
 
/*---------------------------------------------------------------------------------------*/
/* Test for point in sector           */
/* Return 0 - point not inside sector */
/* Return 1 - point inside sector     */
void
icvTestPoint(CvPoint2D64d testPoint,
             CvVect64d line1,
             CvVect64d line2,
             CvPoint2D64d basePoint,
             int* result)
{
    CvPoint2D64d point1, point2;
 
    icvProjectPointToDirect(testPoint, line1, &point1);
    icvProjectPointToDirect(testPoint, line2, &point2);
 
    double sign1 = icvGetVect(basePoint, point1, point2);
    double sign2 = icvGetVect(basePoint, point1, testPoint);
    if (sign1 * sign2 > 0)
    {
        /* Correct for first line */
        sign1 = -sign1;
        sign2 = icvGetVect(basePoint, point2, testPoint);
        if (sign1 * sign2 > 0)
        {
            /* Correct for both lines */
            *result = 1;
        }
        else
        {
            *result = 0;
        }
    }
    else
    {
        *result = 0;
    }
 
    return;
}
 
/*---------------------------------------------------------------------------------------*/
/* Project point to line */
void
icvProjectPointToDirect(CvPoint2D64d point, CvVect64d lineCoeff, CvPoint2D64d* projectPoint)
{
    double a = lineCoeff[0];
    double b = lineCoeff[1];
 
    double det = 1.0 / (a * a + b * b);
    double delta = a * point.y - b * point.x;
 
    projectPoint->x = (-a * lineCoeff[2] - b * delta) * det;
    projectPoint->y = (-b * lineCoeff[2] + a * delta) * det;
 
    return;
}
 
/*---------------------------------------------------------------------------------------*/
/* Get distance from point to direction */
void
icvGetDistanceFromPointToDirect(CvPoint2D64d point, CvVect64d lineCoef, double* dist)
{
    CvPoint2D64d tmpPoint;
    icvProjectPointToDirect(point, lineCoef, &tmpPoint);
    double dx = point.x - tmpPoint.x;
    double dy = point.y - tmpPoint.y;
    *dist = sqrt(dx * dx + dy * dy);
    return;
}
 
/*---------------------------------------------------------------------------------------*/
 
CV_IMPL IplImage*
icvCreateIsometricImage(IplImage* src, IplImage* dst, int desired_depth, int desired_num_channels)
{
    CvSize src_size;
    src_size.width = src->width;
    src_size.height = src->height;
 
    CvSize dst_size = src_size;
 
    if (dst)
    {
        dst_size.width = dst->width;
        dst_size.height = dst->height;
    }
 
    if (!dst || dst->depth != desired_depth || dst->nChannels != desired_num_channels ||
        dst_size.width != src_size.width || dst_size.height != src_size.height)
    {
        cvReleaseImage(&dst);
        dst = cvCreateImage(src_size, desired_depth, desired_num_channels);
        CvRect rect = cvRect(0, 0, src_size.width, src_size.height);
        cvSetImageROI(dst, rect);
    }
 
    return dst;
}
 
static int
icvCvt_32f_64d(float* src, double* dst, int size)
{
    int t;
 
    if (!src || !dst)
    {
        return CV_NULLPTR_ERR;
    }
    if (size <= 0)
    {
        return CV_BADRANGE_ERR;
    }
 
    for (t = 0; t < size; t++)
    {
        dst[t] = (double)(src[t]);
    }
 
    return CV_OK;
}
 
/*======================================================================================*/
/* Type conversion double -> float */
static int
icvCvt_64d_32f(double* src, float* dst, int size)
{
    int t;
 
    if (!src || !dst)
    {
        return CV_NULLPTR_ERR;
    }
    if (size <= 0)
    {
        return CV_BADRANGE_ERR;
    }
 
    for (t = 0; t < size; t++)
    {
        dst[t] = (float)(src[t]);
    }
 
    return CV_OK;
}
 
CV_INLINE CvSize
cvGetMatSize(const CvMat* mat)
{
    CvSize size;
    size.width = mat->cols;
    size.height = mat->rows;
    return size;
}
 
/*----------------------------------------------------------------------------------*/
 
#define __BEGIN__ __CV_BEGIN__
#define __END__ __CV_END__
 
/*--------------------------------------------------------------------------------------*/
 
CV_IMPL void
cvComputePerspectiveMap(const double c[3][3], CvArr* rectMapX, CvArr* rectMapY)
{
    CV_FUNCNAME("cvComputePerspectiveMap");
 
    __BEGIN__;
 
    CvSize size;
    CvMat stubx, *mapx = (CvMat*)rectMapX;
    CvMat stuby, *mapy = (CvMat*)rectMapY;
    int i, j;
 
    CV_CALL(mapx = cvGetMat(mapx, &stubx));
    CV_CALL(mapy = cvGetMat(mapy, &stuby));
 
    if (CV_MAT_TYPE(mapx->type) != CV_32FC1 || CV_MAT_TYPE(mapy->type) != CV_32FC1)
    {
        CV_ERROR(CV_StsUnsupportedFormat, "");
    }
 
    size = cvGetMatSize(mapx);
    assert(fabs(c[2][2] - 1.) < FLT_EPSILON);
 
    for (i = 0; i < size.height; i++)
    {
        float* mx = (float*)(mapx->data.ptr + mapx->step * i);
        float* my = (float*)(mapy->data.ptr + mapy->step * i);
 
        for (j = 0; j < size.width; j++)
        {
            double w = 1. / (c[2][0] * j + c[2][1] * i + 1.);
            double x = (c[0][0] * j + c[0][1] * i + c[0][2]) * w;
            double y = (c[1][0] * j + c[1][1] * i + c[1][2]) * w;
 
            mx[j] = (float)x;
            my[j] = (float)y;
        }
    }
 
    __END__;
}
 
/*--------------------------------------------------------------------------------------*/
 
CV_IMPL void
cvInitPerspectiveTransform(CvSize size,
                           const CvPoint2D32f quad[4],
                           double matrix[3][3],
                           CvArr* rectMap)
{
    /* Computes Perspective Transform coeffs and map if need
        for given image size and given result quad */
    CV_FUNCNAME("cvInitPerspectiveTransform");
 
    __BEGIN__;
 
    double A[64];
    double b[8];
    double c[8];
    CvPoint2D32f pt[4];
    CvMat mapstub, *map = (CvMat*)rectMap;
    int i, j;
 
    if (map)
    {
        CV_CALL(map = cvGetMat(map, &mapstub));
 
        if (CV_MAT_TYPE(map->type) != CV_32FC2)
        {
            CV_ERROR(CV_StsUnsupportedFormat, "");
        }
 
        if (map->width != size.width || map->height != size.height)
        {
            CV_ERROR(CV_StsUnmatchedSizes, "");
        }
    }
 
    pt[0] = cvPoint2D32f(0, 0);
    pt[1] = cvPoint2D32f(size.width, 0);
    pt[2] = cvPoint2D32f(size.width, size.height);
    pt[3] = cvPoint2D32f(0, size.height);
 
    for (i = 0; i < 4; i++)
    {
#if 0
        double x = quad[i].x;
        double y = quad[i].y;
        double X = pt[i].x;
        double Y = pt[i].y;
#else
        double x = pt[i].x;
        double y = pt[i].y;
        double X = quad[i].x;
        double Y = quad[i].y;
#endif
        double* a = A + i * 16;
 
        a[0] = x;
        a[1] = y;
        a[2] = 1;
        a[3] = 0;
        a[4] = 0;
        a[5] = 0;
        a[6] = -X * x;
        a[7] = -X * y;
 
        a += 8;
 
        a[0] = 0;
        a[1] = 0;
        a[2] = 0;
        a[3] = x;
        a[4] = y;
        a[5] = 1;
        a[6] = -Y * x;
        a[7] = -Y * y;
 
        b[i * 2] = X;
        b[i * 2 + 1] = Y;
    }
 
    {
        double invA[64];
        CvMat matA = cvMat(8, 8, CV_64F, A);
        CvMat matInvA = cvMat(8, 8, CV_64F, invA);
        CvMat matB = cvMat(8, 1, CV_64F, b);
        CvMat matX = cvMat(8, 1, CV_64F, c);
 
        CV_CALL(cvPseudoInverse(&matA, &matInvA));
        CV_CALL(cvMatMulAdd(&matInvA, &matB, 0, &matX));
    }
 
    matrix[0][0] = c[0];
    matrix[0][1] = c[1];
    matrix[0][2] = c[2];
    matrix[1][0] = c[3];
    matrix[1][1] = c[4];
    matrix[1][2] = c[5];
    matrix[2][0] = c[6];
    matrix[2][1] = c[7];
    matrix[2][2] = 1.0;
 
    if (map)
    {
        for (i = 0; i < size.height; i++)
        {
            CvPoint2D32f* maprow = (CvPoint2D32f*)(map->data.ptr + map->step * i);
            for (j = 0; j < size.width; j++)
            {
                double w = 1. / (c[6] * j + c[7] * i + 1.);
                double x = (c[0] * j + c[1] * i + c[2]) * w;
                double y = (c[3] * j + c[4] * i + c[5]) * w;
 
                maprow[j].x = (float)x;
                maprow[j].y = (float)y;
            }
        }
    }
 
    __END__;
 
    return;
}
 
/*-----------------------------------------------------------------------*/
/* Compute projected infinite point for second image if first image point is known */
void
icvComputeeInfiniteProject1(CvMatr64d rotMatr,
                            CvMatr64d camMatr1,
                            CvMatr64d camMatr2,
                            CvPoint2D32f point1,
                            CvPoint2D32f* point2)
{
    double invMatr1[9];
    icvInvertMatrix_64d(camMatr1, 3, invMatr1);
    double P1[3];
    double p1[3];
    p1[0] = (double)(point1.x);
    p1[1] = (double)(point1.y);
    p1[2] = 1;
 
    icvMulMatrix_64d(invMatr1, 3, 3, p1, 1, 3, P1);
 
    double invR[9];
    icvTransposeMatrix_64d(rotMatr, 3, 3, invR);
 
    /* Change system 1 to system 2 */
    double P2[3];
    icvMulMatrix_64d(invR, 3, 3, P1, 1, 3, P2);
 
    /* Now we can project this point to image 2 */
    double projP[3];
 
    icvMulMatrix_64d(camMatr2, 3, 3, P2, 1, 3, projP);
 
    point2->x = (float)(projP[0] / projP[2]);
    point2->y = (float)(projP[1] / projP[2]);
 
    return;
}
 
/*-----------------------------------------------------------------------*/
/* Compute projected infinite point for second image if first image point is known */
void
icvComputeeInfiniteProject2(CvMatr64d rotMatr,
                            CvMatr64d camMatr1,
                            CvMatr64d camMatr2,
                            CvPoint2D32f* point1,
                            CvPoint2D32f point2)
{
    double invMatr2[9];
    icvInvertMatrix_64d(camMatr2, 3, invMatr2);
    double P2[3];
    double p2[3];
    p2[0] = (double)(point2.x);
    p2[1] = (double)(point2.y);
    p2[2] = 1;
 
    icvMulMatrix_64d(invMatr2, 3, 3, p2, 1, 3, P2);
 
    /* Change system 1 to system 2 */
 
    double P1[3];
    icvMulMatrix_64d(rotMatr, 3, 3, P2, 1, 3, P1);
 
    /* Now we can project this point to image 2 */
    double projP[3];
 
    icvMulMatrix_64d(camMatr1, 3, 3, P1, 1, 3, projP);
 
    point1->x = (float)(projP[0] / projP[2]);
    point1->y = (float)(projP[1] / projP[2]);
 
    return;
}
 
/* Simpler version of the previous function */
void
cvProjectPointsSimple(int point_count,
                      CvPoint3D64f* _object_points,
                      double* _rotation_matrix,
                      double* _translation_vector,
                      double* _camera_matrix,
                      double* _distortion,
                      CvPoint2D64f* _image_points)
{
    CvMat object_points = cvMat(point_count, 1, CV_64FC3, _object_points);
    CvMat image_points = cvMat(point_count, 1, CV_64FC2, _image_points);
    CvMat rotation_matrix = cvMat(3, 3, CV_64FC1, _rotation_matrix);
    CvMat translation_vector = cvMat(3, 1, CV_64FC1, _translation_vector);
    CvMat camera_matrix = cvMat(3, 3, CV_64FC1, _camera_matrix);
    CvMat dist_coeffs = cvMat(4, 1, CV_64FC1, _distortion);
 
    cvProjectPoints2(&object_points,
                     &rotation_matrix,
                     &translation_vector,
                     &camera_matrix,
                     &dist_coeffs,
                     &image_points,
                     0,
                     0,
                     0,
                     0,
                     0,
                     0);
}
 
/* Select best R and t for given cameras, points, ... */
/* For both cameras */
static int
icvSelectBestRt(int numImages,
                int* numPoints,
                CvPoint2D32f* imagePoints1,
                CvPoint2D32f* imagePoints2,
                CvPoint3D32f* objectPoints,
 
                CvMatr32f cameraMatrix1,
                CvVect32f distortion1,
                CvMatr32f rotMatrs1,
                CvVect32f transVects1,
 
                CvMatr32f cameraMatrix2,
                CvVect32f distortion2,
                CvMatr32f rotMatrs2,
                CvVect32f transVects2,
 
                CvMatr32f bestRotMatr,
                CvVect32f bestTransVect)
{
 
    /* Need to convert input data 32 -> 64 */
    CvPoint3D64d* objectPoints_64d;
 
    double* rotMatrs1_64d;
    double* rotMatrs2_64d;
 
    double* transVects1_64d;
    double* transVects2_64d;
 
    double cameraMatrix1_64d[9];
    double cameraMatrix2_64d[9];
 
    double distortion1_64d[4];
    double distortion2_64d[4];
 
    /* allocate memory for 64d data */
    int totalNum = 0;
 
    for (int i = 0; i < numImages; i++)
    {
        totalNum += numPoints[i];
    }
 
    objectPoints_64d = (CvPoint3D64d*)calloc(totalNum, sizeof(CvPoint3D64d));
 
    rotMatrs1_64d = (double*)calloc(numImages, sizeof(double) * 9);
    rotMatrs2_64d = (double*)calloc(numImages, sizeof(double) * 9);
 
    transVects1_64d = (double*)calloc(numImages, sizeof(double) * 3);
    transVects2_64d = (double*)calloc(numImages, sizeof(double) * 3);
 
    /* Convert input data to 64d */
 
    icvCvt_32f_64d((float*)objectPoints, (double*)objectPoints_64d, totalNum * 3);
 
    icvCvt_32f_64d(rotMatrs1, rotMatrs1_64d, numImages * 9);
    icvCvt_32f_64d(rotMatrs2, rotMatrs2_64d, numImages * 9);
 
    icvCvt_32f_64d(transVects1, transVects1_64d, numImages * 3);
    icvCvt_32f_64d(transVects2, transVects2_64d, numImages * 3);
 
    /* Convert to arrays */
    icvCvt_32f_64d(cameraMatrix1, cameraMatrix1_64d, 9);
    icvCvt_32f_64d(cameraMatrix2, cameraMatrix2_64d, 9);
 
    icvCvt_32f_64d(distortion1, distortion1_64d, 4);
    icvCvt_32f_64d(distortion2, distortion2_64d, 4);
 
 
    /* for each R and t compute error for image pair */
    float* errors;
 
    errors = (float*)calloc(numImages * numImages, sizeof(float));
    if (errors == 0)
    {
        return CV_OUTOFMEM_ERR;
    }
 
    int currImagePair;
    int currRt;
    for (currRt = 0; currRt < numImages; currRt++)
    {
        int begPoint = 0;
        for (currImagePair = 0; currImagePair < numImages; currImagePair++)
        {
            /* For current R,t R,t compute relative position of cameras */
 
            double convRotMatr[9];
            double convTransVect[3];
 
            icvCreateConvertMatrVect(rotMatrs1_64d + currRt * 9,
                                     transVects1_64d + currRt * 3,
                                     rotMatrs2_64d + currRt * 9,
                                     transVects2_64d + currRt * 3,
                                     convRotMatr,
                                     convTransVect);
 
            /* Project points using relative position of cameras */
 
            double convRotMatr2[9];
            double convTransVect2[3];
 
            convRotMatr2[0] = 1;
            convRotMatr2[1] = 0;
            convRotMatr2[2] = 0;
 
            convRotMatr2[3] = 0;
            convRotMatr2[4] = 1;
            convRotMatr2[5] = 0;
 
            convRotMatr2[6] = 0;
            convRotMatr2[7] = 0;
            convRotMatr2[8] = 1;
 
            convTransVect2[0] = 0;
            convTransVect2[1] = 0;
            convTransVect2[2] = 0;
 
            /* Compute error for given pair and Rt */
            /* We must project points to image and compute error */
 
            CvPoint2D64d* projImagePoints1;
            CvPoint2D64d* projImagePoints2;
 
            CvPoint3D64d* points1;
            CvPoint3D64d* points2;
 
            int numberPnt;
            numberPnt = numPoints[currImagePair];
            projImagePoints1 = (CvPoint2D64d*)calloc(numberPnt, sizeof(CvPoint2D64d));
            projImagePoints2 = (CvPoint2D64d*)calloc(numberPnt, sizeof(CvPoint2D64d));
 
            points1 = (CvPoint3D64d*)calloc(numberPnt, sizeof(CvPoint3D64d));
            points2 = (CvPoint3D64d*)calloc(numberPnt, sizeof(CvPoint3D64d));
 
            /* Transform object points to first camera position */
            for (int i = 0; i < numberPnt; i++)
            {
                /* Create second camera point */
                CvPoint3D64d tmpPoint;
                tmpPoint.x = (double)(objectPoints[i].x);
                tmpPoint.y = (double)(objectPoints[i].y);
                tmpPoint.z = (double)(objectPoints[i].z);
 
                icvConvertPointSystem(tmpPoint,
                                      points2 + i,
                                      rotMatrs2_64d + currImagePair * 9,
                                      transVects2_64d + currImagePair * 3);
 
                /* Create first camera point using R, t */
                icvConvertPointSystem(points2[i], points1 + i, convRotMatr, convTransVect);
 
                CvPoint3D64d tmpPoint2 = {0, 0, 0};
                icvConvertPointSystem(tmpPoint,
                                      &tmpPoint2,
                                      rotMatrs1_64d + currImagePair * 9,
                                      transVects1_64d + currImagePair * 3);
            }
 
            /* Project with no translate and no rotation */
 
            cvProjectPointsSimple(numPoints[currImagePair],
                                  points1,
                                  convRotMatr2,
                                  convTransVect2,
                                  cameraMatrix1_64d,
                                  distortion1_64d,
                                  projImagePoints1);
 
            cvProjectPointsSimple(numPoints[currImagePair],
                                  points2,
                                  convRotMatr2,
                                  convTransVect2,
                                  cameraMatrix2_64d,
                                  distortion2_64d,
                                  projImagePoints2);
 
            /* points are projected. Compute error */
 
            int currPoint;
            double err1 = 0;
            double err2 = 0;
            double err;
            for (currPoint = 0; currPoint < numberPnt; currPoint++)
            {
                double len1, len2;
                double dx1, dy1;
                dx1 = imagePoints1[begPoint + currPoint].x - projImagePoints1[currPoint].x;
                dy1 = imagePoints1[begPoint + currPoint].y - projImagePoints1[currPoint].y;
                len1 = sqrt(dx1 * dx1 + dy1 * dy1);
                err1 += len1;
 
                double dx2, dy2;
                dx2 = imagePoints2[begPoint + currPoint].x - projImagePoints2[currPoint].x;
                dy2 = imagePoints2[begPoint + currPoint].y - projImagePoints2[currPoint].y;
                len2 = sqrt(dx2 * dx2 + dy2 * dy2);
                err2 += len2;
            }
 
            err1 /= (float)(numberPnt);
            err2 /= (float)(numberPnt);
 
            err = (err1 + err2) * 0.5;
            begPoint += numberPnt;
 
            /* Set this error to */
            errors[numImages * currImagePair + currRt] = (float)err;
 
            free(points1);
            free(points2);
            free(projImagePoints1);
            free(projImagePoints2);
        }
    }
 
    /* Just select R and t with minimal average error */
 
    int bestnumRt = 0;
    float minError = 0; /* Just for no warnings. Uses 'first' flag. */
    int first = 1;
    for (currRt = 0; currRt < numImages; currRt++)
    {
        float avErr = 0;
        for (currImagePair = 0; currImagePair < numImages; currImagePair++)
        {
            avErr += errors[numImages * currImagePair + currRt];
        }
        avErr /= (float)(numImages);
 
        if (first)
        {
            bestnumRt = 0;
            minError = avErr;
            first = 0;
        }
        else
        {
            if (avErr < minError)
            {
                bestnumRt = currRt;
                minError = avErr;
            }
        }
    }
 
    double bestRotMatr_64d[9];
    double bestTransVect_64d[3];
 
    icvCreateConvertMatrVect(rotMatrs1_64d + bestnumRt * 9,
                             transVects1_64d + bestnumRt * 3,
                             rotMatrs2_64d + bestnumRt * 9,
                             transVects2_64d + bestnumRt * 3,
                             bestRotMatr_64d,
                             bestTransVect_64d);
 
    icvCvt_64d_32f(bestRotMatr_64d, bestRotMatr, 9);
    icvCvt_64d_32f(bestTransVect_64d, bestTransVect, 3);
 
 
    free(errors);
 
    return CV_OK;
}
 
/* ----------------- Stereo calibration functions --------------------- */
 
float
icvDefinePointPosition(CvPoint2D32f point1, CvPoint2D32f point2, CvPoint2D32f point)
{
    float ax = point2.x - point1.x;
    float ay = point2.y - point1.y;
 
    float bx = point.x - point1.x;
    float by = point.y - point1.y;
 
    return (ax * by - ay * bx);
}
 
/* Convert function for stereo warping */
int
icvConvertWarpCoordinates(double coeffs[3][3],
                          CvPoint2D32f* cameraPoint,
                          CvPoint2D32f* warpPoint,
                          int direction)
{
    double x, y;
    double det;
    if (direction == CV_WARP_TO_CAMERA)
    {
        /* convert from camera image to warped image coordinates */
        x = warpPoint->x;
        y = warpPoint->y;
 
        det = (coeffs[2][0] * x + coeffs[2][1] * y + coeffs[2][2]);
        if (fabs(det) > 1e-8)
        {
            cameraPoint->x = (float)((coeffs[0][0] * x + coeffs[0][1] * y + coeffs[0][2]) / det);
            cameraPoint->y = (float)((coeffs[1][0] * x + coeffs[1][1] * y + coeffs[1][2]) / det);
            return CV_OK;
        }
    }
    else if (direction == CV_CAMERA_TO_WARP)
    {
        /* convert from warped image to camera image coordinates */
        x = cameraPoint->x;
        y = cameraPoint->y;
 
        det = (coeffs[2][0] * x - coeffs[0][0]) * (coeffs[2][1] * y - coeffs[1][1]) -
              (coeffs[2][1] * x - coeffs[0][1]) * (coeffs[2][0] * y - coeffs[1][0]);
 
        if (fabs(det) > 1e-8)
        {
            warpPoint->x =
                (float)(((coeffs[0][2] - coeffs[2][2] * x) * (coeffs[2][1] * y - coeffs[1][1]) -
                         (coeffs[2][1] * x - coeffs[0][1]) * (coeffs[1][2] - coeffs[2][2] * y)) /
                        det);
            warpPoint->y =
                (float)(((coeffs[2][0] * x - coeffs[0][0]) * (coeffs[1][2] - coeffs[2][2] * y) -
                         (coeffs[0][2] - coeffs[2][2] * x) * (coeffs[2][0] * y - coeffs[1][0])) /
                        det);
            return CV_OK;
        }
    }
 
    return CV_BADFACTOR_ERR;
}
 
/* Compute stereo params using some camera params */
/* by Valery Mosyagin. int ComputeRestStereoParams(StereoParams *stereoparams) */
int
icvComputeRestStereoParams(CvStereoCamera* stereoparams)
{
 
 
    icvGetQuadsTransformStruct(stereoparams);
 
    cvInitPerspectiveTransform(
        stereoparams->warpSize, stereoparams->quad[0], stereoparams->coeffs[0], 0);
 
    cvInitPerspectiveTransform(
        stereoparams->warpSize, stereoparams->quad[1], stereoparams->coeffs[1], 0);
 
    /* Create border for warped images */
    CvPoint2D32f corns[4];
    corns[0].x = 0;
    corns[0].y = 0;
 
    corns[1].x = (float)(stereoparams->camera[0]->imgSize[0] - 1);
    corns[1].y = 0;
 
    corns[2].x = (float)(stereoparams->camera[0]->imgSize[0] - 1);
    corns[2].y = (float)(stereoparams->camera[0]->imgSize[1] - 1);
 
    corns[3].x = 0;
    corns[3].y = (float)(stereoparams->camera[0]->imgSize[1] - 1);
 
    for (int i = 0; i < 4; i++)
    {
        /* For first camera */
        icvConvertWarpCoordinates(
            stereoparams->coeffs[0], corns + i, stereoparams->border[0] + i, CV_CAMERA_TO_WARP);
 
        /* For second camera */
        icvConvertWarpCoordinates(
            stereoparams->coeffs[1], corns + i, stereoparams->border[1] + i, CV_CAMERA_TO_WARP);
    }
 
    /* Test compute  */
    {
        CvPoint2D32f warpPoints[4];
        warpPoints[0] = cvPoint2D32f(0, 0);
        warpPoints[1] = cvPoint2D32f(stereoparams->warpSize.width - 1, 0);
        warpPoints[2] =
            cvPoint2D32f(stereoparams->warpSize.width - 1, stereoparams->warpSize.height - 1);
        warpPoints[3] = cvPoint2D32f(0, stereoparams->warpSize.height - 1);
 
        CvPoint2D32f camPoints1[4];
        CvPoint2D32f camPoints2[4];
 
        for (int i = 0; i < 4; i++)
        {
            icvConvertWarpCoordinates(
                stereoparams->coeffs[0], camPoints1 + i, warpPoints + i, CV_WARP_TO_CAMERA);
 
            icvConvertWarpCoordinates(
                stereoparams->coeffs[1], camPoints2 + i, warpPoints + i, CV_WARP_TO_CAMERA);
        }
    }
 
 
    /* Allocate memory for scanlines coeffs */
 
    stereoparams->lineCoeffs =
        (CvStereoLineCoeff*)calloc(stereoparams->warpSize.height, sizeof(CvStereoLineCoeff));
 
    /* Compute coeffs for epilines  */
 
    icvComputeCoeffForStereo(stereoparams);
 
    /* all coeffs are known */
    return CV_OK;
}
 
/*-------------------------------------------------------------------------------------------*/
 
int
icvStereoCalibration(int numImages,
                     int* nums,
                     CvSize imageSize,
                     CvPoint2D32f* imagePoints1,
                     CvPoint2D32f* imagePoints2,
                     CvPoint3D32f* objectPoints,
                     CvStereoCamera* stereoparams)
{
    /* Firstly we must calibrate both cameras */
    /*  Alocate memory for data */
    /* Allocate for translate vectors */
    float* transVects1;
    float* transVects2;
    float* rotMatrs1;
    float* rotMatrs2;
 
    transVects1 = (float*)calloc(numImages, sizeof(float) * 3);
    transVects2 = (float*)calloc(numImages, sizeof(float) * 3);
 
    rotMatrs1 = (float*)calloc(numImages, sizeof(float) * 9);
    rotMatrs2 = (float*)calloc(numImages, sizeof(float) * 9);
 
    /* Calibrate first camera */
    cvCalibrateCamera(numImages,
                      nums,
                      imageSize,
                      imagePoints1,
                      objectPoints,
                      stereoparams->camera[0]->distortion,
                      stereoparams->camera[0]->matrix,
                      transVects1,
                      rotMatrs1,
                      1);
 
    /* Calibrate second camera */
    cvCalibrateCamera(numImages,
                      nums,
                      imageSize,
                      imagePoints2,
                      objectPoints,
                      stereoparams->camera[1]->distortion,
                      stereoparams->camera[1]->matrix,
                      transVects2,
                      rotMatrs2,
                      1);
 
    /* Cameras are calibrated */
 
    stereoparams->camera[0]->imgSize[0] = (float)imageSize.width;
    stereoparams->camera[0]->imgSize[1] = (float)imageSize.height;
 
    stereoparams->camera[1]->imgSize[0] = (float)imageSize.width;
    stereoparams->camera[1]->imgSize[1] = (float)imageSize.height;
 
    icvSelectBestRt(numImages,
                    nums,
                    imagePoints1,
                    imagePoints2,
                    objectPoints,
                    stereoparams->camera[0]->matrix,
                    stereoparams->camera[0]->distortion,
                    rotMatrs1,
                    transVects1,
                    stereoparams->camera[1]->matrix,
                    stereoparams->camera[1]->distortion,
                    rotMatrs2,
                    transVects2,
                    stereoparams->rotMatrix,
                    stereoparams->transVector);
 
    /* Free memory */
    free(transVects1);
    free(transVects2);
    free(rotMatrs1);
    free(rotMatrs2);
 
    icvComputeRestStereoParams(stereoparams);
 
    return CV_NO_ERR;
}
 
int
icvGetCrossPieceVector(CvPoint2D32f p1_start,
                       CvPoint2D32f p1_end,
                       CvPoint2D32f v2_start,
                       CvPoint2D32f v2_end,
                       CvPoint2D32f* cross)
{
    double ex1 = p1_start.x;
    double ey1 = p1_start.y;
    double ex2 = p1_end.x;
    double ey2 = p1_end.y;
 
    double px1 = v2_start.x;
    double py1 = v2_start.y;
    double px2 = v2_end.x;
    double py2 = v2_end.y;
 
    double del = (ex1 - ex2) * (py2 - py1) + (ey2 - ey1) * (px2 - px1);
    if (del == 0)
    {
        return -1;
    }
 
    double delA = (px1 - ex1) * (py1 - py2) + (ey1 - py1) * (px1 - px2);
    //double delB =  (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2);
 
    double alpha = delA / del;
    //double betta = -delB / del;
 
    if (alpha < 0 || alpha > 1.0)
    {
        return -1;
    }
 
    double delX = (ex1 - ex2) * (py1 * (px1 - px2) - px1 * (py1 - py2)) +
                  (px1 - px2) * (ex1 * (ey1 - ey2) - ey1 * (ex1 - ex2));
 
    double delY = (ey1 - ey2) * (px1 * (py1 - py2) - py1 * (px1 - px2)) +
                  (py1 - py2) * (ey1 * (ex1 - ex2) - ex1 * (ey1 - ey2));
 
    cross->x = (float)(delX / del);
    cross->y = (float)(-delY / del);
    return 1;
}
 
int
icvGetCrossLineDirect(CvPoint2D32f p1,
                      CvPoint2D32f p2,
                      float a,
                      float b,
                      float c,
                      CvPoint2D32f* cross)
{
    double del;
    double delX, delY, delA;
 
    double px1, px2, py1, py2;
    double X, Y, alpha;
 
    px1 = p1.x;
    py1 = p1.y;
 
    px2 = p2.x;
    py2 = p2.y;
 
    del = a * (px2 - px1) + b * (py2 - py1);
    if (del == 0)
    {
        return -1;
    }
 
    delA = -c - a * px1 - b * py1;
    alpha = delA / del;
 
    if (alpha < 0 || alpha > 1.0)
    {
        return -1; /* no cross */
    }
 
    delX = b * (py1 * (px1 - px2) - px1 * (py1 - py2)) + c * (px1 - px2);
    delY = a * (px1 * (py1 - py2) - py1 * (px1 - px2)) + c * (py1 - py2);
 
    X = delX / del;
    Y = delY / del;
 
    cross->x = (float)X;
    cross->y = (float)Y;
 
    return 1;
}
 
/* Compute epipoles for fundamental matrix */
int
cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr, CvPoint3D32f* epipole1, CvPoint3D32f* epipole2)
{
    /* Decompose fundamental matrix using SVD ( A = U W V') */
    CvMat fundMatrC = cvMat(3, 3, CV_MAT32F, fundMatr);
 
    CvMat* matrW = cvCreateMat(3, 3, CV_MAT32F);
    CvMat* matrU = cvCreateMat(3, 3, CV_MAT32F);
    CvMat* matrV = cvCreateMat(3, 3, CV_MAT32F);
 
    /* From svd we need just last vector of U and V or last row from U' and V' */
    /* We get transposed matrices U and V */
    cvSVD(&fundMatrC, matrW, matrU, matrV, CV_SVD_V_T | CV_SVD_U_T);
 
    /* Get last row from U' and compute epipole1 */
    epipole1->x = matrU->data.fl[6];
    epipole1->y = matrU->data.fl[7];
    epipole1->z = matrU->data.fl[8];
 
    /* Get last row from V' and compute epipole2 */
    epipole2->x = matrV->data.fl[6];
    epipole2->y = matrV->data.fl[7];
    epipole2->z = matrV->data.fl[8];
 
    cvReleaseMat(&matrW);
    cvReleaseMat(&matrU);
    cvReleaseMat(&matrV);
    return CV_OK;
}
 
int
cvConvertEssential2Fundamental(CvMatr32f essMatr,
                               CvMatr32f fundMatr,
                               CvMatr32f cameraMatr1,
                               CvMatr32f cameraMatr2)
{
    /* Fund = inv(CM1') * Ess * inv(CM2) */
 
    CvMat essMatrC = cvMat(3, 3, CV_MAT32F, essMatr);
    CvMat fundMatrC = cvMat(3, 3, CV_MAT32F, fundMatr);
    CvMat cameraMatr1C = cvMat(3, 3, CV_MAT32F, cameraMatr1);
    CvMat cameraMatr2C = cvMat(3, 3, CV_MAT32F, cameraMatr2);
 
    CvMat* invCM2 = cvCreateMat(3, 3, CV_MAT32F);
    CvMat* tmpMatr = cvCreateMat(3, 3, CV_MAT32F);
    CvMat* invCM1T = cvCreateMat(3, 3, CV_MAT32F);
 
    cvTranspose(&cameraMatr1C, tmpMatr);
    cvInvert(tmpMatr, invCM1T);
    cvmMul(invCM1T, &essMatrC, tmpMatr);
    cvInvert(&cameraMatr2C, invCM2);
    cvmMul(tmpMatr, invCM2, &fundMatrC);
 
    /* Scale fundamental matrix */
    double scale;
    scale = 1.0 / fundMatrC.data.fl[8];
    cvConvertScale(&fundMatrC, &fundMatrC, scale);
 
    cvReleaseMat(&invCM2);
    cvReleaseMat(&tmpMatr);
    cvReleaseMat(&invCM1T);
 
    return CV_OK;
}
 
/* Compute essential matrix */
 
int
cvComputeEssentialMatrix(CvMatr32f rotMatr, CvMatr32f transVect, CvMatr32f essMatr)
{
    float transMatr[9];
 
    /* Make antisymmetric matrix from transpose vector */
    transMatr[0] = 0;
    transMatr[1] = -transVect[2];
    transMatr[2] = transVect[1];
 
    transMatr[3] = transVect[2];
    transMatr[4] = 0;
    transMatr[5] = -transVect[0];
 
    transMatr[6] = -transVect[1];
    transMatr[7] = transVect[0];
    transMatr[8] = 0;
 
    icvMulMatrix_32f(transMatr, 3, 3, rotMatr, 3, 3, essMatr);
 
    return CV_OK;
}
 
// END
 
void
CvCalibFilter::Stop(bool calibrate)
{
    int i, j;
    isCalibrated = false;
 
    // deallocate undistortion maps
    for (i = 0; i < cameraCount; i++)
    {
        cvReleaseMat(&undistMap[i][0]);
        cvReleaseMat(&undistMap[i][1]);
        cvReleaseMat(&rectMap[i][0]);
        cvReleaseMat(&rectMap[i][1]);
    }
 
    if (calibrate && framesAccepted > 0)
    {
        int n = framesAccepted;
        CvPoint3D32f* buffer = (CvPoint3D32f*)cvAlloc(n * etalonPointCount * sizeof(buffer[0]));
        CvMat mat;
        float* rotMatr = (float*)cvAlloc(n * 9 * sizeof(rotMatr[0]));
        float* transVect = (float*)cvAlloc(n * 3 * sizeof(transVect[0]));
        int* counts = (int*)cvAlloc(n * sizeof(counts[0]));
 
        cvInitMatHeader(&mat, 1, sizeof(CvCamera) / sizeof(float), CV_32FC1, 0);
        memset(cameraParams, 0, cameraCount * sizeof(cameraParams[0]));
 
        for (i = 0; i < framesAccepted; i++)
        {
            counts[i] = etalonPointCount;
            for (j = 0; j < etalonPointCount; j++)
                buffer[i * etalonPointCount + j] =
                    cvPoint3D32f(etalonPoints[j].x, etalonPoints[j].y, 0);
        }
 
        for (i = 0; i < cameraCount; i++)
        {
            cvCalibrateCamera(framesAccepted,
                              counts,
                              imgSize,
                              points[i],
                              buffer,
                              cameraParams[i].distortion,
                              cameraParams[i].matrix,
                              transVect,
                              rotMatr,
                              0);
 
            cameraParams[i].imgSize[0] = (float)imgSize.width;
            cameraParams[i].imgSize[1] = (float)imgSize.height;
 
            //            cameraParams[i].focalLength[0] = cameraParams[i].matrix[0];
            //            cameraParams[i].focalLength[1] = cameraParams[i].matrix[4];
 
            //            cameraParams[i].principalPoint[0] = cameraParams[i].matrix[2];
            //            cameraParams[i].principalPoint[1] = cameraParams[i].matrix[5];
 
            memcpy(cameraParams[i].rotMatr, rotMatr, 9 * sizeof(rotMatr[0]));
            memcpy(cameraParams[i].transVect, transVect, 3 * sizeof(transVect[0]));
 
            mat.data.ptr = (uchar*)(cameraParams + i);
 
            /* check resultant camera parameters: if there are some INF's or NAN's,
               stop and reset results */
            if (!cvCheckArr(&mat, CV_CHECK_RANGE | CV_CHECK_QUIET, -10000, 10000))
            {
                break;
            }
        }
 
 
        isCalibrated = i == cameraCount;
 
        {
            /* calibrate stereo cameras */
            if (cameraCount == 2)
            {
                stereo.camera[0] = &cameraParams[0];
                stereo.camera[1] = &cameraParams[1];
 
                icvStereoCalibration(
                    framesAccepted, counts, imgSize, points[0], points[1], buffer, &stereo);
            }
        }
 
        cvFree(&buffer);
        cvFree(&counts);
        cvFree(&rotMatr);
        cvFree(&transVect);
    }
 
    framesAccepted = 0;
}
 
bool
CvCalibFilter::FindEtalon(IplImage** imgs)
{
    return FindEtalon((CvMat**)imgs);
}
 
int
cvFindChessBoardCornerGuesses(const void* arr,
                              void*,
                              CvMemStorage*,
                              CvSize pattern_size,
                              CvPoint2D32f* corners,
                              int* corner_count)
{
    return cvFindChessboardCorners(
        arr, pattern_size, corners, corner_count, CV_CALIB_CB_ADAPTIVE_THRESH);
}
 
bool
CvCalibFilter::FindEtalon(CvMat** mats)
{
    bool result = true;
 
    if (!mats || etalonPointCount == 0)
    {
        assert(0);
        result = false;
    }
 
    if (result)
    {
        int i, tempPointCount0 = etalonPointCount * 2;
 
        for (i = 0; i < cameraCount; i++)
        {
            if (!latestPoints[i])
                latestPoints[i] =
                    (CvPoint2D32f*)cvAlloc(tempPointCount0 * 2 * sizeof(latestPoints[0]));
        }
 
        for (i = 0; i < cameraCount; i++)
        {
            CvSize size;
            int tempPointCount = tempPointCount0;
            bool found = false;
 
            if (!CV_IS_MAT(mats[i]) && !CV_IS_IMAGE(mats[i]))
            {
                assert(0);
                break;
            }
 
            size = cvGetSize(mats[i]);
 
            if (size.width != imgSize.width || size.height != imgSize.height)
            {
                imgSize = size;
            }
 
            if (!grayImg || grayImg->width != imgSize.width || grayImg->height != imgSize.height)
            {
                cvReleaseMat(&grayImg);
                cvReleaseMat(&tempImg);
                grayImg = cvCreateMat(imgSize.height, imgSize.width, CV_8UC1);
                tempImg = cvCreateMat(imgSize.height, imgSize.width, CV_8UC1);
            }
 
            if (!storage)
            {
                storage = cvCreateMemStorage();
            }
 
            switch (etalonType)
            {
                case CV_CALIB_ETALON_CHESSBOARD:
                    if (CV_MAT_CN(cvGetElemType(mats[i])) == 1)
                    {
                        cvCopy(mats[i], grayImg);
                    }
                    else
                    {
                        cvCvtColor(mats[i], grayImg, CV_BGR2GRAY);
                    }
                    found = cvFindChessBoardCornerGuesses(
                                grayImg,
                                tempImg,
                                storage,
                                cvSize(cvRound(etalonParams[0]), cvRound(etalonParams[1])),
                                latestPoints[i],
                                &tempPointCount) != 0;
                    if (found)
                        cvFindCornerSubPix(
                            grayImg,
                            latestPoints[i],
                            tempPointCount,
                            cvSize(5, 5),
                            cvSize(-1, -1),
                            cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 10, 0.1));
                    break;
                default:
                    assert(0);
                    result = false;
                    break;
            }
 
            latestCounts[i] = found ? tempPointCount : -tempPointCount;
            result = result && found;
        }
    }
 
    if (storage)
    {
        cvClearMemStorage(storage);
    }
 
    return result;
}
 
bool
CvCalibFilter::Push(const CvPoint2D32f** pts)
{
    bool result = true;
    int i, newMaxPoints = etalonPointCount * (MAX(framesAccepted, framesTotal) + 1);
 
    isCalibrated = false;
 
    if (!pts)
    {
        for (i = 0; i < cameraCount; i++)
            if (latestCounts[i] <= 0)
            {
                return false;
            }
        pts = (const CvPoint2D32f**)latestPoints;
    }
 
    for (i = 0; i < cameraCount; i++)
    {
        if (!pts[i])
        {
            assert(0);
            break;
        }
 
        if (maxPoints < newMaxPoints)
        {
            CvPoint2D32f* prev = points[i];
            cvFree(points + i);
            points[i] = (CvPoint2D32f*)cvAlloc(newMaxPoints * sizeof(prev[0]));
            memcpy(points[i], prev, maxPoints * sizeof(prev[0]));
        }
 
        memcpy(points[i] + framesAccepted * etalonPointCount,
               pts[i],
               etalonPointCount * sizeof(points[0][0]));
    }
 
    if (maxPoints < newMaxPoints)
    {
        maxPoints = newMaxPoints;
    }
 
    result = i == cameraCount;
 
    if (++framesAccepted >= framesTotal)
    {
        Stop(true);
    }
    return result;
}
 
bool
CvCalibFilter::GetLatestPoints(int idx, CvPoint2D32f** pts, int* count, bool* found)
{
    int n;
 
    if ((unsigned)idx >= (unsigned)cameraCount || !pts || !count || !found)
    {
        assert(0);
        return false;
    }
 
    n = latestCounts[idx];
 
    *found = n > 0;
    *count = abs(n);
    *pts = latestPoints[idx];
 
    return true;
}
 
void
CvCalibFilter::DrawPoints(IplImage** dst)
{
    DrawPoints((CvMat**)dst);
}
 
void
CvCalibFilter::DrawPoints(CvMat** dstarr)
{
    int i, j;
 
    if (!dstarr)
    {
        assert(0);
        return;
    }
 
    for (i = 0; i < cameraCount; i++)
    {
        if (dstarr[i] && latestCounts[i])
        {
            CvMat dst_stub, *dst;
            int count = 0;
            bool found = false;
            CvPoint2D32f* pts = 0;
 
            GetLatestPoints(i, &pts, &count, &found);
 
            dst = cvGetMat(dstarr[i], &dst_stub);
 
            static const CvScalar line_colors[] = {CvScalar(0, 0, 255),
                                                   CvScalar(0, 128, 255),
                                                   CvScalar(0, 200, 200),
                                                   CvScalar(0, 255, 0),
                                                   CvScalar(200, 200, 0),
                                                   CvScalar(255, 0, 0),
                                                   CvScalar(255, 0, 255)};
 
            const int colorCount = sizeof(line_colors) / sizeof(line_colors[0]);
            const int r = 4;
            CvScalar color = line_colors[0];
            CvPoint prev_pt = {0, 0};
 
            for (j = 0; j < count; j++)
            {
                CvPoint pt;
                pt.x = cvRound(pts[j].x);
                pt.y = cvRound(pts[j].y);
 
                if (found)
                {
                    if (etalonType == CV_CALIB_ETALON_CHESSBOARD)
                    {
                        color = line_colors[(j / cvRound(etalonParams[0])) % colorCount];
                    }
                    else
                    {
                        color = CV_RGB(0, 255, 0);
                    }
 
                    if (j != 0)
                    {
                        cvLine(dst, prev_pt, pt, color, 1, CV_AA);
                    }
                }
 
                cvLine(
                    dst, cvPoint(pt.x - r, pt.y - r), cvPoint(pt.x + r, pt.y + r), color, 1, CV_AA);
 
                cvLine(
                    dst, cvPoint(pt.x - r, pt.y + r), cvPoint(pt.x + r, pt.y - r), color, 1, CV_AA);
 
                cvCircle(dst, pt, r + 1, color, 1, CV_AA);
 
                prev_pt = pt;
            }
        }
    }
}
 
/* Get total number of frames and already accepted pair of frames */
int
CvCalibFilter::GetFrameCount(int* total) const
{
    if (total)
    {
        *total = framesTotal;
    }
 
    return framesAccepted;
}
 
/* Get camera parameters for specified camera. If camera is not calibrated
   the function returns 0 */
const CvCamera*
CvCalibFilter::GetCameraParams(int idx) const
{
    if ((unsigned)idx >= (unsigned)cameraCount)
    {
        assert(0);
        return 0;
    }
 
    return isCalibrated ? cameraParams + idx : 0;
}
 
/* Get camera parameters for specified camera. If camera is not calibrated
   the function returns 0 */
const CvStereoCamera*
CvCalibFilter::GetStereoParams() const
{
    if (!(isCalibrated && cameraCount == 2))
    {
        assert(0);
        return 0;
    }
 
    return &stereo;
}
 
/* Sets camera parameters for all cameras */
bool
CvCalibFilter::SetCameraParams(CvCamera* params)
{
    CvMat mat;
    int arrSize;
 
    Stop();
 
    if (!params)
    {
        assert(0);
        return false;
    }
 
    arrSize = cameraCount * sizeof(params[0]);
 
    cvInitMatHeader(&mat, 1, cameraCount * (arrSize / sizeof(float)), CV_32FC1, params);
    cvCheckArr(&mat, CV_CHECK_RANGE, -10000, 10000);
 
    memcpy(cameraParams, params, arrSize);
    isCalibrated = true;
 
    return true;
}
 
bool
CvCalibFilter::SaveCameraParams(const char* filename)
{
    if (isCalibrated)
    {
        int i, j;
 
        FILE* f = fopen(filename, "w");
 
        if (!f)
        {
            return false;
        }
 
        fprintf(f, "%d\n\n", cameraCount);
 
        for (i = 0; i < cameraCount; i++)
        {
            for (j = 0; j < (int)(sizeof(cameraParams[i]) / sizeof(float)); j++)
            {
                fprintf(f, "%15.10f ", ((float*)(cameraParams + i))[j]);
            }
            fprintf(f, "\n\n");
        }
 
        /* Save stereo params */
 
        /* Save quad */
        for (i = 0; i < 2; i++)
        {
            for (j = 0; j < 4; j++)
            {
                fprintf(f, "%15.10f ", stereo.quad[i][j].x);
                fprintf(f, "%15.10f ", stereo.quad[i][j].y);
            }
            fprintf(f, "\n");
        }
 
        /* Save coeffs */
        for (i = 0; i < 2; i++)
        {
            for (j = 0; j < 9; j++)
            {
                fprintf(f, "%15.10lf ", stereo.coeffs[i][j / 3][j % 3]);
            }
            fprintf(f, "\n");
        }
 
 
        fclose(f);
        return true;
    }
 
    return true;
}
 
bool
CvCalibFilter::LoadCameraParams(const char* filename)
{
    int i, j;
    int d = 0;
    FILE* f = fopen(filename, "r");
 
    isCalibrated = false;
 
    if (!f)
    {
        return false;
    }
 
    if (fscanf(f, "%d", &d) != 1 || d <= 0 || d > 10)
    {
        return false;
    }
 
    SetCameraCount(d);
 
    for (i = 0; i < cameraCount; i++)
    {
        for (j = 0; j < (int)(sizeof(cameraParams[i]) / sizeof(float)); j++)
        {
            int values_read = fscanf(f, "%f", &((float*)(cameraParams + i))[j]);
            CV_Assert(values_read == 1);
        }
    }
 
 
    /* Load stereo params */
 
    /* load quad */
    for (i = 0; i < 2; i++)
    {
        for (j = 0; j < 4; j++)
        {
            int values_read = fscanf(f, "%f ", &(stereo.quad[i][j].x));
            CV_Assert(values_read == 1);
            values_read = fscanf(f, "%f ", &(stereo.quad[i][j].y));
            CV_Assert(values_read == 1);
        }
    }
 
    /* Load coeffs */
    for (i = 0; i < 2; i++)
    {
        for (j = 0; j < 9; j++)
        {
            int values_read = fscanf(f, "%lf ", &(stereo.coeffs[i][j / 3][j % 3]));
            CV_Assert(values_read == 1);
        }
    }
 
 
    fclose(f);
 
    stereo.warpSize =
        cvSize(cvRound(cameraParams[0].imgSize[0]), cvRound(cameraParams[0].imgSize[1]));
 
    isCalibrated = true;
 
    return true;
}
 
bool
CvCalibFilter::Rectify(IplImage** srcarr, IplImage** dstarr)
{
    return Rectify((CvMat**)srcarr, (CvMat**)dstarr);
}
 
bool
CvCalibFilter::Rectify(CvMat** srcarr, CvMat** dstarr)
{
    int i;
 
    if (!srcarr || !dstarr)
    {
        assert(0);
        return false;
    }
 
    if (isCalibrated && cameraCount == 2)
    {
        for (i = 0; i < cameraCount; i++)
        {
            if (srcarr[i] && dstarr[i])
            {
                IplImage src_stub, *src;
                IplImage dst_stub, *dst;
 
                src = cvGetImage(srcarr[i], &src_stub);
                dst = cvGetImage(dstarr[i], &dst_stub);
 
                if (src->imageData == dst->imageData)
                {
                    if (!undistImg || undistImg->width != src->width ||
                        undistImg->height != src->height ||
                        CV_MAT_CN(undistImg->type) != src->nChannels)
                    {
                        cvReleaseMat(&undistImg);
                        undistImg =
                            cvCreateMat(src->height, src->width, CV_8U + (src->nChannels - 1) * 8);
                    }
                    cvCopy(src, undistImg);
                    src = cvGetImage(undistImg, &src_stub);
                }
 
                cvZero(dst);
 
                if (!rectMap[i][0] || rectMap[i][0]->width != src->width ||
                    rectMap[i][0]->height != src->height)
                {
                    cvReleaseMat(&rectMap[i][0]);
                    cvReleaseMat(&rectMap[i][1]);
                    rectMap[i][0] =
                        cvCreateMat(stereo.warpSize.height, stereo.warpSize.width, CV_32FC1);
                    rectMap[i][1] =
                        cvCreateMat(stereo.warpSize.height, stereo.warpSize.width, CV_32FC1);
                    cvComputePerspectiveMap(stereo.coeffs[i], rectMap[i][0], rectMap[i][1]);
                }
                cvRemap(src, dst, rectMap[i][0], rectMap[i][1]);
            }
        }
    }
    else
    {
        for (i = 0; i < cameraCount; i++)
        {
            if (srcarr[i] != dstarr[i])
            {
                cvCopy(srcarr[i], dstarr[i]);
            }
        }
    }
 
    return true;
}
 
bool
CvCalibFilter::Undistort(IplImage** srcarr, IplImage** dstarr)
{
    return Undistort((CvMat**)srcarr, (CvMat**)dstarr);
}
 
bool
CvCalibFilter::Undistort(CvMat** srcarr, CvMat** dstarr)
{
    int i;
 
    if (!srcarr || !dstarr)
    {
        assert(0);
        return false;
    }
 
    if (isCalibrated)
    {
        for (i = 0; i < cameraCount; i++)
        {
            if (srcarr[i] && dstarr[i])
            {
                CvMat src_stub, *src;
                CvMat dst_stub, *dst;
 
                src = cvGetMat(srcarr[i], &src_stub);
                dst = cvGetMat(dstarr[i], &dst_stub);
 
                if (src->data.ptr == dst->data.ptr)
                {
                    if (!undistImg || undistImg->width != src->width ||
                        undistImg->height != src->height || CV_ARE_TYPES_EQ(undistImg, src))
                    {
                        cvReleaseMat(&undistImg);
                        undistImg = cvCreateMat(src->height, src->width, src->type);
                    }
 
                    cvCopy(src, undistImg);
                    src = undistImg;
                }
 
#if 1
                {
                    CvMat A = cvMat(3, 3, CV_32FC1, cameraParams[i].matrix);
                    CvMat k = cvMat(1, 4, CV_32FC1, cameraParams[i].distortion);
 
                    if (!undistMap[i][0] || undistMap[i][0]->width != src->width ||
                        undistMap[i][0]->height != src->height)
                    {
                        cvReleaseMat(&undistMap[i][0]);
                        cvReleaseMat(&undistMap[i][1]);
                        undistMap[i][0] = cvCreateMat(src->height, src->width, CV_32FC1);
                        undistMap[i][1] = cvCreateMat(src->height, src->width, CV_32FC1);
                        cvInitUndistortMap(&A, &k, undistMap[i][0], undistMap[i][1]);
                    }
 
                    cvRemap(src, dst, undistMap[i][0], undistMap[i][1]);
#else
                cvUndistort2(src, dst, &A, &k);
#endif
                }
            }
        }
    }
 
    else
    {
        for (i = 0; i < cameraCount; i++)
        {
            if (srcarr[i] != dstarr[i])
            {
                cvCopy(srcarr[i], dstarr[i]);
            }
        }
    }
 
 
    return true;
}