--- /dev/null
+/*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.
+//
+//
+// License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2009, PhaseSpace Inc., 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 names of the copyright holders 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 "cvtest.h"
+
+#if defined WIN32 || defined _WIN32
+#include "direct.h"
+#else
+#include <sys/stat.h>
+#endif
+
+using namespace cv;
+
+//global variable
+#ifndef MAX_PATH
+#define MAX_PATH 1024
+#endif
+
+char g_filepath[MAX_PATH];
+
+//objects for virtual environmnt
+
+/*****************************Camera class ************************************************/
+class Camera
+{
+public:
+ Camera(void);
+ virtual ~Camera(void);
+
+ inline void SetPosition(double x, double y, double z)
+ {
+ position[0] = x;
+ position[1] = y;
+ position[2] = z;
+ }
+
+ inline double* GetPosition() { return position; }
+
+ inline void SetDistortion(double r1, double r2, double t1, double t2)
+ {
+ distortion[0] = r1;
+ distortion[1] = r2;
+ distortion[2] = t1;
+ distortion[3] = t2;
+ }
+ inline void SetIntrinsics( double fx, double fy, double cx, double cy ) //fx,fy,cx,cy
+ {
+ intrinsic[0] = fx;
+ intrinsic[1] = 0;
+ intrinsic[2] = cx;
+ intrinsic[3] = 0;
+ intrinsic[4] = fy;
+ intrinsic[5] = cy;
+ intrinsic[6] = 0;
+ intrinsic[7] = 0;
+ intrinsic[8] = 1.0;
+ }
+
+ CvPoint3D64f ConvertPoint2WCS( CvPoint3D64f pt )
+ {
+ double tmp[3];
+ CvMat tmp_point = cvMat( 3, 1, CV_64FC1, tmp );
+
+ CvMat rot = cvMat( 3, 3, CV_64FC1, rotation );
+ CvMat trans = cvMat( 3, 1, CV_64FC1, translation );
+ CvMat in_point = cvMat( 3, 1, CV_64FC1, &pt );
+
+ // out = inv(R) * (in - t)
+
+ cvSub(&in_point, &trans, &tmp_point);
+ cvGEMM( &rot, &tmp_point, 1, NULL, 0, &tmp_point, CV_GEMM_A_T /* use transposed rotmat*/ );
+
+ CvPoint3D64f out;
+ out.x = tmp[0];
+ out.y = tmp[1];
+ out.z = tmp[2];
+
+ return out;
+ }
+
+ inline void SetRotation( double* rotmat ) //fx,fy,cx,cy
+ {
+ memcpy( rotation, rotmat, 9 * sizeof(double) );
+ }
+
+ double* GetRotation()
+ {
+ return rotation;
+ }
+
+ void ComputeTranslation()
+ {
+ //convert camera position in WCS into translation vector of the camera
+ //t = -R*pos
+ translation[0] = -(rotation[0]*position[0]+rotation[1]*position[1]+rotation[2]*position[2]);
+ translation[1] = -(rotation[3]*position[0]+rotation[4]*position[1]+rotation[5]*position[2]);
+ translation[2] = -(rotation[6]*position[0]+rotation[7]*position[1]+rotation[8]*position[2]);
+ }
+
+ double* GetTranslation()
+ {
+ return translation;
+ }
+ double* GetIntrinsics()
+ {
+ return intrinsic;
+ }
+
+ double* GetDistortion()
+ {
+ return distortion;
+ }
+
+ void SetResolution(CvSize res)
+ {
+ resolution = res;
+ }
+ CvSize GetResolution()
+ {
+ return resolution;
+ }
+ void saveCamParams(FILE* stream);
+ void readCamParams(FILE* stream);
+
+
+protected:
+ double distortion[4]; //distortion coeffs according to OpenCV (2 radial and 2 tangential)
+ double intrinsic[9]; //matrix of intrinsic parameters
+ double translation[3]; //camera's translation vector
+ double rotation[9]; //camera rotation matrix (probably need to convert to camera axis vector
+ double position[3]; //camera's position in WCS
+
+ CvSize resolution;
+};
+
+Camera::Camera(void)
+{
+ //default parameters
+ translation[0] = translation[1] = translation[2] = 0.0;
+ rotation[0] = rotation[1] = rotation[2] =
+ rotation[3] = rotation[4] = rotation[5] =
+ rotation[6] = rotation[7] = rotation[8] = 0.0;
+ rotation[0] = rotation[4] = rotation[8] = 1.0;
+
+ distortion[0] = distortion[1] = distortion[2] = distortion[3] = 0.0;
+}
+
+Camera::~Camera(void)
+{
+}
+
+void Camera::saveCamParams(FILE * stream)
+{
+ float tmp0 = 0.0f, tmp1 = 1.0f;
+
+ // printing camera distortion. 4 parameters
+ fprintf(stream, "%.12f %.12f %.12f %.12f\n", distortion[0], distortion[1],
+ distortion[2], distortion[3]);
+
+ //printing camera intrinsics matrix
+ fprintf(stream, "%.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f\n",
+ intrinsic[0], intrinsic[1], intrinsic[2],
+ intrinsic[3], intrinsic[4], intrinsic[5],
+ intrinsic[6], intrinsic[7], intrinsic[8]);
+
+ //printing camera extrinsic transform
+ fprintf(stream, "%.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f\n",
+ rotation[0], rotation[1], rotation[2], translation[0],
+ rotation[3], rotation[4], rotation[5], translation[1],
+ rotation[6], rotation[7], rotation[8], translation[2],
+ tmp0, tmp0, tmp0, tmp1);
+}
+
+void Camera::readCamParams(FILE* stream)
+{
+ double dummy;
+
+ // read camera distortion. 4 parameters
+ fscanf(stream, "%lf %lf %lf %lf\n", &(distortion[0]), &(distortion[1]), &(distortion[2]), &(distortion[3]) );
+
+ //read camera intrinsics matrix
+ fscanf(stream, "%lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
+ &(intrinsic[0]), &(intrinsic[1]), &(intrinsic[2]),
+ &(intrinsic[3]), &(intrinsic[4]), &(intrinsic[5]),
+ &(intrinsic[6]), &(intrinsic[7]), &(intrinsic[8]));
+
+ //read camera extrinsic transform
+ fscanf(stream, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
+ &(rotation[0]), &(rotation[1]), &(rotation[2]), &(translation[0]),
+ &(rotation[3]), &(rotation[4]), &(rotation[5]), &(translation[1]),
+ &(rotation[6]), &(rotation[7]), &(rotation[8]), &(translation[2]),
+ &dummy, &dummy, &dummy, &dummy);
+}
+
+/******************************** body**************************************************/
+
+class RigidBody
+{
+ //global position of body's center
+ double pos[3];
+ double rotation[9]; //body's rotation matrix
+
+ int* m_ids; //points ids, these are integer numbers not necessarily zero-based indices
+ CvPoint3D64f* m_points3D; //these are points in object coordinate system
+ CvPoint3D64f* m_points3D_WCS; //these are points in WCS
+ int m_numPoints;
+
+public:
+
+ RigidBody()
+ {
+ m_ids = 0;
+ m_points3D = 0;
+ m_points3D_WCS = 0;
+ m_numPoints = 0;
+
+ SetPosition(0,0,0);
+ double t[9] = {1,0,0,0,1,0,0,0,1};
+ SetRotation( t );
+ }
+ virtual ~RigidBody() {}
+
+ virtual void SetPosition(double x, double y, double z )
+ {
+ pos[0] = x;
+ pos[1] = y;
+ pos[2] = z;
+ }
+
+ virtual void SetRotation( double* rotmat )
+ {
+ memcpy( rotation, rotmat, 9 * sizeof(double) );
+ }
+
+ void Save(char* fname, bool wcs /* 1 - save coordinates in wcs, 0 - save coordinates in object coordinate system*/ )
+ {
+ FILE* fp = fopen(fname, "w");
+ fprintf(fp, "%d\n", m_numPoints );
+ if( wcs )
+ {
+ for( int i = 0; i < m_numPoints; i++ )
+ {
+ fprintf(fp, "%d %.12f %.12f %.12f\n", m_ids[i], m_points3D_WCS[i].x, m_points3D_WCS[i].y, m_points3D_WCS[i].z );
+ }
+ }
+ else
+ {
+ for( int i = 0; i < m_numPoints; i++ )
+ {
+ fprintf(fp, "%d, %.12f, %.12f, %.12f\n", m_ids[i], m_points3D[i].x, m_points3D[i].y, m_points3D[i].z );
+ }
+ }
+ fclose(fp);
+ }
+
+ void Load(char* fname)
+ {
+ clear_points();
+ FILE* fp = fopen(fname, "r");
+ fscanf(fp, "%d\n", &m_numPoints );
+ //allocate arrays
+ m_points3D = new CvPoint3D64f[m_numPoints];
+ m_points3D_WCS = new CvPoint3D64f[m_numPoints];
+ m_ids = new int[m_numPoints];
+
+ for(int i = 0; i < m_numPoints; i++ )
+ {
+ fscanf(fp, "%d %lf %lf %lf\n", &(m_ids[i]), &(m_points3D[i].x), &(m_points3D[i].y), &(m_points3D[i].z));
+ }
+ }
+
+ void clear_points()
+ {
+ if(m_points3D)
+ delete m_points3D;
+ m_points3D = NULL;
+ if(m_points3D_WCS)
+ delete m_points3D_WCS;
+ m_points3D_WCS = NULL;
+ if( m_ids )
+ delete m_ids;
+ m_ids = NULL;
+ m_numPoints = 0;
+ }
+
+ void generate_random( int n, double x_min, double x_max,
+ double y_min, double y_max,
+ double z_min, double z_max )
+ {
+ clear_points();
+ m_numPoints = n;
+ m_points3D = new CvPoint3D64f[m_numPoints];
+ m_points3D_WCS = new CvPoint3D64f[m_numPoints];
+ m_ids = new int[m_numPoints];
+
+ //fill ids
+ for( int i = 0 ; i < n; i++ )
+ {
+ m_ids[i] = i;
+ }
+
+ CvRNG rng;
+
+ CvMat values = cvMat( m_numPoints, 1, CV_64FC3, m_points3D );
+
+ cvRandArr( &rng, &values, CV_RAND_UNI,
+ cvScalar(x_min, y_min, z_min), // min
+ cvScalar(x_max, y_max, z_max) // deviation
+ );
+ }
+ CvPoint3D64f* GetPoints3D(bool wcs)
+ {
+ if( wcs )
+ {
+ //fill points in WCS accordingly to rotation matrix and board position
+ for(int i = 0; i < NumPoints(); i++ )
+ {
+ m_points3D_WCS[i].x = rotation[0]*m_points3D[i].x +
+ rotation[1]*m_points3D[i].y +
+ rotation[2]*m_points3D[i].z + pos[0];
+
+ m_points3D_WCS[i].y = rotation[3]*m_points3D[i].x +
+ rotation[4]*m_points3D[i].y +
+ rotation[5]*m_points3D[i].z + pos[1];
+
+ m_points3D_WCS[i].z = rotation[6]*m_points3D[i].x +
+ rotation[7]*m_points3D[i].y +
+ rotation[8]*m_points3D[i].z + pos[2];
+ }
+ //return points in global cooordinates
+ return m_points3D_WCS;
+
+ }
+ else
+ return m_points3D;
+ }
+
+ double* GetRotation() { return rotation; }
+
+ int NumPoints()
+ {
+ return m_numPoints;
+ }
+
+ int* GetPointsIds()
+ {
+ return this->m_ids;
+ }
+
+};
+
+
+/********************************* environment *****************************************/
+#define PT_INVISIBLE 0
+#define PT_VISIBLE 1
+#define PT_OUTBORDER 2
+
+class Environment
+{
+public:
+ Environment(void);
+ virtual ~Environment(void);
+
+ inline int NumCameras()
+ {
+ return m_cameras.size();
+ }
+
+ inline int AddCamera(Camera* cam)
+ {
+ m_cameras.push_back(cam);
+ return m_cameras.size();
+ }
+
+ RigidBody* GetBody()
+ {
+ return m_body;
+ }
+
+ void SetNoise(float n)
+ {
+ noise = n;
+ }
+
+ int Capture();
+ int Save(char* filename);
+
+protected:
+
+ std::vector<Camera*> m_cameras;
+ RigidBody* m_body;
+ std::vector<CvPoint2D64d*> m_image_points;
+ std::vector<int*> m_point_visibility;
+ float noise;
+
+};
+
+Environment::Environment(void)
+{
+ m_body = new RigidBody();
+}
+
+Environment::~Environment(void)
+{
+ delete m_body;
+}
+
+int Environment::Capture()
+{
+ //clear points
+ for( size_t i = 0; i < m_image_points.size(); i++ )
+ {
+ if( m_image_points[i] )
+ delete m_image_points[i];
+ }
+ m_image_points.clear();
+
+ for( size_t i = 0; i < m_point_visibility.size(); i++ )
+ {
+ if( m_point_visibility[i] )
+ delete m_point_visibility[i];
+ }
+ m_point_visibility.clear();
+
+ CvPoint3D64f* board_pts = m_body->GetPoints3D(true);
+ int num_points = m_body->NumPoints();
+
+ //loop over cameras
+ for( size_t i = 0; i < m_cameras.size(); i++ )
+ {
+ //get camera parameters
+ //project points onto camera image
+
+ double* rot = m_cameras[i]->GetRotation();
+ double* trans = m_cameras[i]->GetTranslation();
+ double* intr = m_cameras[i]->GetIntrinsics();
+ double* dist = m_cameras[i]->GetDistortion();
+
+ CvPoint2D64f* image_points = new CvPoint2D64f[num_points];
+ m_image_points.push_back(image_points);
+
+ int* points_visibility = new int[num_points];
+ m_point_visibility.push_back(points_visibility);
+
+ cvProjectPointsSimple(num_points, board_pts, rot, trans, intr, dist, image_points);
+
+ CvRNG rng;
+
+ if( noise > 0)
+ {
+ CvMat* values = cvCreateMat( num_points, 1, CV_32FC2 );
+
+ float stdev = noise;
+
+ cvRandArr( &rng, values, CV_RAND_NORMAL,
+ cvScalar(0.0, 0.0), // mean
+ cvScalar(stdev, stdev) // deviation
+ );
+
+ //add gaussian noise to image points
+
+ for( int j = 0; j < num_points; j++ )
+ {
+ CvPoint2D32f pt = *(CvPoint2D32f*)cvPtr1D( values, j, 0 );
+
+ pt.x = min( pt.x, stdev);
+ pt.x = max( pt.x, -stdev);
+
+ pt.y = min( pt.y, stdev);
+ pt.y = max( pt.y, -stdev);
+
+ image_points[j].x += pt.x;
+ image_points[j].y += pt.y;
+ }
+ cvReleaseMat( &values );
+ }
+
+ //decide if point visible to camera
+ //loop over points and assign visibility flag to them
+ for( int j = 0; j < num_points; j++ )
+ {
+ //generate random visibility of the point
+ int visible = cvRandInt(&rng) % 2; //visibility 50%
+
+ //check the point is in camera FOV (1-pixel border assumed invisible)
+ if( image_points[j].x > 0 && image_points[j].x < m_cameras[i]->GetResolution().width-1
+ && image_points[j].y > 0 && image_points[j].y < m_cameras[i]->GetResolution().height-1 )
+ {
+ if(!visible)
+ points_visibility[j] = PT_INVISIBLE;
+ else
+ points_visibility[j] = PT_VISIBLE;
+ }
+ else
+ points_visibility[j] = PT_OUTBORDER;
+ }
+ }
+
+ //some points may become completely invisible for all cameras or visible only at one camera
+ //we will forcely make them visible for at least 2 cameras
+ for( int i = 0 ; i < num_points; i++ )
+ {
+ int numvis = 0;
+ for( size_t j = 0; j < m_cameras.size(); j++ )
+ {
+ if( m_point_visibility[j][i] == PT_VISIBLE )
+ numvis++;
+ }
+
+ if(numvis < 2)
+ {
+ for( size_t j = 0; j < m_cameras.size(); j++ )
+ {
+ if( m_point_visibility[j][i] == PT_INVISIBLE )
+ {
+ m_point_visibility[j][i] = PT_VISIBLE;
+ numvis++;
+ }
+ if(numvis > 1)
+ break;
+ }
+ assert(numvis > 1 );
+ }
+ }
+
+ return 0;
+}
+
+/*void Environment::TestCamera(int camind)
+{
+ CvPoint3D64f* board_pts = m_body->GetPoints3D(false);
+
+ int count = 0;
+ for( int j = 0; j < m_body->NumPoints(); j++ )
+ {
+ if( m_point_visibility[camind][j] == PT_VISIBLE )
+ count++;
+ }
+
+ CvMat* object_points = cvCreateMat( 1, count, CV_64FC3 );
+ CvMat* image_points = cvCreateMat( 1, count, CV_64FC2 );
+ CvMat* intrinsic_matrix = cvCreateMatHeader( 3, 3, CV_64FC1 );
+ cvSetData(intrinsic_matrix, m_cameras[camind]->GetIntrinsics(), 3*sizeof(double) );
+
+ CvMat* distortion_coeffs = cvCreateMat( 4, 1, CV_64FC1 ); cvSetZero(distortion_coeffs);
+ CvMat* rotation_vector = cvCreateMat( 3, 1, CV_64FC1 );
+ CvMat* translation_vector = cvCreateMat( 3, 1, CV_64FC1 );
+
+ CvMat* rotation_matrix = cvCreateMat( 3, 3, CV_64FC1 );
+
+ //loop over points and assign visibility flag to them
+ int ind = 0;
+ for( int j = 0; j < m_body->NumPoints(); j++ )
+ {
+ if( m_point_visibility[camind][j] == PT_VISIBLE)
+ {
+ //add to matrix
+ *((CvPoint3D64f*)(object_points->data.db + ind*3)) = board_pts[j];
+ *((CvPoint2D64f*)(image_points->data.db + ind*2)) = m_image_points[camind][j];
+ ind++;
+ }
+ }
+
+
+ //find extrinsic parameters of the board
+ cvFindExtrinsicCameraParams2( object_points,
+ image_points,
+ intrinsic_matrix,
+ distortion_coeffs,
+ rotation_vector,
+ translation_vector );
+
+ //cvRodrigues2( rotation_vector, rotation_matrix );
+
+
+ //reproject points and see error of reprojection
+
+ CvMat* image_points_repro = cvCloneMat( image_points );
+
+ cvProjectPoints2( object_points, rotation_vector, translation_vector, intrinsic_matrix, distortion_coeffs,
+ image_points_repro );
+
+ for( int j = 0; j < image_points_repro->cols; j++ )
+ {
+ CvPoint2D64f pt_orig = *((CvPoint2D64f*)(image_points->data.db + j*2));
+ CvPoint2D64f pt_repro = *((CvPoint2D64f*)(image_points_repro->data.db + j*2));
+
+ CvPoint2D64f diff;
+ diff.x = pt_repro.x - pt_orig.x;
+ diff.y = pt_repro.y - pt_orig.y;
+
+ }
+} */
+
+int Environment::Save(char* filename)
+{
+ int* ind = m_body->GetPointsIds();
+ FILE* saveFile = fopen( filename, "w" );
+ if(saveFile)
+ {
+ for( size_t cam_index = 0; cam_index < m_cameras.size(); cam_index++ )
+ {
+ CvPoint2D64f* image_points = m_image_points[cam_index];
+
+ int numPnt = 0; //no visible points by default
+
+ //check points visibility
+ //camera is visible, check individual points
+ for(int i = 0; i < m_body->NumPoints(); i++ )
+ {
+ if( m_point_visibility[cam_index][i] == PT_VISIBLE)
+ {
+ //point is visible
+ numPnt++;
+ }
+ }
+
+ if(numPnt) //some points are visible
+ {
+ for( int i = 0; i < m_body->NumPoints(); i++)
+ {
+ if( m_point_visibility[cam_index][i] == PT_VISIBLE )
+ {
+ //point is visible
+ fprintf(saveFile, "%d %d %d %.12f %.12f\n", 0 /*snapshot_id*/, (int)cam_index, ind[i], image_points[i].x, image_points[i].y );
+ }
+ }
+ }
+
+ }
+ }
+
+ //close file
+ fclose(saveFile);
+ return 0;
+}
+
+
+
+
+//input parameters for envirinment generation
+struct Params
+{
+ int width; //width of images
+ int height; //height of images
+ float FOVx; //camera field of view in horizontal direction. vertical FOV is detected from aspect ratio
+ float noise; //corners projection noise
+ float k1, k2, k3; //radial distortion coeffs
+ float p1,p2; //tangential distortion coeffs
+};
+
+int GenerateTestData2(Params& params)
+{
+ //create environment
+ Environment* env = new Environment();
+ env->SetNoise(params.noise);
+
+ CvSize im_res = cvSize(params.width,params.height);
+
+ double FOVx = params.FOVx; // field of view relatively to width (in degrees)
+ double fov_rad = FOVx * CV_PI / 180;
+ double fx = im_res.width/2.0 / tan( fov_rad/2);
+ double fy = fx;
+ double cx = im_res.width/2.0-0.5;
+ double cy = im_res.height/2.0-0.5;
+
+ //model cube room of size 2x2x2 meters
+ //8 cameras on the perimeter at the height 1 meter in corners and in the middle of walls
+ // all they look to the center of the room
+ //coordinate center in the center of the cube
+ //Z coordinate is oriented up
+
+ Camera* cam[8];
+ for( int i = 0; i < 8; i++ )
+ {
+ cam[i] = new Camera();
+ cam[i]->SetDistortion(params.k1, params.k2,0,0); //only radial distortion
+ cam[i]->SetIntrinsics(fx,fy, cx, cy ); //fx,fy,cx,cy
+ cam[i]->SetResolution(im_res);
+ env->AddCamera(cam[i]);
+ }
+
+ //set positions
+ cam[0]->SetPosition( 1.,1.,0);
+ cam[1]->SetPosition( 0.,1.,0);
+ cam[2]->SetPosition( -1.,1.,0);
+ cam[3]->SetPosition( -1.,0.,0);
+ cam[4]->SetPosition( -1.,-1.,0);
+ cam[5]->SetPosition( 0.,-1.,0);
+ cam[6]->SetPosition( 1.,-1.,0);
+ cam[7]->SetPosition( 1.,0.,0);
+
+ //set rotation matrices
+ //they will be oriented strongly vertically and rotated only around vertical axis (Z coorinate in WCS)
+
+ CvMat* camrot = cvCreateMat( 3, 3, CV_64FC1);
+
+ for( int i = 0; i < 8; i++ )
+ {
+ //y of the camera is oriented along negative Z of WCS
+ double yDirCamera[3] = {0,0,-1};
+ double zDirCamera[3]; //oriented from camera center to WCS center
+ double* pos = cam[i]->GetPosition();
+ zDirCamera[0] = -pos[0];
+ zDirCamera[1] = -pos[1];
+ zDirCamera[2] = -pos[2];
+
+ double xDirCamera[3]; //cross product Y*Z
+ xDirCamera[0] = yDirCamera[1]*zDirCamera[2] - yDirCamera[2]*zDirCamera[1];
+ xDirCamera[1] = yDirCamera[2]*zDirCamera[0] - yDirCamera[0]*zDirCamera[2];
+ xDirCamera[2] = yDirCamera[0]*zDirCamera[1] - yDirCamera[1]*zDirCamera[0];
+
+ //normalize z and x
+ double inv_norm = 1.0/sqrt(xDirCamera[0]*xDirCamera[0] + xDirCamera[1]*xDirCamera[1] + xDirCamera[2]*xDirCamera[2]);
+ xDirCamera[0]*=inv_norm;
+ xDirCamera[1]*=inv_norm;
+ xDirCamera[2]*=inv_norm;
+
+ inv_norm = 1.0 / sqrt(zDirCamera[0]*zDirCamera[0] + zDirCamera[1]*zDirCamera[1] + zDirCamera[2]*zDirCamera[2]);
+ zDirCamera[0] *= inv_norm;
+ zDirCamera[1] *= inv_norm;
+ zDirCamera[2] *= inv_norm;
+
+ camrot->data.db[0] = xDirCamera[0];
+ camrot->data.db[3] = xDirCamera[1];
+ camrot->data.db[6] = xDirCamera[2];
+
+ camrot->data.db[1] = yDirCamera[0];
+ camrot->data.db[4] = yDirCamera[1];
+ camrot->data.db[7] = yDirCamera[2];
+
+ camrot->data.db[2] = zDirCamera[0];
+ camrot->data.db[5] = zDirCamera[1];
+ camrot->data.db[8] = zDirCamera[2];
+
+ //get inverse matrix (equal to transposed)
+ cvTranspose(camrot, camrot);
+
+ cam[i]->SetRotation(camrot->data.db);
+ cam[i]->ComputeTranslation();// compute translation after we set position and rotation matrix
+ }
+
+#if defined WIN32 || defined _WIN32
+ _mkdir(g_filepath);
+#else
+ mkdir(g_filepath, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+#endif
+ char fname[2048];
+ sprintf(fname, "%scameras.txt", g_filepath );
+ FILE* clist = fopen(fname, "w");
+ for(unsigned int i = 0; i < 8; i++)
+ {
+ sprintf(fname, "%scamera%d.calib", g_filepath, i );
+ FILE *calibFile = fopen(fname,"w");
+ cam[i]->saveCamParams(calibFile);
+ fclose( calibFile );
+ fprintf(clist, "%s\n", fname);
+ }
+ fclose(clist);
+
+ std::vector<CvPoint3D64f> board_pos;
+ board_pos.clear();
+
+ //generate body position, set zero for now
+ CvPoint3D64f pos;
+ pos.x = 0; pos.y = 0; pos.z = 0;
+ board_pos.push_back(pos);
+
+ RigidBody* body = env->GetBody();
+ body->generate_random(50, -0.25, 0.25, -0.25, 0.25, -0.25, 0.25 );
+
+ CvRNG rng = cvRNG();
+ int index = 0; //index of board's position to save, is used to enumerate snapshots
+
+ for(size_t i = 0; i < board_pos.size(); i++ )
+ {
+ CvPoint3D64f point = board_pos[i];
+ body->SetPosition(point.x,point.y,point.z);
+
+ //generate random orientation of the board
+ unsigned int rn = cvRandInt(&rng);
+ //scale to 90 degrees
+ rn = rn%90;
+ double slant = (double)rn-45-90; //angle of slant of the bord relatively to its horizontal direction
+
+ rn = cvRandInt(&rng);
+ //scale to 360 degrees
+ rn = rn%360;
+ double azimuth = rn; //azimuth
+
+
+ //set slant and azimuth to 0
+ slant = azimuth = 0;
+
+ double sn = sin(azimuth*CV_PI/180);
+ double cs = cos(azimuth*CV_PI/180);
+
+ //generate rotation matrix
+ CvMat* mat_azimuth = cvCreateMat( 3, 3, CV_64FC1);
+ mat_azimuth->data.db[0] = cs;
+ mat_azimuth->data.db[1] = -sn;
+ mat_azimuth->data.db[2] = 0;
+
+ mat_azimuth->data.db[3] = sn;
+ mat_azimuth->data.db[4] = cs;
+ mat_azimuth->data.db[5] = 0;
+
+ mat_azimuth->data.db[6] = 0;
+ mat_azimuth->data.db[7] = 0;
+ mat_azimuth->data.db[8] = 1;
+
+ sn = sin(slant*CV_PI/180);
+ cs = cos(slant*CV_PI/180);
+ CvMat* mat_slant = cvCreateMat( 3, 3, CV_64FC1); //rotation around X axis
+ mat_slant->data.db[0] = 1;
+ mat_slant->data.db[1] = 0;
+ mat_slant->data.db[2] = 0;
+
+ mat_slant->data.db[3] = 0;
+ mat_slant->data.db[4] = cs;
+ mat_slant->data.db[5] = -sn;
+
+ mat_slant->data.db[6] = 0;
+ mat_slant->data.db[7] = sn;
+ mat_slant->data.db[8] = cs;
+
+ CvMat* rot = cvCreateMat( 3, 3, CV_64FC1); //rotation around X axis
+ //create complete rotation matrix
+ cvMatMul(mat_azimuth, mat_slant, rot);
+
+ //these are coordinates of board's axis in WCS
+ body->SetRotation(rot->data.db);
+
+ //project images onto all cameras
+ env->Capture();
+
+ //save 3d points of corners into the file
+ char fname[2048];
+ sprintf(fname, "%sPoints%d.3d", g_filepath, index );
+ body->Save(fname, true);
+ //save shots from camera
+
+ sprintf(fname, "%salldata.txt", g_filepath );
+ env->Save(fname);
+ }
+ //destroy everything
+ delete env;
+ return 0;
+}
+
+int TestLevmar2()
+{
+ //load data from file
+
+ // 1. load ground truth 3D points
+ RigidBody body;
+
+ char fname[2048];
+ sprintf(fname, "%sPoints0.3d", g_filepath );
+ body.Load(fname);
+ int num_points = body.NumPoints();
+
+ // 2. load cameras
+ int num_cams = 0;
+ //int num_cam_param = 10;
+ std::vector<Camera*> cams;
+
+ sprintf(fname, "%scameras.txt", g_filepath );
+ FILE* fp = fopen(fname, "r" );
+ while( !feof(fp) )
+ {
+ char fname[MAX_PATH];
+ fscanf(fp, "%s\n", fname );
+ Camera* newcam = new Camera();
+ FILE* fp2 = fopen( fname, "r" );
+ newcam->readCamParams(fp2);
+ cams.push_back(newcam);
+ fclose(fp2);
+ num_cams++;
+ }
+ fclose(fp);
+
+ // 2. Load projection data
+ CvMat* m = cvCreateMat( body.NumPoints(), num_cams, CV_64FC2 );
+ //all invisible point will have (-1,-1) projection
+ cvSet( m, cvScalar(-1,-1) );
+
+ sprintf(fname, "%salldata.txt", g_filepath );
+ fp = fopen( fname, "r");
+
+ int counter = 0;
+ while( !feof(fp) )
+ {
+ //read line
+ int snapid, cameraid, pointid;
+ double x,y;
+ fscanf(fp, "%d %d %d %lf %lf\n", &snapid, &cameraid, &pointid, &x, &y);
+ cvSet2D(m, pointid, cameraid, cvScalar(x,y) );
+ counter++;
+ }
+
+ vector<vector<int> > visibility;
+
+ //transform this matrix to measurement vector
+ vector<vector<Point2d> > imagePoints;
+
+ for(int j = 0; j < num_cams; j++ )
+ {
+ vector<Point2d> vec;
+ vector<int> visvec;
+ for(int i = 0; i < num_points; i++ )
+ {
+ CvScalar s = cvGet2D( m, i, j );
+ if( s.val[0] != -1 ) //point is visible
+ {
+ vec.push_back(Point2d(s.val[0],s.val[1]));
+ visvec.push_back(1);
+ }
+ else
+ {
+ vec.push_back(Point2d(DBL_MAX,DBL_MAX));
+ visvec.push_back(0);
+ }
+ }
+ imagePoints.push_back(vec);
+ visibility.push_back(visvec);
+ }
+
+ //form initial params
+ vector<Mat> cameraMatrix; //intrinsic matrices of all cameras (input and output)
+ vector<Mat> R; //rotation matrices of all cameras (input and output)
+ vector<Mat> T; //translation vector of all cameras (input and output)
+ vector<Mat> distCoeffs; //distortion coefficients of all cameras (input and output)
+
+ //store original camera positions
+ vector<Mat> T_backup;
+ //store original camera rotations
+ vector<Mat> R_backup;
+ //store original intrinsic matrix
+ vector<Mat> cameraMatrix_backup;
+ //store original distortion
+ vector<Mat> distCoeffs_backup;
+
+ for( int i = 0; i < (int)cams.size(); i++ )
+ {
+ //fill params
+ Camera* c = cams[i];
+ //rotation
+ double* rotmat = c->GetRotation();
+ Mat rm(3,3,CV_64F,rotmat);
+ R_backup.push_back(rm);
+ //generate small random rotation
+ Mat rodr; Rodrigues(rm, rodr);
+ double n = norm(rodr);
+ //add small angle
+ //add about 5 degrees to rotation
+ rodr *= ((n+0.1)/n);
+ Rodrigues(rodr, rm);
+ R.push_back(rm);
+
+ //translation
+ double* tr = c->GetTranslation();
+ Mat tv(Size(1,3), CV_64F, tr);
+ T_backup.push_back(tv);
+ //add random translation within 1 cm
+ Mat t_(3,1,CV_64F);
+ randu(t_, -0.01, 0.01);
+ tv+=t_;
+
+ T.push_back(tv);
+ //intrinsic matrix
+ double* intr = c->GetIntrinsics();
+ cameraMatrix_backup.push_back(Mat(Size(3,3), CV_64F, intr));
+ cameraMatrix.push_back(Mat(Size(3,3), CV_64F, intr));
+
+ //distortion
+ Mat d(4, 1, CV_64F, c->GetDistortion() );
+ distCoeffs_backup.push_back(d);
+
+ //variate distortion by 5%
+ d*=1.05;
+ distCoeffs.push_back(d);
+
+ }
+
+ //form input points
+ const Point3d* ptptr = (const Point3d*)body.GetPoints3D(true);
+ vector<Point3d> points(ptptr, ptptr + num_points);
+ //store points
+ vector<Point3d> points_backup(num_points);
+ std::copy(points.begin(), points.end(), points_backup.begin());
+
+ //variate initial points
+ CvRNG rng;
+ CvMat* values = cvCreateMat( num_points*3, 1, CV_64F );
+ cvRandArr( &rng, values, CV_RAND_NORMAL,
+ cvScalar(0.0), // mean
+ cvScalar(0.02) // deviation (in meters)
+ );
+ CvMat tmp = cvMat(values->rows, values->cols, values->type, &points[0] );
+ cvAdd( &tmp, values, &tmp );
+ cvReleaseMat( &values );
+
+ LevMarqSparse::bundleAdjust( points, //positions of points in global coordinate system (input and output)
+ imagePoints, //projections of 3d points for every camera
+ visibility, //visibility of 3d points for every camera
+ cameraMatrix, //intrinsic matrices of all cameras (input and output)
+ R, //rotation matrices of all cameras (input and output)
+ T, //translation vector of all cameras (input and output)
+ distCoeffs, //distortion coefficients of all cameras (input and output)
+ TermCriteria(TermCriteria::COUNT|TermCriteria::EPS, 3000, DBL_EPSILON ));
+ //,enum{MOTION_AND_STRUCTURE,MOTION,STRUCTURE})
+
+ //compare input points and found points
+ double maxdist = 0;
+ for( size_t i = 0; i < points.size(); i++ )
+ {
+ Point3d in = points_backup[i];
+ Point3d out = points[i];
+ double dist = sqrt(in.dot(out));
+ if(dist > maxdist)
+ maxdist = dist;
+ }
+ printf("Maximal distance between points: %.12lf\n", maxdist );
+
+ //compare camera positions
+ maxdist = 0;
+ for( size_t i = 0; i < T.size(); i++ )
+ {
+ double dist = norm(T[i], T_backup[i]);
+
+ if(dist > maxdist)
+ maxdist = dist;
+ }
+ printf("Maximal distance between cameras centers: %.12lf\n", maxdist );
+
+ //compare rotation matrices
+ maxdist = 0;
+ for( size_t i = 0; i < R.size(); i++ )
+ {
+ double dist = norm(R[i], R_backup[i], NORM_INF);
+
+ if(dist > maxdist)
+ maxdist = dist;
+ }
+ printf("Maximal difference in rotation matrices elements: %.12lf\n", maxdist );
+
+ //compare intrinsic matrices
+ maxdist = 0;
+ double total_diff = 0;
+ for( size_t i = 0; i < cameraMatrix.size(); i++ )
+ {
+ double fx_ratio = cameraMatrix[i].at<double>(0,0)/
+ cameraMatrix_backup[i].at<double>(0,0);
+ double fy_ratio = cameraMatrix[i].at<double>(1,1)/
+ cameraMatrix_backup[i].at<double>(1,1);
+ double cx_diff = cameraMatrix[i].at<double>(0,2) -
+ cameraMatrix_backup[i].at<double>(0,2);
+ double cy_diff = cameraMatrix[i].at<double>(1,2) -
+ cameraMatrix_backup[i].at<double>(1,2);
+ total_diff += fabs(fx_ratio - 1) + fabs(fy_ratio - 1) + fabs(cx_diff) + fabs(cy_diff);
+ }
+ //ts->printf(CvTS::LOG, "total diff = %g\n", total_diff);
+
+ return 1;
+}
+
+
+class CV_BundleAdjustmentTest : public CvTest
+{
+
+public:
+ CV_BundleAdjustmentTest();
+ ~CV_BundleAdjustmentTest();
+ void clear();
+ //int write_default_params(CvFileStorage* fs);
+
+protected:
+ //int read_params( CvFileStorage* fs );
+ int compare(double* val, double* ref_val, int len,
+ double eps, const char* param_name);
+
+ void run(int);
+};
+
+
+CV_BundleAdjustmentTest::CV_BundleAdjustmentTest():
+ CvTest( "bundleadjust", "bundleAdjust" )
+{
+ support_testing_modes = CvTS::CORRECTNESS_CHECK_MODE;
+}
+
+
+CV_BundleAdjustmentTest::~CV_BundleAdjustmentTest()
+{
+ clear();
+}
+
+
+void CV_BundleAdjustmentTest::clear()
+{
+ CvTest::clear();
+}
+
+
+int CV_BundleAdjustmentTest::compare(double* val, double* ref_val, int len,
+ double eps, const char* param_name )
+{
+ return cvTsCmpEps2_64f( ts, val, ref_val, len, eps, param_name );
+}
+
+void CV_BundleAdjustmentTest::run( int start_from )
+{
+ int code = CvTS::OK;
+ char filepath[100];
+ char filename[100];
+
+ CvSize imageSize;
+ int numImages;
+ CvSize etalonSize;
+
+ CvPoint2D64d* imagePoints;
+ CvPoint3D64d* objectPoints;
+ CvPoint2D64d* reprojectPoints;
+
+ CvVect64d transVects;
+ CvMatr64d rotMatrs;
+
+ CvVect64d goodTransVects;
+ CvMatr64d goodRotMatrs;
+
+ double cameraMatrix[3*3];
+ double distortion[4];
+
+ double goodDistortion[4];
+
+ int* numbers;
+ FILE* file = 0;
+ FILE* datafile = 0;
+ int i,j;
+ int currImage;
+ int currPoint;
+
+ int calibFlags;
+ char i_dat_file[100];
+ int numPoints;
+ int numTests;
+ int currTest;
+
+ imagePoints = 0;
+ objectPoints = 0;
+ reprojectPoints = 0;
+ numbers = 0;
+
+ transVects = 0;
+ rotMatrs = 0;
+ goodTransVects = 0;
+ goodRotMatrs = 0;
+ int progress = 0;
+
+ //generate test data
+ Params params;
+ //set default params
+
+ params.FOVx = 30;
+ params.height = 480;
+ params.width = 640;
+ //params.noise = 0.25f;
+ params.k1 = -0.5f;
+ params.k2 = -0.5f;
+ params.k3 = 0.0f;
+ params.noise = 0.0f;
+ params.p1 = 0;
+ params.p2 = 0;
+
+ sprintf( g_filepath, "%sSBA/", ts->get_data_path() );
+
+ GenerateTestData2(params);
+ TestLevmar2();
+
+ sprintf( filepath, "%scameracalibration/", ts->get_data_path() );
+ sprintf( filename, "%sdatafiles.txt", filepath );
+ datafile = fopen( filename, "r" );
+ if( datafile == 0 )
+ {
+ ts->printf( CvTS::LOG, "Could not open file with list of test files: %s\n", filename );
+ code = CvTS::FAIL_MISSING_TEST_DATA;
+ goto _exit_;
+ }
+
+ fscanf(datafile,"%d",&numTests);
+
+ for( currTest = start_from; currTest < numTests; currTest++ )
+ {
+ fscanf(datafile,"%s",i_dat_file);
+ sprintf(filename, "%s%s", filepath, i_dat_file);
+ file = fopen(filename,"r");
+
+ ts->update_context( this, currTest, true );
+
+ if( file == 0 )
+ {
+ ts->printf( CvTS::LOG,
+ "Can't open current test file: %s\n",filename);
+ if( numTests == 1 )
+ {
+ code = CvTS::FAIL_MISSING_TEST_DATA;
+ goto _exit_;
+ }
+ continue; // if there is more than one test, just skip the test
+ }
+
+ fscanf(file,"%d %d\n",&(imageSize.width),&(imageSize.height));
+ if( imageSize.width <= 0 || imageSize.height <= 0 )
+ {
+ ts->printf( CvTS::LOG, "Image size in test file is incorrect\n" );
+ code = CvTS::FAIL_INVALID_TEST_DATA;
+ goto _exit_;
+ }
+
+ /* Read etalon size */
+ fscanf(file,"%d %d\n",&(etalonSize.width),&(etalonSize.height));
+ if( etalonSize.width <= 0 || etalonSize.height <= 0 )
+ {
+ ts->printf( CvTS::LOG, "Pattern size in test file is incorrect\n" );
+ code = CvTS::FAIL_INVALID_TEST_DATA;
+ goto _exit_;
+ }
+
+ numPoints = etalonSize.width * etalonSize.height;
+
+ /* Read number of images */
+ fscanf(file,"%d\n",&numImages);
+ if( numImages <=0 )
+ {
+ ts->printf( CvTS::LOG, "Number of images in test file is incorrect\n");
+ code = CvTS::FAIL_INVALID_TEST_DATA;
+ goto _exit_;
+ }
+
+ /* Need to allocate memory */
+ imagePoints = (CvPoint2D64d*)cvAlloc( numPoints *
+ numImages * sizeof(CvPoint2D64d));
+
+ objectPoints = (CvPoint3D64d*)cvAlloc( numPoints *
+ numImages * sizeof(CvPoint3D64d));
+
+ reprojectPoints = (CvPoint2D64d*)cvAlloc( numPoints *
+ numImages * sizeof(CvPoint2D64d));
+
+ /* Alloc memory for numbers */
+ numbers = (int*)cvAlloc( numImages * sizeof(int));
+
+ /* Fill it by numbers of points of each image*/
+ for( currImage = 0; currImage < numImages; currImage++ )
+ {
+ numbers[currImage] = etalonSize.width * etalonSize.height;
+ }
+
+ /* Allocate memory for translate vectors and rotmatrixs*/
+ transVects = (CvVect64d)cvAlloc(3 * 1 * numImages * sizeof(double));
+ rotMatrs = (CvMatr64d)cvAlloc(3 * 3 * numImages * sizeof(double));
+
+ goodTransVects = (CvVect64d)cvAlloc(3 * 1 * numImages * sizeof(double));
+ goodRotMatrs = (CvMatr64d)cvAlloc(3 * 3 * numImages * sizeof(double));
+
+ /* Read object points */
+ i = 0;/* shift for current point */
+ for( currImage = 0; currImage < numImages; currImage++ )
+ {
+ for( currPoint = 0; currPoint < numPoints; currPoint++ )
+ {
+ double x,y,z;
+ fscanf(file,"%lf %lf %lf\n",&x,&y,&z);
+
+ (objectPoints+i)->x = x;
+ (objectPoints+i)->y = y;
+ (objectPoints+i)->z = z;
+ i++;
+ }
+ }
+
+ /* Read image points */
+ i = 0;/* shift for current point */
+ for( currImage = 0; currImage < numImages; currImage++ )
+ {
+ for( currPoint = 0; currPoint < numPoints; currPoint++ )
+ {
+ double x,y;
+ fscanf(file,"%lf %lf\n",&x,&y);
+
+ (imagePoints+i)->x = x;
+ (imagePoints+i)->y = y;
+ i++;
+ }
+ }
+
+ /* Read good data computed before */
+
+ /* Focal lengths */
+ double goodFcx,goodFcy;
+ fscanf(file,"%lf %lf",&goodFcx,&goodFcy);
+
+ /* Principal points */
+ double goodCx,goodCy;
+ fscanf(file,"%lf %lf",&goodCx,&goodCy);
+
+ /* Read distortion */
+
+ fscanf(file,"%lf",goodDistortion+0);
+ fscanf(file,"%lf",goodDistortion+1);
+ fscanf(file,"%lf",goodDistortion+2);
+ fscanf(file,"%lf",goodDistortion+3);
+
+ /* Read good Rot matrixes */
+ for( currImage = 0; currImage < numImages; currImage++ )
+ {
+ for( i = 0; i < 3; i++ )
+ for( j = 0; j < 3; j++ )
+ fscanf(file, "%lf", goodRotMatrs + currImage * 9 + j * 3 + i);
+ }
+
+ /* Read good Trans vectors */
+ for( currImage = 0; currImage < numImages; currImage++ )
+ {
+ for( i = 0; i < 3; i++ )
+ fscanf(file, "%lf", goodTransVects + currImage * 3 + i);
+ }
+
+ calibFlags =
+ //CV_CALIB_FIX_PRINCIPAL_POINT +
+ //CV_CALIB_ZERO_TANGENT_DIST +
+ //CV_CALIB_FIX_ASPECT_RATIO +
+ //CV_CALIB_USE_INTRINSIC_GUESS +
+ 0;
+ memset( cameraMatrix, 0, 9*sizeof(cameraMatrix[0]) );
+ cameraMatrix[0] = cameraMatrix[4] = 807.;
+ cameraMatrix[2] = (imageSize.width - 1)*0.5;
+ cameraMatrix[5] = (imageSize.height - 1)*0.5;
+ cameraMatrix[8] = 1.;
+
+ /* Now we can calibrate camera */
+ cvCalibrateCamera_64d( numImages,
+ numbers,
+ imageSize,
+ imagePoints,
+ objectPoints,
+ distortion,
+ cameraMatrix,
+ transVects,
+ rotMatrs,
+ calibFlags );
+
+ /* ---- Reproject points to the image ---- */
+ for( currImage = 0; currImage < numImages; currImage++ )
+ {
+ int numPoints = etalonSize.width * etalonSize.height;
+ cvProjectPointsSimple( numPoints,
+ objectPoints + currImage * numPoints,
+ rotMatrs + currImage * 9,
+ transVects + currImage * 3,
+ cameraMatrix,
+ distortion,
+ reprojectPoints + currImage * numPoints);
+ }
+
+
+ /* ----- Compute reprojection error ----- */
+ i = 0;
+ double dx,dy;
+ double rx,ry;
+ double meanDx,meanDy;
+ double maxDx = 0.0;
+ double maxDy = 0.0;
+
+ meanDx = 0;
+ meanDy = 0;
+ for( currImage = 0; currImage < numImages; currImage++ )
+ {
+ for( currPoint = 0; currPoint < etalonSize.width * etalonSize.height; currPoint++ )
+ {
+ rx = reprojectPoints[i].x;
+ ry = reprojectPoints[i].y;
+ dx = rx - imagePoints[i].x;
+ dy = ry - imagePoints[i].y;
+
+ meanDx += dx;
+ meanDy += dy;
+
+ dx = fabs(dx);
+ dy = fabs(dy);
+
+ if( dx > maxDx )
+ maxDx = dx;
+
+ if( dy > maxDy )
+ maxDy = dy;
+ i++;
+ }
+ }
+
+ meanDx /= numImages * etalonSize.width * etalonSize.height;
+ meanDy /= numImages * etalonSize.width * etalonSize.height;
+
+ /* ========= Compare parameters ========= */
+
+ /* ----- Compare focal lengths ----- */
+ code = compare(cameraMatrix+0,&goodFcx,1,0.01,"fx");
+ if( code < 0 )
+ goto _exit_;
+
+ code = compare(cameraMatrix+4,&goodFcy,1,0.01,"fy");
+ if( code < 0 )
+ goto _exit_;
+
+ /* ----- Compare principal points ----- */
+ code = compare(cameraMatrix+2,&goodCx,1,0.01,"cx");
+ if( code < 0 )
+ goto _exit_;
+
+ code = compare(cameraMatrix+5,&goodCy,1,0.01,"cy");
+ if( code < 0 )
+ goto _exit_;
+
+ /* ----- Compare distortion ----- */
+ code = compare(distortion,goodDistortion,4,0.01,"[k1,k2,p1,p2]");
+ if( code < 0 )
+ goto _exit_;
+
+ /* ----- Compare rot matrixs ----- */
+ code = compare(rotMatrs,goodRotMatrs, 9*numImages,0.05,"rotation matrices");
+ if( code < 0 )
+ goto _exit_;
+
+ /* ----- Compare rot matrixs ----- */
+ code = compare(transVects,goodTransVects, 3*numImages,0.05,"translation vectors");
+ if( code < 0 )
+ goto _exit_;
+
+ if( maxDx > 1.0 )
+ {
+ ts->printf( CvTS::LOG,
+ "Error in reprojection maxDx=%f > 1.0\n",maxDx);
+ code = CvTS::FAIL_BAD_ACCURACY; goto _exit_;
+ }
+
+ if( maxDy > 1.0 )
+ {
+ ts->printf( CvTS::LOG,
+ "Error in reprojection maxDy=%f > 1.0\n",maxDy);
+ code = CvTS::FAIL_BAD_ACCURACY; goto _exit_;
+ }
+
+ progress = update_progress( progress, currTest, numTests, 0 );
+
+ cvFree(&imagePoints);
+ cvFree(&objectPoints);
+ cvFree(&reprojectPoints);
+ cvFree(&numbers);
+
+ cvFree(&transVects);
+ cvFree(&rotMatrs);
+ cvFree(&goodTransVects);
+ cvFree(&goodRotMatrs);
+
+ fclose(file);
+ file = 0;
+ }
+
+_exit_:
+
+ if( file )
+ fclose(file);
+
+ if( datafile )
+ fclose(datafile);
+
+ /* Free all allocated memory */
+ cvFree(&imagePoints);
+ cvFree(&objectPoints);
+ cvFree(&reprojectPoints);
+ cvFree(&numbers);
+
+ cvFree(&transVects);
+ cvFree(&rotMatrs);
+ cvFree(&goodTransVects);
+ cvFree(&goodRotMatrs);
+
+ if( code < 0 )
+ ts->set_failed_test_info( code );
+}
+
+//CV_BundleAdjustmentTest bundleadjustment_test;
projects / opencv / blobdiff