cv3dtracker.cpp.svn-base

来自「非结构化路识别」· SVN-BASE 代码 · 共 590 行 · 第 1/2 页

/*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) 2002, 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:
//
//   * Redistributions of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistributions 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 "_cvaux.h"

#if _MSC_VER >= 1200
#pragma warning(disable:4786) // Disable MSVC warnings in the standard library.
#pragma warning(disable:4100)
#pragma warning(disable:4512)
#endif
#include <stdio.h>
#include <map>
#include <algorithm>
#if _MSC_VER >= 1200
#pragma warning(default:4100)
#pragma warning(default:4512)
#endif

#define ARRAY_SIZEOF(a) (sizeof(a)/sizeof((a)[0]))

static void FillObjectPoints(CvPoint3D32f *obj_points, CvSize etalon_size, float square_size);
static void DrawEtalon(IplImage *img, CvPoint2D32f *corners,
                       int corner_count, CvSize etalon_size, int draw_ordered);
static void MultMatrix(float rm[4][4], const float m1[4][4], const float m2[4][4]);
static void MultVectorMatrix(float rv[4], const float v[4], const float m[4][4]);
static CvPoint3D32f ImageCStoWorldCS(const Cv3dTrackerCameraInfo &camera_info, CvPoint p);
static bool intersection(CvPoint3D32f o1, CvPoint3D32f p1,
                         CvPoint3D32f o2, CvPoint3D32f p2,
                         CvPoint3D32f &r1, CvPoint3D32f &r2);

/////////////////////////////////
// cv3dTrackerCalibrateCameras //
/////////////////////////////////
CV_IMPL CvBool cv3dTrackerCalibrateCameras(int num_cameras,
                   const Cv3dTrackerCameraIntrinsics camera_intrinsics[], // size is num_cameras
                   CvSize etalon_size,
                   float square_size,
                   IplImage *samples[],                                   // size is num_cameras
                   Cv3dTrackerCameraInfo camera_info[])                   // size is num_cameras
{
    CV_FUNCNAME("cv3dTrackerCalibrateCameras");
    const int num_points = etalon_size.width * etalon_size.height;
    int cameras_done = 0;        // the number of cameras whose positions have been determined
    CvPoint3D32f *object_points = NULL; // real-world coordinates of checkerboard points
    CvPoint2D32f *points = NULL; // 2d coordinates of checkerboard points as seen by a camera
    IplImage *gray_img = NULL;   // temporary image for color conversion
    IplImage *tmp_img = NULL;    // temporary image used by FindChessboardCornerGuesses
    int c, i, j;

    if (etalon_size.width < 3 || etalon_size.height < 3)
        CV_ERROR(CV_StsBadArg, "Chess board size is invalid");

    for (c = 0; c < num_cameras; c++)
    {
        // CV_CHECK_IMAGE is not available in the cvaux library
        // so perform the checks inline.

        //CV_CALL(CV_CHECK_IMAGE(samples[c]));

        if( samples[c] == NULL )
            CV_ERROR( CV_HeaderIsNull, "Null image" );

        if( samples[c]->dataOrder != IPL_DATA_ORDER_PIXEL && samples[c]->nChannels > 1 )
            CV_ERROR( CV_BadOrder, "Unsupported image format" );

        if( samples[c]->maskROI != 0 || samples[c]->tileInfo != 0 )
            CV_ERROR( CV_StsBadArg, "Unsupported image format" );

        if( samples[c]->imageData == 0 )
            CV_ERROR( CV_BadDataPtr, "Null image data" );

        if( samples[c]->roi &&
            ((samples[c]->roi->xOffset | samples[c]->roi->yOffset
              | samples[c]->roi->width | samples[c]->roi->height) < 0 ||
             samples[c]->roi->xOffset + samples[c]->roi->width > samples[c]->width ||
             samples[c]->roi->yOffset + samples[c]->roi->height > samples[c]->height ||
             (unsigned) (samples[c]->roi->coi) > (unsigned) (samples[c]->nChannels)))
            CV_ERROR( CV_BadROISize, "Invalid ROI" );

        // End of CV_CHECK_IMAGE inline expansion

        if (samples[c]->depth != IPL_DEPTH_8U)
            CV_ERROR(CV_BadDepth, "Channel depth of source image must be 8");

        if (samples[c]->nChannels != 3 && samples[c]->nChannels != 1)
            CV_ERROR(CV_BadNumChannels, "Source image must have 1 or 3 channels");
    }

    CV_CALL(object_points = (CvPoint3D32f *)cvAlloc(num_points * sizeof(CvPoint3D32f)));
    CV_CALL(points = (CvPoint2D32f *)cvAlloc(num_points * sizeof(CvPoint2D32f)));

    // fill in the real-world coordinates of the checkerboard points
    FillObjectPoints(object_points, etalon_size, square_size);

    for (c = 0; c < num_cameras; c++)
    {
        CvSize image_size = cvSize(samples[c]->width, samples[c]->height);
        IplImage *img;

        // The input samples are not required to all have the same size or color
        // format. If they have different sizes, the temporary images are
        // reallocated as necessary.
        if (samples[c]->nChannels == 3)
        {
            // convert to gray
            if (gray_img == NULL || !CV_ARE_SIZES_EQ(gray_img, samples[c]))
            {
                if (gray_img != NULL)
                    cvReleaseImage(&gray_img);
                CV_CALL(gray_img = cvCreateImage(image_size, IPL_DEPTH_8U, 1));
            }
            
            CV_CALL(cvCvtColor(samples[c], gray_img, CV_BGR2GRAY));

            img = gray_img;
        }
        else
        {
            // no color conversion required
            img = samples[c];
        }

        if (tmp_img == NULL || !CV_ARE_SIZES_EQ(tmp_img, samples[c]))
        {
            if (tmp_img != NULL)
                cvReleaseImage(&tmp_img);
            CV_CALL(tmp_img = cvCreateImage(image_size, IPL_DEPTH_8U, 1));
        }

        int count = num_points;
        bool found = cvFindChessBoardCornerGuesses(img, tmp_img, 0,
                                                   etalon_size, points, &count) != 0;
        if (count == 0)
            continue;
        
        // If found is true, it means all the points were found (count = num_points).
        // If found is false but count is non-zero, it means that not all points were found.

        cvFindCornerSubPix(img, points, count, cvSize(5,5), cvSize(-1,-1),
                    cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10, 0.01f));

        // If the image origin is BL (bottom-left), fix the y coordinates
        // so they are relative to the true top of the image.
        if (samples[c]->origin == IPL_ORIGIN_BL)
        {
            for (i = 0; i < count; i++)
                points[i].y = samples[c]->height - 1 - points[i].y;
        }

        if (found)
        {
            // Make sure x coordinates are increasing and y coordinates are decreasing.
            // (The y coordinate of point (0,0) should be the greatest, because the point
            // on the checkerboard that is the origin is nearest the bottom of the image.)
            // This is done after adjusting the y coordinates according to the image origin.
            if (points[0].x > points[1].x)
            {
                // reverse points in each row
                for (j = 0; j < etalon_size.height; j++)
                {
                    CvPoint2D32f *row = &points[j*etalon_size.width];
                    for (i = 0; i < etalon_size.width/2; i++)
                        std::swap(row[i], row[etalon_size.width-i-1]);
                }
            }

            if (points[0].y < points[etalon_size.width].y)
            {
                // reverse points in each column
                for (i = 0; i < etalon_size.width; i++)
                {
                    for (j = 0; j < etalon_size.height/2; j++)
                        std::swap(points[i+j*etalon_size.width],
                                  points[i+(etalon_size.height-j-1)*etalon_size.width]);
                }
            }
        }

        DrawEtalon(samples[c], points, count, etalon_size, found);

        if (!found)
            continue;

        float rotVect[3];
        float rotMatr[9];
        float transVect[3];

        cvFindExtrinsicCameraParams(count,
                                    image_size,
                                    points,
                                    object_points,
                                    const_cast<float *>(camera_intrinsics[c].focal_length),
                                    camera_intrinsics[c].principal_point,
                                    const_cast<float *>(camera_intrinsics[c].distortion),
                                    rotVect,
                                    transVect);

        // Check result against an arbitrary limit to eliminate impossible values.
        // (If the chess board were truly that far away, the camera wouldn't be able to
        // see the squares.)
        if (transVect[0] > 1000*square_size
            || transVect[1] > 1000*square_size
            || transVect[2] > 1000*square_size)
        {
            // ignore impossible results
            continue;
        }

        CvMat rotMatrDescr = cvMat(3, 3, CV_32FC1, rotMatr);
        CvMat rotVectDescr = cvMat(3, 1, CV_32FC1, rotVect);

        /* Calc rotation matrix by Rodrigues Transform */
        cvRodrigues(&rotMatrDescr, &rotVectDescr, 0, CV_RODRIGUES_V2M);

        //combine the two transformations into one matrix
        //order is important! rotations are not commutative
        float tmat[4][4] = { { 1.f, 0.f, 0.f, 0.f },
                             { 0.f, 1.f, 0.f, 0.f },
                             { 0.f, 0.f, 1.f, 0.f },
                             { transVect[0], transVect[1], transVect[2], 1.f } };
        
        float rmat[4][4] = { { rotMatr[0], rotMatr[1], rotMatr[2], 0.f },
                             { rotMatr[3], rotMatr[4], rotMatr[5], 0.f },
                             { rotMatr[6], rotMatr[7], rotMatr[8], 0.f },
                             { 0.f, 0.f, 0.f, 1.f } };


        MultMatrix(camera_info[c].mat, tmat, rmat);

        // change the transformation of the cameras to put them in the world coordinate 
        // system we want to work with.

        // Start with an identity matrix; then fill in the values to accomplish
        // the desired transformation.
        float smat[4][4] = { { 1.f, 0.f, 0.f, 0.f },
                             { 0.f, 1.f, 0.f, 0.f },
                             { 0.f, 0.f, 1.f, 0.f },
                             { 0.f, 0.f, 0.f, 1.f } };

        // First, reflect through the origin by inverting all three axes.
        smat[0][0] = -1.f;
        smat[1][1] = -1.f;
        smat[2][2] = -1.f;
        MultMatrix(tmat, camera_info[c].mat, smat);

        // Scale x and y coordinates by the focal length (allowing for non-square pixels
        // and/or non-symmetrical lenses).
        smat[0][0] = 1.0f / camera_intrinsics[c].focal_length[0];
        smat[1][1] = 1.0f / camera_intrinsics[c].focal_length[1];
        smat[2][2] = 1.0f;
        MultMatrix(camera_info[c].mat, smat, tmat);

        camera_info[c].principal_point = camera_intrinsics[c].principal_point;
        camera_info[c].valid = true;

        cameras_done++;
    }