Skip to content

O
opencv_contrib
Commit 9011368a authored by Vadim Pisarevsky's avatar Vadim Pisarevsky
Browse files
Options

Merge pull request #266 from jiuerbujie:omnidir_camera_calib

parents cdbe3352 bf9dd72f
Hide whitespace changes
Inline Side-by-side
Showing with 6534 additions and 43 deletions
modules/ccalib/README.md
View file @ 9011368a
Custom Calibration Pattern for 3D reconstruction
================================================
1. Custom calibration pattern for 3D reconstruction
2. Omnidirectional camera calibration
3. random pattern calibration object
4. multi-camera calibration
\ No newline at end of file
modules/ccalib/include/opencv2/ccalib/multicalib.hpp 0 → 100644
View file @ 9011368a
/*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) 2015, Baisheng Lai (laibaisheng@gmail.com), Zhejiang University,
// all rights reserved.
//
// 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 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*/
#ifndef __OPENCV_MULTICAMERACALIBRATION_HPP__
#define __OPENCV_MULTICAMERACALIBRATION_HPP__
#include "opencv2/ccalib/randpattern.hpp"
#include "opencv2/ccalib/omnidir.hpp"
#include <string>
#include <iostream>
namespace cv { namespace multicalib {
//! @addtogroup ccalib
//! @{
#define HEAD -1
#define INVALID -2
/** @brief Class for multiple camera calibration that supports pinhole camera and omnidirection camera.
For omnidirectional camera model, please refer to omnidir.hpp in ccalib module.
It first calibrate each camera individually, then a bundle adjustment like optimization is applied to
refine extrinsic parameters. So far, it only support "random" pattern for calibration,
see randomPattern.hpp in ccalib module for details.
Images that are used should be named by "cameraIdx-timestamp.*", several images with the same timestamp
means that they are the same pattern that are photographed. cameraIdx should start from 0.
For more details, please refer to paper
B. Li, L. Heng, K. Kevin and M. Pollefeys, "A Multiple-Camera System
Calibration Toolbox Using A Feature Descriptor-Based Calibration
Pattern", in IROS 2013.
*/
class CV_EXPORTS MultiCameraCalibration
{
public:
enum {
PINHOLE,
OMNIDIRECTIONAL
//FISHEYE
};
// an edge connects a camera and pattern
struct edge
{
int cameraVertex; // vertex index for camera in this edge
int photoVertex; // vertex index for pattern in this edge
int photoIndex; // photo index among photos for this camera
Mat transform; // transform from pattern to camera
edge(int cv, int pv, int pi, Mat trans)
{
cameraVertex = cv;
photoVertex = pv;
photoIndex = pi;
transform = trans;
}
};
struct vertex
{
Mat pose; // relative pose to the first camera. For camera vertex, it is the
// transform from the first camera to this camera, for pattern vertex,
// it is the transform from pattern to the first camera
int timestamp; // timestamp of photo, only available for photo vertex
vertex(Mat po, int ts)
{
pose = po;
timestamp = ts;
}
vertex()
{
pose = Mat::eye(4, 4, CV_32F);
timestamp = -1;
}
};
/* @brief Constructor
@param cameraType camera type, PINHOLE or OMNIDIRECTIONAL
@param nCameras number of cameras
@fileName filename of string list that are used for calibration, the file is generated
by imagelist_creator from OpenCv samples. The first one in the list is the pattern filename.
@patternWidth the physical width of pattern, in user defined unit.
@patternHeight the physical height of pattern, in user defined unit.
@showExtration whether show extracted features and feature filtering.
@nMiniMatches minimal number of matched features for a frame.
@flags Calibration flags
@criteria optimization stopping criteria.
@detector feature detector that detect feature points in pattern and images.
@descriptor feature descriptor.
@matcher feature matcher.
*/
MultiCameraCalibration(int cameraType, int nCameras, const std::string& fileName, float patternWidth,
float patternHeight, int verbose = 0, int showExtration = 0, int nMiniMatches = 20, int flags = 0,
TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 200, 1e-7),
Ptr<FeatureDetector> detector = AKAZE::create(AKAZE::DESCRIPTOR_MLDB, 0, 3, 0.006f),
Ptr<DescriptorExtractor> descriptor = AKAZE::create(AKAZE::DESCRIPTOR_MLDB,0, 3, 0.006f),
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-L1"));
/* @brief load images
*/
void loadImages();
/* @brief initialize multiple camera calibration. It calibrates each camera individually.
*/
void initialize();
/* @brief optimization extrinsic parameters
*/
double optimizeExtrinsics();
/* @brief run multi-camera camera calibration, it runs loadImage(), initialize() and optimizeExtrinsics()
*/
double run();
/* @brief write camera parameters to file.
*/
void writeParameters(const std::string& filename);
private:
std::vector<std::string> readStringList();
int getPhotoVertex(int timestamp);
void graphTraverse(const Mat& G, int begin, std::vector<int>& order, std::vector<int>& pre);
void findRowNonZero(const Mat& row, Mat& idx);
void computeJacobianExtrinsic(const Mat& extrinsicParams, Mat& JTJ_inv, Mat& JTE);
void computePhotoCameraJacobian(const Mat& rvecPhoto, const Mat& tvecPhoto, const Mat& rvecCamera,
const Mat& tvecCamera, Mat& rvecTran, Mat& tvecTran, const Mat& objectPoints, const Mat& imagePoints, const Mat& K,
const Mat& distort, const Mat& xi, Mat& jacobianPhoto, Mat& jacobianCamera, Mat& E);
void compose_motion(InputArray _om1, InputArray _T1, InputArray _om2, InputArray _T2, Mat& om3, Mat& T3, Mat& dom3dom1,
Mat& dom3dT1, Mat& dom3dom2, Mat& dom3dT2, Mat& dT3dom1, Mat& dT3dT1, Mat& dT3dom2, Mat& dT3dT2);
void JRodriguesMatlab(const Mat& src, Mat& dst);
void dAB(InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB);
double computeProjectError(Mat& parameters);
void vector2parameters(const Mat& parameters, std::vector<Vec3f>& rvecVertex, std::vector<Vec3f>& tvecVertexs);
void parameters2vector(const std::vector<Vec3f>& rvecVertex, const std::vector<Vec3f>& tvecVertex, Mat& parameters);
int _camType; //PINHOLE, FISHEYE or OMNIDIRECTIONAL
int _nCamera;
int _nMiniMatches;
int _flags;
int _verbose;
double _error;
float _patternWidth, _patternHeight;
TermCriteria _criteria;
std::string _filename;
int _showExtraction;
Ptr<FeatureDetector> _detector;
Ptr<DescriptorExtractor> _descriptor;
Ptr<DescriptorMatcher> _matcher;
std::vector<edge> _edgeList;
std::vector<vertex> _vertexList;
std::vector<std::vector<cv::Mat> > _objectPointsForEachCamera;
std::vector<std::vector<cv::Mat> > _imagePointsForEachCamera;
std::vector<cv::Mat> _cameraMatrix;
std::vector<cv::Mat> _distortCoeffs;
std::vector<cv::Mat> _xi;
std::vector<std::vector<Mat> > _omEachCamera, _tEachCamera;
};
//! @}
}} // namespace multicalib, cv
#endif
\ No newline at end of file
modules/ccalib/include/opencv2/ccalib/omnidir.hpp 0 → 100644
View file @ 9011368a
/*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) 2015, Baisheng Lai (laibaisheng@gmail.com), Zhejiang University,
// all rights reserved.
//
// 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 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 <opencv2/core.hpp>
#include <vector>
#ifndef __OPENCV_OMNIDIR_HPP__
#define __OPENCV_OMNIDIR_HPP__
namespace cv
{
namespace omnidir
{
//! @addtogroup ccalib
//! @{
enum {
CALIB_USE_GUESS = 1,
CALIB_FIX_SKEW = 2,
CALIB_FIX_K1 = 4,
CALIB_FIX_K2 = 8,
CALIB_FIX_P1 = 16,
CALIB_FIX_P2 = 32,
CALIB_FIX_XI = 64,
CALIB_FIX_GAMMA = 128,
CALIB_FIX_CENTER = 256
};
enum{
RECTIFY_PERSPECTIVE = 1,
RECTIFY_CYLINDRICAL = 2,
RECTIFY_LONGLATI = 3,
RECTIFY_STEREOGRAPHIC = 4
};
enum{
XYZRGB = 1,
XYZ = 2
};
/**
* This module was accepted as a GSoC 2015 project for OpenCV, authored by
* Baisheng Lai, mentored by Bo Li.
*/
/** @brief Projects points for omnidirectional camera using CMei's model
@param objectPoints Object points in world coordinate, vector of vector of Vec3f or Mat of
1xN/Nx1 3-channel of type CV_32F and N is the number of points. 64F is also acceptable.
@param imagePoints Output array of image points, vector of vector of Vec2f or
1xN/Nx1 2-channel of type CV_32F. 64F is also acceptable.
@param rvec vector of rotation between world coordinate and camera coordinate, i.e., om
@param tvec vector of translation between pattern coordinate and camera coordinate
@param K Camera matrix \f$K = \vecthreethree{f_x}{s}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$.
@param D Input vector of distortion coefficients \f$(k_1, k_2, p_1, p_2)\f$.
@param xi The parameter xi for CMei's model
@param jacobian Optional output 2Nx16 of type CV_64F jacobian matrix, contains the derivatives of
image pixel points wrt parameters including \f$om, T, f_x, f_y, s, c_x, c_y, xi, k_1, k_2, p_1, p_2\f$.
This matrix will be used in calibration by optimization.
The function projects object 3D points of world coordinate to image pixels, parameter by intrinsic
and extrinsic parameters. Also, it optionally compute a by-product: the jacobian matrix containing
contains the derivatives of image pixel points wrt intrinsic and extrinsic parameters.
*/
CV_EXPORTS_W void projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray rvec, InputArray tvec,
InputArray K, double xi, InputArray D, OutputArray jacobian = noArray());
/** @brief Undistort 2D image points for omnidirectional camera using CMei's model
@param distorted Array of distorted image points, vector of Vec2f
or 1xN/Nx1 2-channel Mat of type CV_32F, 64F depth is also acceptable
@param K Camera matrix \f$K = \vecthreethree{f_x}{s}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$.
@param D Distortion coefficients \f$(k_1, k_2, p_1, p_2)\f$.
@param xi The parameter xi for CMei's model
@param R Rotation trainsform between the original and object space : 3x3 1-channel, or vector: 3x1/1x3
1-channel or 1x1 3-channel
@param undistorted array of normalized object points, vector of Vec2f/Vec2d or 1xN/Nx1 2-channel Mat with the same
depth of distorted points.
*/
CV_EXPORTS_W void undistortPoints(InputArray distorted, OutputArray undistorted, InputArray K, InputArray D, InputArray xi, InputArray R);
/** @brief Computes undistortion and rectification maps for omnidirectional camera image transform by a rotation R.
It output two maps that are used for cv::remap(). If D is empty then zero distortion is used,
if R or P is empty then identity matrices are used.
@param K Camera matrix \f$K = \vecthreethree{f_x}{s}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$, with depth CV_32F or CV_64F
@param D Input vector of distortion coefficients \f$(k_1, k_2, p_1, p_2)\f$, with depth CV_32F or CV_64F
@param xi The parameter xi for CMei's model
@param R Rotation transform between the original and object space : 3x3 1-channel, or vector: 3x1/1x3, with depth CV_32F or CV_64F
@param P New camera matrix (3x3) or new projection matrix (3x4)
@param size Undistorted image size.
@param mltype Type of the first output map that can be CV_32FC1 or CV_16SC2 . See convertMaps()
for details.
@param map1 The first output map.
@param map2 The second output map.
@param flags Flags indicates the rectification type, RECTIFY_PERSPECTIVE, RECTIFY_CYLINDRICAL, RECTIFY_LONGLATI and RECTIFY_STEREOGRAPHIC
are supported.
*/
CV_EXPORTS_W void initUndistortRectifyMap(InputArray K, InputArray D, InputArray xi, InputArray R, InputArray P, const cv::Size& size,
int mltype, OutputArray map1, OutputArray map2, int flags);
/** @brief Undistort omnidirectional images to perspective images
@param distorted The input omnidirectional image.
@param undistorted The output undistorted image.
@param K Camera matrix \f$K = \vecthreethree{f_x}{s}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$.
@param D Input vector of distortion coefficients \f$(k_1, k_2, p_1, p_2)\f$.
@param xi The parameter xi for CMei's model.
@param flags Flags indicates the rectification type, RECTIFY_PERSPECTIVE, RECTIFY_CYLINDRICAL, RECTIFY_LONGLATI and RECTIFY_STEREOGRAPHIC
@param Knew Camera matrix of the distorted image. If it is not assigned, it is just K.
@param new_size The new image size. By default, it is the size of distorted.
@param R Rotation matrix between the input and output images. By default, it is identity matrix.
*/
CV_EXPORTS_W void undistortImage(InputArray distorted, OutputArray undistorted, InputArray K, InputArray D, InputArray xi, int flags,
InputArray Knew = cv::noArray(), const Size& new_size = Size(), InputArray R = Mat::eye(3, 3, CV_64F));
/** @brief Perform omnidirectional camera calibration, the default depth of outputs is CV_64F.
@param objectPoints Vector of vector of Vec3f object points in world (pattern) coordinate.
It also can be vector of Mat with size 1xN/Nx1 and type CV_32FC3. Data with depth of 64_F is also acceptable.
@param imagePoints Vector of vector of Vec2f corresponding image points of objectPoints. It must be the same
size and the same type with objectPoints.
@param size Image size of calibration images.
@param K Output calibrated camera matrix.
@param xi Output parameter xi for CMei's model
@param D Output distortion parameters \f$(k_1, k_2, p_1, p_2)\f$
@param rvecs Output rotations for each calibration images
@param tvecs Output translation for each calibration images
@param flags The flags that control calibrate
@param criteria Termination criteria for optimization
@param idx Indices of images that pass initialization, which are really used in calibration. So the size of rvecs is the
same as idx.total().
*/
CV_EXPORTS_W double calibrate(InputArray objectPoints, InputArray imagePoints, Size size,
InputOutputArray K, InputOutputArray xi, InputOutputArray D, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
int flags, TermCriteria criteria, OutputArray idx=noArray());
/** @brief Stereo calibration for omnidirectional camera model. It computes the intrinsic parameters for two
cameras and the extrinsic parameters between two cameras. The default depth of outputs is CV_64F.
@param objectPoints Object points in world (pattern) coordinate. Its type is vector<vector<Vec3f> >.
It also can be vector of Mat with size 1xN/Nx1 and type CV_32FC3. Data with depth of 64_F is also acceptable.
@param imagePoints1 The corresponding image points of the first camera, with type vector<vector<Vec2f> >.
It must be the same size and the same type as objectPoints.
@param imagePoints2 The corresponding image points of the second camera, with type vector<vector<Vec2f> >.
It must be the same size and the same type as objectPoints.
@param imageSize1 Image size of calibration images of the first camera.
@param imageSize2 Image size of calibration images of the second camera.
@param K1 Output camera matrix for the first camera.
@param xi1 Output parameter xi of Mei's model for the first camera
@param D1 Output distortion parameters \f$(k_1, k_2, p_1, p_2)\f$ for the first camera
@param K2 Output camera matrix for the first camera.
@param xi2 Output parameter xi of CMei's model for the second camera
@param D2 Output distortion parameters \f$(k_1, k_2, p_1, p_2)\f$ for the second camera
@param rvec Output rotation between the first and second camera
@param tvec Output translation between the first and second camera
@param rvecsL Output rotation for each image of the first camera
@param tvecsL Output translation for each image of the first camera
@param flags The flags that control stereoCalibrate
@param criteria Termination criteria for optimization
@param idx Indices of image pairs that pass initialization, which are really used in calibration. So the size of rvecs is the
same as idx.total().
@
*/
CV_EXPORTS_W double stereoCalibrate(InputOutputArrayOfArrays objectPoints, InputOutputArrayOfArrays imagePoints1, InputOutputArrayOfArrays imagePoints2,
const Size& imageSize1, const Size& imageSize2, InputOutputArray K1, InputOutputArray xi1, InputOutputArray D1, InputOutputArray K2, InputOutputArray xi2,
InputOutputArray D2, OutputArray rvec, OutputArray tvec, OutputArrayOfArrays rvecsL, OutputArrayOfArrays tvecsL, int flags, TermCriteria criteria, OutputArray idx=noArray());
/** @brief Stereo rectification for omnidirectional camera model. It computes the rectification rotations for two cameras
@param R Rotation between the first and second camera
@param T Translation between the first and second camera
@param R1 Output 3x3 rotation matrix for the first camera
@param R2 Output 3x3 rotation matrix for the second camera
*/
CV_EXPORTS_W void stereoRectify(InputArray R, InputArray T, OutputArray R1, OutputArray R2);
/** @brief Stereo 3D reconstruction from a pair of images
@param image1 The first input image
@param image2 The second input image
@param K1 Input camera matrix of the first camera
@param D1 Input distortion parameters \f$(k_1, k_2, p_1, p_2)\f$ for the first camera
@param xi1 Input parameter xi for the first camera for CMei's model
@param K2 Input camera matrix of the second camera
@param D2 Input distortion parameters \f$(k_1, k_2, p_1, p_2)\f$ for the second camera
@param xi2 Input parameter xi for the second camera for CMei's model
@param R Rotation between the first and second camera
@param T Translation between the first and second camera
@param flag Flag of rectification type, RECTIFY_PERSPECTIVE or RECTIFY_LONGLATI
@param numDisparities The parameter 'numDisparities' in StereoSGBM, see StereoSGBM for details.
@param SADWindowSize The parameter 'SADWindowSize' in StereoSGBM, see StereoSGBM for details.
@param disparity Disparity map generated by stereo matching
@param image1Rec Rectified image of the first image
@param image2Rec rectified image of the second image
@param newSize Image size of rectified image, see omnidir::undistortImage
@param Knew New camera matrix of rectified image, see omnidir::undistortImage
@param pointCloud Point cloud of 3D reconstruction, with type CV_64FC3
@param pointType Point cloud type, it can be XYZRGB or XYZ
*/
CV_EXPORTS_W void stereoReconstruct(InputArray image1, InputArray image2, InputArray K1, InputArray D1, InputArray xi1,
InputArray K2, InputArray D2, InputArray xi2, InputArray R, InputArray T, int flag, int numDisparities, int SADWindowSize,
OutputArray disparity, OutputArray image1Rec, OutputArray image2Rec, const Size& newSize = Size(), InputArray Knew = cv::noArray(),
OutputArray pointCloud = cv::noArray(), int pointType = XYZRGB);
namespace internal
{
void initializeCalibration(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, Size size, OutputArrayOfArrays omAll,
OutputArrayOfArrays tAll, OutputArray K, double& xi, OutputArray idx = noArray());
void initializeStereoCalibration(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
const Size& size1, const Size& size2, OutputArray om, OutputArray T, OutputArrayOfArrays omL, OutputArrayOfArrays tL, OutputArray K1, OutputArray D1, OutputArray K2, OutputArray D2,
double &xi1, double &xi2, int flags, OutputArray idx);
void computeJacobian(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags,
double epsilon);
void computeJacobianStereo(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags, double epsilon);
void encodeParameters(InputArray K, InputArrayOfArrays omAll, InputArrayOfArrays tAll, InputArray distoaration, double xi, OutputArray parameters);
void encodeParametersStereo(InputArray K1, InputArray K2, InputArray om, InputArray T, InputArrayOfArrays omL, InputArrayOfArrays tL,
InputArray D1, InputArray D2, double xi1, double xi2, OutputArray parameters);
void decodeParameters(InputArray paramsters, OutputArray K, OutputArrayOfArrays omAll, OutputArrayOfArrays tAll, OutputArray distoration, double& xi);
void decodeParametersStereo(InputArray parameters, OutputArray K1, OutputArray K2, OutputArray om, OutputArray T, OutputArrayOfArrays omL,
OutputArrayOfArrays tL, OutputArray D1, OutputArray D2, double& xi1, double& xi2);
void estimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray parameters, Mat& errors, Vec2d& std_error, double& rms, int flags);
void estimateUncertaintiesStereo(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, InputArray parameters, Mat& errors,
Vec2d& std_error, double& rms, int flags);
double computeMeanReproErr(InputArrayOfArrays imagePoints, InputArrayOfArrays proImagePoints);
double computeMeanReproErr(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray K, InputArray D, double xi, InputArrayOfArrays omAll,
InputArrayOfArrays tAll);
double computeMeanReproErrStereo(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, InputArray K1, InputArray K2,
InputArray D1, InputArray D2, double xi1, double xi2, InputArray om, InputArray T, InputArrayOfArrays omL, InputArrayOfArrays TL);
void checkFixed(Mat &G, int flags, int n);
void subMatrix(const Mat& src, Mat& dst, const std::vector<int>& cols, const std::vector<int>& rows);
void flags2idx(int flags, std::vector<int>& idx, int n);
void flags2idxStereo(int flags, std::vector<int>& idx, int n);
void fillFixed(Mat&G, int flags, int n);
void fillFixedStereo(Mat& G, int flags, int n);
double findMedian(const Mat& row);
Vec3d findMedian3(InputArray mat);
void getInterset(InputArray idx1, InputArray idx2, OutputArray inter1, OutputArray inter2, OutputArray inter_ori);
void compose_motion(InputArray _om1, InputArray _T1, InputArray _om2, InputArray _T2, Mat& om3, Mat& T3, Mat& dom3dom1,
Mat& dom3dT1, Mat& dom3dom2, Mat& dom3dT2, Mat& dT3dom1, Mat& dT3dT1, Mat& dT3dom2, Mat& dT3dT2);
//void JRodriguesMatlab(const Mat& src, Mat& dst);
//void dAB(InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB);
} // internal
//! @}
} // omnidir
} //cv
#endif
\ No newline at end of file
modules/ccalib/include/opencv2/ccalib/randpattern.hpp 0 → 100644
View file @ 9011368a
/*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) 2015, Baisheng Lai (laibaisheng@gmail.com), Zhejiang University,
// all rights reserved.
//
// 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 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*/
#ifndef __OPENCV_RANDOMPATTERN_HPP__
#define __OPENCV_RANDOMPATTERN_HPP__
#include "opencv2/features2d.hpp"
#include "opencv2/highgui.hpp"
namespace cv { namespace randpattern {
//! @addtogroup ccalib
//! @{
/** @brief Class for finding features points and corresponding 3D in world coordinate of
a "random" pattern, which can be to be used in calibration. It is useful when pattern is
partly occluded or only a part of pattern can be observed in multiple cameras calibration.
The pattern can be generated by RandomPatternGenerator class described in this file.
Please refer to paper
B. Li, L. Heng, K. Kevin and M. Pollefeys, "A Multiple-Camera System
Calibration Toolbox Using A Feature Descriptor-Based Calibration
Pattern", in IROS 2013.
*/
class CV_EXPORTS RandomPatternCornerFinder
{
public:
/* @brief Construct RandomPatternCornerFinder object
@param patternWidth the real width of "random" pattern in a user defined unit.
@param patternHeight the real height of "random" pattern in a user defined unit.
@param nMiniMatch number of minimal matches, otherwise that image is abandoned
@depth depth of output objectPoints and imagePoints, set it to be CV_32F or CV_64F.
@showExtraction whether show feature extraction, 0 for no and 1 for yes.
@detector feature detector to detect feature points in pattern and images.
@descriptor feature descriptor.
@matcher feature matcher.
*/
RandomPatternCornerFinder(float patternWidth, float patternHeight,
int nminiMatch = 20, int depth = CV_32F, int verbose = 0, int showExtraction = 0,
Ptr<FeatureDetector> detector = AKAZE::create(AKAZE::DESCRIPTOR_MLDB, 0, 3, 0.005f),
Ptr<DescriptorExtractor> descriptor = AKAZE::create(AKAZE::DESCRIPTOR_MLDB,0, 3, 0.005f),
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-L1"));
/* @brief Load pattern image and compute features for pattern
@param patternImage image for "random" pattern generated by RandomPatternGenerator, run it first.
*/
void loadPattern(cv::Mat patternImage);
/* @brief Compute matched object points and image points which are used for calibration
The objectPoints (3D) and imagePoints (2D) are stored inside the class. Run getObjectPoints()
and getImagePoints() to get them.
@param inputImages vector of 8-bit grayscale images containing "random" pattern
that are used for calibration.
*/
void computeObjectImagePoints(std::vector<cv::Mat> inputImages);
//void computeObjectImagePoints2(std::vector<cv::Mat> inputImages);
/* @brief Compute object and image points for a single image. It returns a vector<Mat> that
the first element stores the imagePoints and the second one stores the objectPoints.
@param inputImage single input image for calibration
*/
std::vector<cv::Mat> computeObjectImagePointsForSingle(cv::Mat inputImage);
/* @brief Get object(3D) points
*/
std::vector<cv::Mat> getObjectPoints();
/* @brief and image(2D) points
*/
std::vector<cv::Mat> getImagePoints();
private:
std::vector<cv::Mat> _objectPonits, _imagePoints;
float _patternWidth, _patternHeight;
cv::Size _patternImageSize;
int _nminiMatch;
int _depth;
int _verbose;
Ptr<FeatureDetector> _detector;
Ptr<DescriptorExtractor> _descriptor;
Ptr<DescriptorMatcher> _matcher;
Mat _descriptorPattern;
std::vector<cv::KeyPoint> _keypointsPattern;
Mat _patternImage;
int _showExtraction;
void keyPoints2MatchedLocation(const std::vector<cv::KeyPoint>& imageKeypoints,
const std::vector<cv::KeyPoint>& patternKeypoints, const std::vector<cv::DMatch> matchces,
cv::Mat& matchedImagelocation, cv::Mat& matchedPatternLocation);
void getFilteredLocation(cv::Mat& imageKeypoints, cv::Mat& patternKeypoints, const cv::Mat mask);
void getObjectImagePoints(const cv::Mat& imageKeypoints, const cv::Mat& patternKeypoints);
void crossCheckMatching( cv::Ptr<DescriptorMatcher>& descriptorMatcher,
const Mat& descriptors1, const Mat& descriptors2,
std::vector<DMatch>& filteredMatches12, int knn=1 );
void drawCorrespondence(const Mat& image1, const std::vector<cv::KeyPoint> keypoint1,
const Mat& image2, const std::vector<cv::KeyPoint> keypoint2, const std::vector<cv::DMatch> matchces,
const Mat& mask1, const Mat& mask2, const int step);
};
/* @brief Class to generate "random" pattern image that are used for RandomPatternCornerFinder
Please refer to paper
B. Li, L. Heng, K. Kevin and M. Pollefeys, "A Multiple-Camera System
Calibration Toolbox Using A Feature Descriptor-Based Calibration
Pattern", in IROS 2013.
*/
class CV_EXPORTS RandomPatternGenerator
{
public:
/* @brief Construct RandomPatternGenerator
@param imageWidth image width of the generated pattern image
@param imageHeight image height of the generated pattern image
*/
RandomPatternGenerator(int imageWidth, int imageHeight);
/* @brief Generate pattern
*/
void generatePattern();
/* @brief Get pattern
*/
cv::Mat getPattern();
private:
cv::Mat _pattern;
int _imageWidth, _imageHeight;
};
//! @}
}} //namespace randpattern, cv
#endif
\ No newline at end of file
modules/ccalib/samples/multi_cameras_calibration.cpp 0 → 100644
View file @ 9011368a
#include "opencv2/ccalib/omnidir.hpp"
#include "opencv2/ccalib/multicalib.hpp"
#include "opencv2/ccalib/randpattern.hpp"
using namespace std;
using namespace cv;
const char * usage =
"\n example command line for multi-camera calibration by using random pattern \n"
" multi_cameras_calibration -nc 5 -pw 800 -ph 600 -ct 1 -fe 0 -nm 25 -v 0 multi_camera_omnidir.xml \n"
"\n"
" the file multi_camera_omnidir.xml is generated by imagelist_creator as \n"
" imagelist_creator multi_camera_omnidir.xml *.* \n"
" note the first filename in multi_camera_omnidir.xml is the pattern, the rest are photo names,\n"
" photo names should be in form of cameraIdx-timestamp.*, and cameraIdx starts from 0";
static void help()
{
printf("\n This is a sample for multi-camera calibration, so far it only support random pattern,\n"
"see randomPattern.hpp for detail. Pinhole and omnidirectional cameras are both supported, \n"
"for omnidirectional camera, see omnidir.hpp for detail.\n"
"Usage: mutiCamCalib \n"
" -nc <num_camera> # number of cameras \n"
" -pw <pattern_width> # physical width of random pattern \n"
" -ph <pattern_height> # physical height of random pattern \n"
" -ct <camera_type> # camera type, 0 for pinhole and 1 for omnidirectional \n"
" -fe # whether show feature extraction\n"
" -nm # number of minimal matches of an image \n"
" -v # whether show verbose information \n"
" input_data # text file with pattern file names and a list of photo names, the file is generated by imagelist_creator \n");
printf("\n %s", usage);
}
int main(int argc, char** argv)
{
float patternWidth = 0.0f, patternHeight = 0.0f;
int nCamera = 0, nMiniMatches = 0, cameraType = 0;
const char* outputFilename = "multi-camera-results.xml";
const char* inputFilename = 0;
int showFeatureExtraction = 0, verbose = 0;
if (argc < 2)
{
help();
return 1;
}
for (int i = 1; i < argc; ++i)
{
const char* s = argv[i];
if (strcmp( s, "-nc") == 0)
{
if (sscanf( argv[++i], "%u", &nCamera) != 1 || nCamera <= 0)
{
return fprintf(stderr, "Invalid number of cameras \n"), -1;
}
}
else if ( strcmp( s, "-pw" ) == 0 )
{
if (sscanf( argv[++i], "%f", &patternWidth) != 1 || patternWidth <=0 )
{
return fprintf(stderr, "Invalid pattern width \n"), -1;
}
}
else if ( strcmp( s, "-ph" ) == 0 )
{
if (sscanf( argv[++i], "%f", &patternHeight) != 1 || patternHeight <=0 )
{
return fprintf(stderr, "Invalid pattern height \n"), -1;
}
}
else if ( strcmp( s, "-ct" ) == 0 )
{
if (sscanf( argv[++i], "%u", &cameraType) != 1 || (cameraType !=0 && cameraType !=1 && cameraType !=2) )
{
return fprintf(stderr, "Invalid camera type, 0 for pinhole and 1 for omnidirectional \n"), -1;
}
}
else if ( strcmp( s, "-fe" ) == 0 )
{
if (sscanf( argv[++i], "%u", &showFeatureExtraction) != 1 || (showFeatureExtraction !=1 && showFeatureExtraction !=0) )
{
return fprintf(stderr, "Not bool value, set to 0 or 1 \n"), -1;
}
}
else if ( strcmp( s, "-nm" ) == 0 )
{
if (sscanf( argv[++i], "%u", &nMiniMatches) != 1 || nMiniMatches <=0 )
{
return fprintf(stderr, "Invalid number of minimal matches \n"), -1;
}
}
else if ( strcmp( s, "-v" ) == 0 )
{
if (sscanf( argv[++i], "%u", &verbose) != 1 || (verbose !=1 && verbose !=0) )
{
return fprintf(stderr, "verbose is not bool value, set to 0 or 1 \n"), -1;
}
}
else if( s[0] != '-')
{
inputFilename = s;
}
else
{
return fprintf( stderr, "Unknown option %s\n", s ), -1;
}
}
// do multi-camera calibration
multicalib::MultiCameraCalibration multiCalib(cameraType, nCamera, inputFilename, patternWidth, patternHeight, verbose, showFeatureExtraction, nMiniMatches);
multiCalib.loadImages();
multiCalib.initialize();
multiCalib.optimizeExtrinsics();
// the above three lines can be replaced by multiCalib.run();
multiCalib.writeParameters(outputFilename);
}
\ No newline at end of file
modules/ccalib/samples/omni_calibration.cpp 0 → 100644
View file @ 9011368a
#include "opencv2/ccalib/omnidir.hpp"
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/highgui.hpp"
#include <vector>
#include <iostream>
#include <string>
#include <time.h>
using namespace cv;
using namespace std;
const char * usage =
"\n example command line for omnidirectional camera calibration.\n"
" omni_calibration -w 6 -h 9 -sw 80 -sh 80 imagelist.xml \n"
" \n"
" the file imagelist.xml is generated by imagelist_creator as\n"
"imagelist_creator imagelist.xml *.*";
static void help()
{
printf("\n This is a sample for omnidirectional camera calibration.\n"
"Usage: omni_calibration\n"
" -w <board_width> # the number of inner corners per one of board dimension\n"
" -h <board_height> # the number of inner corners per another board dimension\n"
" [-sw <square_width>] # the width of square in some user-defined units (1 by default)\n"
" [-sh <square_height>] # the height of square in some user-defined units (1 by default)\n"
" [-o <out_camera_params>] # the output filename for intrinsic [and extrinsic] parameters\n"
" [-fs <fix_skew>] # fix skew\n"
" [-fp ] # fix the principal point at the center\n"
" input_data # input data - text file with a list of the images of the board, which is generated by imagelist_creator"
);
printf("\n %s", usage);
}
static void calcChessboardCorners(Size boardSize, double square_width, double square_height,
Mat& corners)
{
// corners has type of CV_64FC3
corners.release();
int n = boardSize.width * boardSize.height;
corners.create(n, 1, CV_64FC3);
Vec3d *ptr = corners.ptr<Vec3d>();
for (int i = 0; i < boardSize.height; ++i)
{
for (int j = 0; j < boardSize.width; ++j)
{
ptr[i*boardSize.width + j] = Vec3d(double(j * square_width), double(i * square_height), 0.0);
}
}
}
static bool detecChessboardCorners(const vector<string>& list, vector<string>& list_detected,
vector<Mat>& imagePoints, Size boardSize, Size& imageSize)
{
imagePoints.resize(0);
list_detected.resize(0);
int n_img = (int)list.size();
Mat img;
for(int i = 0; i < n_img; ++i)
{
Mat points;
img = imread(list[i], IMREAD_GRAYSCALE);
bool found = findChessboardCorners( img, boardSize, points);
if (found)
{
if (points.type() != CV_64FC2)
points.convertTo(points, CV_64FC2);
imagePoints.push_back(points);
list_detected.push_back(list[i]);
}
}
if (!img.empty())
imageSize = img.size();
if (imagePoints.size() < 3)
return false;
else
return true;
}
static bool readStringList( const string& filename, vector<string>& l )
{
l.resize(0);
FileStorage fs(filename, FileStorage::READ);
if( !fs.isOpened() )
return false;
FileNode n = fs.getFirstTopLevelNode();
if( n.type() != FileNode::SEQ )
return false;
FileNodeIterator it = n.begin(), it_end = n.end();
for( ; it != it_end; ++it )
l.push_back((string)*it);
return true;
}
static void saveCameraParams( const string & filename, int flags, const Mat& cameraMatrix,
const Mat& distCoeffs, const double xi, const vector<Vec3d>& rvecs, const vector<Vec3d>& tvecs,
vector<string> detec_list, const Mat& idx, const double rms, const vector<Mat>& imagePoints)
{
FileStorage fs( filename, FileStorage::WRITE );
time_t tt;
time( &tt );
struct tm *t2 = localtime( &tt );
char buf[1024];
strftime( buf, sizeof(buf)-1, "%c", t2 );
fs << "calibration_time" << buf;
if ( !rvecs.empty())
fs << "nFrames" << (int)rvecs.size();
if ( flags != 0)
{
sprintf( buf, "flags: %s%s%s%s%s%s%s%s%s",
flags & omnidir::CALIB_USE_GUESS ? "+use_intrinsic_guess" : "",
flags & omnidir::CALIB_FIX_SKEW ? "+fix_skew" : "",
flags & omnidir::CALIB_FIX_K1 ? "+fix_k1" : "",
flags & omnidir::CALIB_FIX_K2 ? "+fix_k2" : "",
flags & omnidir::CALIB_FIX_P1 ? "+fix_p1" : "",
flags & omnidir::CALIB_FIX_P2 ? "+fix_p2" : "",
flags & omnidir::CALIB_FIX_XI ? "+fix_xi" : "",
flags & omnidir::CALIB_FIX_GAMMA ? "+fix_gamma" : "",
flags & omnidir::CALIB_FIX_CENTER ? "+fix_center" : "");
//cvWriteComment( *fs, buf, 0 );
}
fs << "flags" << flags;
fs << "camera_matrix" << cameraMatrix;
fs << "distortion_coefficients" << distCoeffs;
fs << "xi" << xi;
//cvWriteComment( *fs, "names of images that are acturally used in calibration", 0 );
fs << "used_imgs" << "[";
for (int i = 0; i < (int)idx.total(); ++i)
{
fs << detec_list[(int)idx.at<int>(i)];
}
fs << "]";
if ( !rvecs.empty() && !tvecs.empty() )
{
Mat rvec_tvec((int)rvecs.size(), 6, CV_64F);
for (int i = 0; i < (int)rvecs.size(); ++i)
{
Mat(rvecs[i]).reshape(1, 1).copyTo(rvec_tvec(Rect(0, i, 3, 1)));
Mat(tvecs[i]).reshape(1, 1).copyTo(rvec_tvec(Rect(3, i, 3, 1)));
}
//cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );
fs << "extrinsic_parameters" << rvec_tvec;
}
fs << "rms" << rms;
if ( !imagePoints.empty() )
{
Mat imageMat((int)imagePoints.size(), (int)imagePoints[0].total(), CV_64FC2);
for (int i = 0; i < (int)imagePoints.size(); ++i)
{
Mat r = imageMat.row(i).reshape(2, imageMat.cols);
Mat imagei(imagePoints[i]);
imagei.copyTo(r);
}
fs << "image_points" << imageMat;
}
}
int main(int argc, char** argv)
{
Size boardSize, imageSize;
int flags = 0;
double square_width = 0.0, square_height = 0.0;
const char* outputFilename = "out_camera_params.xml";
const char* inputFilename = 0;
vector<Mat> objectPoints;
vector<Mat> imagePoints;
if(argc < 2)
{
help();
return 1;
}
for(int i = 1; i < argc; i++)
{
const char* s = argv[i];
if( strcmp( s, "-w") == 0)
{
if( sscanf( argv[++i], "%u", &boardSize.width ) != 1 || boardSize.width <= 0 )
return fprintf( stderr, "Invalid board width\n" ), -1;
}
else if( strcmp( s, "-h" ) == 0 )
{
if( sscanf( argv[++i], "%u", &boardSize.height ) != 1 || boardSize.height <= 0 )
return fprintf( stderr, "Invalid board height\n" ), -1;
}
else if( strcmp( s, "-sw" ) == 0 )
{
if( sscanf( argv[++i], "%lf", &square_width ) != 1 || square_width <= 0 )
return fprintf(stderr, "Invalid square width\n"), -1;
}
else if( strcmp( s, "-sh" ) == 0 )
{
if( sscanf( argv[++i], "%lf", &square_height) != 1 || square_height <= 0 )
return fprintf(stderr, "Invalid square height\n"), -1;
}
else if( strcmp( s, "-o" ) == 0 )
{
outputFilename = argv[++i];
}
else if( strcmp( s, "-fs" ) == 0 )
{
flags |= omnidir::CALIB_FIX_SKEW;
}
else if( strcmp( s, "-fp" ) == 0 )
{
flags |= omnidir::CALIB_FIX_CENTER;
}
else if( s[0] != '-')
{
inputFilename = s;
}
else
{
return fprintf( stderr, "Unknown option %s\n", s ), -1;
}
}
// get image name list
vector<string> image_list, detec_list;
if(!readStringList(inputFilename, image_list))
return fprintf( stderr, "Failed to read image list\n"), -1;
// find corners in images
// some images may be failed in automatic corner detection, passed cases are in detec_list
if(!detecChessboardCorners(image_list, detec_list, imagePoints, boardSize, imageSize))
return fprintf(stderr, "Not enough corner detected images\n"), -1;
// calculate object coordinates
Mat object;
calcChessboardCorners(boardSize, square_width, square_height, object);
for(int i = 0; i < (int)detec_list.size(); ++i)
objectPoints.push_back(object);
// run calibration, some images are discarded in calibration process because they are failed
// in initialization. Retained image indexes are in idx variable.
Mat K, D, xi, idx;
vector<Vec3d> rvecs, tvecs;
double _xi, rms;
TermCriteria criteria(3, 200, 1e-8);
rms = omnidir::calibrate(objectPoints, imagePoints, imageSize, K, xi, D, rvecs, tvecs, flags, criteria, idx);
_xi = xi.at<double>(0);
saveCameraParams(outputFilename, flags, K, D, _xi,
rvecs, tvecs, detec_list, idx, rms, imagePoints);
}
modules/ccalib/samples/omni_stereo_calibration.cpp 0 → 100644
View file @ 9011368a
#include "opencv2/ccalib/omnidir.hpp"
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/calib3d.hpp"
#include <vector>
#include <iostream>
#include <string>
#include <time.h>
using namespace cv;
using namespace std;
const char * usage =
"\n example command line for calibrate a pair of omnidirectional camera.\n"
" omni_stereo_calibration -w 8 -h 6 -sw 2.4399 -sh 2.4399 imagelist_left.xml imagelist_right.xml\n"
" \n"
" the file image_list_1.xml and image_list_2.xml generated by imagelist_creator as\n"
"imagelist_creator image_list_1.xml *.*";
static void help()
{
printf("\n This is a sample for omnidirectional camera calibration.\n"
"Usage: omni_calibration\n"
" -w <board_width> # the number of inner corners per one of board dimension\n"
" -h <board_height> # the number of inner corners per another board dimension\n"
" [-sw <square_width>] # the width of square in some user-defined units (1 by default)\n"
" [-sh <square_height>] # the height of square in some user-defined units (1 by default)\n"
" [-o <out_camera_params>] # the output filename for intrinsic [and extrinsic] parameters\n"
" [-fs <fix_skew>] # fix skew\n"
" [-fp ] # fix the principal point at the center\n"
" input_data_1 # input data - text file with a list of the images of the first camera, which is generated by imagelist_creator"
" input_data_2 # input data - text file with a list of the images of the second camera, which is generated by imagelist_creator"
);
printf("\n %s", usage);
}
static void calcChessboardCorners(Size boardSize, double square_width, double square_height,
Mat& corners)
{
// corners has type of CV_64FC3
corners.release();
int n = boardSize.width * boardSize.height;
corners.create(n, 1, CV_64FC3);
Vec3d *ptr = corners.ptr<Vec3d>();
for (int i = 0; i < boardSize.height; ++i)
{
for (int j = 0; j < boardSize.width; ++j)
{
ptr[i*boardSize.width + j] = Vec3d(double(j * square_width), double(i * square_height), 0.0);
}
}
}
static bool detecChessboardCorners(const vector<string>& list1, vector<string>& list_detected_1,
const vector<string>& list2, vector<string>& list_detected_2,
vector<Mat>& image_points_1, vector<Mat>& image_points_2, Size boardSize, Size& imageSize1, Size& imageSize2)
{
image_points_1.resize(0);
image_points_2.resize(0);
list_detected_1.resize(0);
list_detected_2.resize(0);
int n_img = (int)list1.size();
Mat img_l, img_r;
for(int i = 0; i < n_img; ++i)
{
Mat points_1, points_2;
img_l = imread(list1[i], IMREAD_GRAYSCALE);
img_r = imread(list2[i], IMREAD_GRAYSCALE);
bool found_l = findChessboardCorners( img_l, boardSize, points_1);
bool found_r = findChessboardCorners( img_r, boardSize, points_2);
if (found_l && found_r)
{
if (points_1.type() != CV_64FC2)
points_1.convertTo(points_1, CV_64FC2);
if (points_2.type() != CV_64FC2)
points_2.convertTo(points_2, CV_64FC2);
image_points_1.push_back(points_1);
image_points_2.push_back(points_2);
list_detected_1.push_back(list1[i]);
list_detected_2.push_back(list2[i]);
}
}
if (!img_l.empty())
imageSize1 = img_l.size();
if (!img_r.empty())
{
imageSize2 = img_r.size();
}
if (image_points_1.size() < 3)
return false;
else
return true;
}
static bool readStringList( const string& filename, vector<string>& l )
{
l.resize(0);
FileStorage fs(filename, FileStorage::READ);
if( !fs.isOpened() )
return false;
FileNode n = fs.getFirstTopLevelNode();
if( n.type() != FileNode::SEQ )
return false;
FileNodeIterator it = n.begin(), it_end = n.end();
for( ; it != it_end; ++it )
l.push_back((string)*it);
return true;
}
static void saveCameraParams( const string & filename, const int flags, const Mat& cameraMatrix1, const Mat& cameraMatrix2, const Mat& distCoeffs1,
const Mat& disCoeffs2, const double xi1, const double xi2, const Vec3d rvec, const Vec3d tvec,
const vector<Vec3d>& rvecs, const vector<Vec3d>& tvecs, vector<string> detec_list_1, vector<string> detec_list_2,
const Mat& idx, const double rms, const vector<Mat>& imagePoints1, const vector<Mat>& imagePoints2)
{
FileStorage fs( filename, FileStorage::WRITE );
time_t tt;
time( &tt );
struct tm *t2 = localtime( &tt );
char buf[1024];
strftime( buf, sizeof(buf)-1, "%c", t2 );
fs << "calibration_time" << buf;
if ( !rvecs.empty())
fs << "nFrames" << (int)rvecs.size();
if ( flags != 0)
{
sprintf( buf, "flags: %s%s%s%s%s%s%s%s%s",
flags & omnidir::CALIB_USE_GUESS ? "+use_intrinsic_guess" : "",
flags & omnidir::CALIB_FIX_SKEW ? "+fix_skew" : "",
flags & omnidir::CALIB_FIX_K1 ? "+fix_k1" : "",
flags & omnidir::CALIB_FIX_K2 ? "+fix_k2" : "",
flags & omnidir::CALIB_FIX_P1 ? "+fix_p1" : "",
flags & omnidir::CALIB_FIX_P2 ? "+fix_p2" : "",
flags & omnidir::CALIB_FIX_XI ? "+fix_xi" : "",
flags & omnidir::CALIB_FIX_GAMMA ? "+fix_gamma" : "",
flags & omnidir::CALIB_FIX_CENTER ? "+fix_center" : "");
//cvWriteComment( *fs, buf, 0 );
}
fs << "flags" << flags;
fs << "camera_matrix_1" << cameraMatrix1;
fs << "distortion_coefficients_1" << distCoeffs1;
fs << "xi_1" << xi1;
fs << "camera_matrix_2" << cameraMatrix2;
fs << "distortion_coefficients_2" << disCoeffs2;
fs << "xi_2" << xi2;
Mat om_t(1, 6, CV_64F);
Mat(rvec).reshape(1, 1).copyTo(om_t.colRange(0, 3));
Mat(tvec).reshape(1, 1).copyTo(om_t.colRange(3, 6));
//cvWriteComment( *fs, "6-tuples (rotation vector + translation vector) for each view", 0 );
fs << "extrinsic_parameters" << om_t;
if ( !rvecs.empty() && !tvecs.empty() )
{
Mat rvec_tvec((int)rvecs.size(), 6, CV_64F);
for (int i = 0; i < (int)rvecs.size(); ++i)
{
Mat(rvecs[i]).reshape(1, 1).copyTo(rvec_tvec(Rect(0, i, 3, 1)));
Mat(tvecs[i]).reshape(1, 1).copyTo(rvec_tvec(Rect(3, i, 3, 1)));
}
//cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );
fs << "extrinsic_parameters_1" << rvec_tvec;
}
fs << "rms" << rms;
//cvWriteComment( *fs, "names of images that are acturally used in calibration", 0 );
fs << "used_imgs_1" << "[";
for (int i = 0; i < (int)idx.total(); ++i)
{
fs << detec_list_1[(int)idx.at<int>(i)];
}
fs << "]";
fs << "used_imgs_2" << "[";
for (int i = 0; i < (int)idx.total(); ++i)
{
fs << detec_list_2[(int)idx.at<int>(i)];
}
fs << "]";
if ( !imagePoints1.empty() )
{
Mat imageMat((int)imagePoints1.size(), (int)imagePoints1[0].total(), CV_64FC2);
for (int i = 0; i < (int)imagePoints1.size(); ++i)
{
Mat r = imageMat.row(i).reshape(2, imageMat.cols);
Mat imagei(imagePoints1[i]);
imagei.copyTo(r);
}
fs << "image_points_1" << imageMat;
}
if ( !imagePoints2.empty() )
{
Mat imageMat((int)imagePoints2.size(), (int)imagePoints2[0].total(), CV_64FC2);
for (int i = 0; i < (int)imagePoints2.size(); ++i)
{
Mat r = imageMat.row(i).reshape(2, imageMat.cols);
Mat imagei(imagePoints2[i]);
imagei.copyTo(r);
}
fs << "image_points_2" << imageMat;
}
}
int main(int argc, char** argv)
{
Size boardSize, imageSize1, imageSize2;
int flags = 0;
double square_width = 0.0, square_height = 0.0;
const char* outputFilename = "out_camera_params_stereo.xml";
const char* inputFilename1 = 0;
const char* inputFilename2 = 0;
vector<Mat> objectPoints;
vector<Mat> imagePoints1;
vector<Mat> imagePoints2;
if(argc < 2)
{
help();
return 1;
}
bool fist_flag = true;
for(int i = 1; i < argc; i++)
{
const char* s = argv[i];
if( strcmp( s, "-w") == 0)
{
if( sscanf( argv[++i], "%u", &boardSize.width ) != 1 || boardSize.width <= 0 )
return fprintf( stderr, "Invalid board width\n" ), -1;
}
else if( strcmp( s, "-h" ) == 0 )
{
if( sscanf( argv[++i], "%u", &boardSize.height ) != 1 || boardSize.height <= 0 )
return fprintf( stderr, "Invalid board height\n" ), -1;
}
else if( strcmp( s, "-sw" ) == 0 )
{
if( sscanf( argv[++i], "%lf", &square_width ) != 1 || square_width <= 0 )
return fprintf(stderr, "Invalid square width\n"), -1;
}
else if( strcmp( s, "-sh" ) == 0 )
{
if( sscanf( argv[++i], "%lf", &square_height) != 1 || square_height <= 0 )
return fprintf(stderr, "Invalid square height\n"), -1;
}
else if( strcmp( s, "-o" ) == 0 )
{
outputFilename = argv[++i];
}
else if( strcmp( s, "-fs" ) == 0 )
{
flags |= omnidir::CALIB_FIX_SKEW;
}
else if( strcmp( s, "-fp" ) == 0 )
{
flags |= omnidir::CALIB_FIX_CENTER;
}
else if( s[0] != '-' && fist_flag)
{
fist_flag = false;
inputFilename1 = s;
}
else if( s[0] != '-' && !fist_flag)
{
inputFilename2 = s;
}
else
{
return fprintf( stderr, "Unknown option %s\n", s ), -1;
}
}
// get image name list
vector<string> image_list1, detec_list_1, image_list2, detec_list_2;
if((!readStringList(inputFilename1, image_list1)) || (!readStringList(inputFilename2, image_list2)))
return fprintf( stderr, "Failed to read image list\n"), -1;
// find corners in images
// some images may be failed in automatic corner detection, passed cases are in detec_list
if(!detecChessboardCorners(image_list1, detec_list_1, image_list2, detec_list_2,
imagePoints1, imagePoints2, boardSize, imageSize1, imageSize2))
return fprintf(stderr, "Not enough corner detected images\n"), -1;
// calculate object coordinates
Mat object;
calcChessboardCorners(boardSize, square_width, square_height, object);
for(int i = 0; i < (int)detec_list_1.size(); ++i)
{
objectPoints.push_back(object);
}
// run calibration, some images are discarded in calibration process because they are failed
// in initialization. Retained image indexes are in idx variable.
Mat K1, K2, D1, D2, xi1, xi2, idx;
vector<Vec3d> rvecs, tvecs;
Vec3d rvec, tvec;
double _xi1, _xi2, rms;
TermCriteria criteria(3, 200, 1e-8);
rms = omnidir::stereoCalibrate(objectPoints, imagePoints1, imagePoints2, imageSize1, imageSize2, K1, xi1, D1,
K2, xi2, D2, rvec, tvec, rvecs, tvecs, flags, criteria, idx);
_xi1 = xi1.at<double>(0);
_xi2 = xi2.at<double>(0);
saveCameraParams(outputFilename, flags, K1, K2, D1, D2, _xi1, _xi2, rvec, tvec, rvecs, tvecs,
detec_list_1, detec_list_2, idx, rms, imagePoints1, imagePoints2);
}
\ No newline at end of file
modules/ccalib/samples/random_pattern_calibration.cpp 0 → 100644
View file @ 9011368a
#include "opencv2/ccalib/randpattern.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/calib3d.hpp"
#include <vector>
#include <iostream>
#include <time.h>
using namespace std;
using namespace cv;
const char * usage =
"\n example command line for calibrate a camera by random pattern. \n"
" random_pattern_calibration -pw 600 -ph 850 -mm 20 image_list.xml \n"
"\n"
" the file image_list.xml is generated by imagelist_creator as\n"
"imagelist_creator image_list.xml *.*";
static void help()
{
printf("\n This is a sample for camera calibration by a random pattern.\n"
"Usage: random_pattern_calibration\n"
" -pw <pattern_width> # the physical width of random pattern\n"
" -ph <pattern_height> # the physical height of random pattern\n"
" -mm <minimal_match> # minimal number of matches\n"
" [-fp ] # fix the principal point at the center \n"
" input_data # input data - text file with a list of the images of the board, which is generated by imagelist_creator"
);
printf("\n %s", usage);
}
static bool readStringList( const string& filename, vector<string>& l )
{
l.resize(0);
FileStorage fs(filename, FileStorage::READ);
if( !fs.isOpened() )
return false;
FileNode n = fs.getFirstTopLevelNode();
if( n.type() != FileNode::SEQ )
return false;
FileNodeIterator it = n.begin(), it_end = n.end();
for( ; it != it_end; ++it )
l.push_back((string)*it);
return true;
}
static void saveCameraParams(const string& filename, Size imageSize, float patternWidth,
float patternHeight, int flags, const Mat& cameraMatrix, const Mat& distCoeffs,
const vector<Mat>& rvecs, const vector<Mat>& tvecs, double rms)
{
FileStorage fs (filename, FileStorage::WRITE );
time_t tt;
time( &tt );
struct tm *t2 = localtime( &tt );
char buf[1024];
strftime( buf, sizeof(buf)-1, "%c", t2 );
fs << "calibration_time" << buf;
if( !rvecs.empty())
fs << "nframes" << (int)rvecs.size();
fs << "image_width" << imageSize.width;
fs << "image_height" << imageSize.height;
fs << "pattern_width" << patternWidth;
fs << "pattern_height" << patternHeight;
fs << "flags" <<flags;
fs << "camera_matrix" << cameraMatrix;
fs << "distortion_coefficients" << distCoeffs;
fs << "rms" << rms;
if( !rvecs.empty() && !tvecs.empty() )
{
CV_Assert(rvecs[0].type() == tvecs[0].type());
Mat bigmat((int)rvecs.size(), 6, rvecs[0].type());
for( int i = 0; i < (int)rvecs.size(); i++ )
{
Mat r = bigmat(Range(i, i+1), Range(0,3));
Mat t = bigmat(Range(i, i+1), Range(3,6));
CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1);
CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1);
//*.t() is MatExpr (not Mat) so we can use assignment operator
r = rvecs[i].t();
t = tvecs[i].t();
}
//cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );
fs << "extrinsic_parameters" << bigmat;
}
}
int main(int argc, char** argv)
{
const char* inputFilename = 0;
const char* outputFilename = "out_camera_params.xml";
vector<string> imglist;
vector<Mat> vecImg;
int flags = 0;
float patternWidth = 0.0f, patternHeight = 0.0f;
int nMiniMatches = 0;
if(argc < 2)
{
help();
return 1;
}
for (int i = 1; i < argc; ++i)
{
const char* s = argv[i];
if(strcmp(s, "-pw") == 0)
{
if(sscanf(argv[++i], "%f", &patternWidth) != 1 || patternWidth <= 0)
return fprintf( stderr, "Invalid pattern width\n"), -1;
}
else if(strcmp(s, "-ph") == 0)
{
if(sscanf(argv[++i], "%f", &patternHeight) != 1 || patternHeight <= 0)
return fprintf( stderr, "Invalid pattern height\n"), -1;
}
else if (strcmp(s, "-mm") == 0)
{
if (sscanf(argv[++i], "%d", &nMiniMatches) != 1 || nMiniMatches < 15)
return fprintf( stderr, "Invalid number of minimal matches or number is too small"), -1;
}
else if( strcmp( s, "-fp" ) == 0 )
{
flags |= CALIB_FIX_PRINCIPAL_POINT;
}
else if( s[0] != '-')
{
inputFilename = s;
}
else
{
return fprintf( stderr, "Unknown option %s\n", s ), -1;
}
}
readStringList(inputFilename, imglist);
// the first image is the pattern
Mat pattern = cv::imread(imglist[0], cv::IMREAD_GRAYSCALE);
for (int i = 1; i < (int)imglist.size(); ++i)
{
Mat img;
img = cv::imread(imglist[i], cv::IMREAD_GRAYSCALE);
vecImg.push_back(img);
}
randpattern::RandomPatternCornerFinder finder(patternWidth, patternHeight, nMiniMatches);
finder.loadPattern(pattern);
finder.computeObjectImagePoints(vecImg);
vector<Mat> objectPoints = finder.getObjectPoints();
vector<Mat> imagePoints = finder.getImagePoints();
Mat K;
Mat D;
vector<Mat> rvec, tvec;
double rms = calibrateCamera(objectPoints, imagePoints, vecImg[0].size(), K, D, rvec, tvec);
saveCameraParams(outputFilename, vecImg[0].size(), patternWidth, patternHeight, flags, K, D, rvec, tvec, rms);
}
modules/ccalib/samples/random_pattern_generator.cpp 0 → 100644
View file @ 9011368a
#include "opencv2/ccalib/randpattern.hpp"
using namespace cv;
const char * usage =
"\n example command line for generating a random pattern. \n"
" random_patterng_generator -iw 600 -ih 850 pattern.png\n"
"\n";
static void help()
{
printf("\n This is a sample for generating a random pattern that can be used for calibration.\n"
"Usage: random_patterng_generator\n"
" -iw <image_width> # the width of pattern image\n"
" -ih <image_height> # the height of pattern image\n"
" filename # the filename for pattern image \n"
);
printf("\n %s", usage);
}
int main(int argc, char** argv)
{
const char* filename = 0;
Mat pattern;
int width = 0, height = 0;
if(argc < 2)
{
help();
return 1;
}
for (int i = 1; i < argc; ++i)
{
const char* s = argv[i];
if(strcmp(s, "-iw") == 0)
{
if(sscanf(argv[++i], "%d", &width) != 1 || width <= 0)
return fprintf( stderr, "Invalid pattern image width\n"), -1;
}
else if(strcmp(s, "-ih") == 0)
{
if(sscanf(argv[++i], "%d", &height) != 1 || height <= 0)
return fprintf( stderr, "Invalid pattern image height\n"), -1;
}
else if( s[0] != '-')
{
filename = s;
}
else
{
return fprintf( stderr, "Unknown option %s\n", s ), -1;
}
}
randpattern::RandomPatternGenerator generator(width, height);
generator.generatePattern();
pattern = generator.getPattern();
imwrite(filename, pattern);
}
\ No newline at end of file
modules/ccalib/src/multicalib.cpp 0 → 100644
View file @ 9011368a
/*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) 2015, Baisheng Lai (laibaisheng@gmail.com), Zhejiang University,
// all rights reserved.
//
// 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 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*/
/**
* This module was accepted as a GSoC 2015 project for OpenCV, authored by
* Baisheng Lai, mentored by Bo Li.
*
* The omnidirectional camera in this module is denoted by the catadioptric
* model. Please refer to Mei's paper for details of the camera model:
*
* C. Mei and P. Rives, "Single view point omnidirectional camera
* calibration from planar grids", in ICRA 2007.
*
* The implementation of the calibration part is based on Li's calibration
* toolbox:
*
* B. Li, L. Heng, K. Kevin and M. Pollefeys, "A Multiple-Camera System
* Calibration Toolbox Using A Feature Descriptor-Based Calibration
* Pattern", in IROS 2013.
*/
#include "precomp.hpp"
#include "opencv2/ccalib/multicalib.hpp"
#include "opencv2/core.hpp"
#include <string>
#include <vector>
#include <queue>
#include <iostream>
namespace cv { namespace multicalib {
MultiCameraCalibration::MultiCameraCalibration(int cameraType, int nCameras, const std::string& fileName,
float patternWidth, float patternHeight, int verbose, int showExtration, int nMiniMatches, int flags, TermCriteria criteria,
Ptr<FeatureDetector> detector, Ptr<DescriptorExtractor> descriptor,
Ptr<DescriptorMatcher> matcher)
{
_camType = cameraType;
_nCamera = nCameras;
_flags = flags;
_nMiniMatches = nMiniMatches;
_filename = fileName;
_patternWidth = patternWidth;
_patternHeight = patternHeight;
_criteria = criteria;
_showExtraction = showExtration;
_objectPointsForEachCamera.resize(_nCamera);
_imagePointsForEachCamera.resize(_nCamera);
_cameraMatrix.resize(_nCamera);
_distortCoeffs.resize(_nCamera);
_xi.resize(_nCamera);
_omEachCamera.resize(_nCamera);
_tEachCamera.resize(_nCamera);
_detector = detector;
_descriptor = descriptor;
_matcher = matcher;
_verbose = verbose;
for (int i = 0; i < _nCamera; ++i)
{
_vertexList.push_back(vertex());
}
}
double MultiCameraCalibration::run()
{
loadImages();
initialize();
double error = optimizeExtrinsics();
return error;
}
std::vector<std::string> MultiCameraCalibration::readStringList()
{
std::vector<std::string> l;
l.resize(0);
FileStorage fs(_filename, FileStorage::READ);
FileNode n = fs.getFirstTopLevelNode();
FileNodeIterator it = n.begin(), it_end = n.end();
for( ; it != it_end; ++it )
l.push_back((std::string)*it);
return l;
}
void MultiCameraCalibration::loadImages()
{
std::vector<std::string> file_list;
file_list = readStringList();
Ptr<FeatureDetector> detector = _detector;
Ptr<DescriptorExtractor> descriptor = _descriptor;
Ptr<DescriptorMatcher> matcher = _matcher;
randpattern::RandomPatternCornerFinder finder(_patternWidth, _patternHeight, _nMiniMatches, CV_32F, _verbose, this->_showExtraction, detector, descriptor, matcher);
Mat pattern = cv::imread(file_list[0]);
finder.loadPattern(pattern);
std::vector<std::vector<std::string> > filesEachCameraFull(_nCamera);
std::vector<std::vector<int> > timestampFull(_nCamera);
std::vector<std::vector<int> > timestampAvailable(_nCamera);
for (int i = 1; i < (int)file_list.size(); ++i)
{
int cameraVertex, timestamp;
std::string filename = file_list[i].substr(0, file_list[i].find('.'));
size_t spritPosition1 = filename.rfind('/');
size_t spritPosition2 = filename.rfind('\\');
if (spritPosition1!=std::string::npos)
{
filename = filename.substr(spritPosition1+1, filename.size() - 1);
}
else if(spritPosition2!= std::string::npos)
{
filename = filename.substr(spritPosition2+1, filename.size() - 1);
}
sscanf(filename.c_str(), "%d-%d", &cameraVertex, &timestamp);
filesEachCameraFull[cameraVertex].push_back(file_list[i]);
timestampFull[cameraVertex].push_back(timestamp);
}
// calibrate each camera individually
for (int camera = 0; camera < _nCamera; ++camera)
{
Mat image, cameraMatrix, distortCoeffs;
// find image and object points
for (int imgIdx = 0; imgIdx < (int)filesEachCameraFull[camera].size(); ++imgIdx)
{
image = imread(filesEachCameraFull[camera][imgIdx], IMREAD_GRAYSCALE);
if (!image.empty() && _verbose)
{
std::cout << "open image " << filesEachCameraFull[camera][imgIdx] << " successfully" << std::endl;
}
else if (image.empty() && _verbose)
{
std::cout << "open image" << filesEachCameraFull[camera][imgIdx] << " failed" << std::endl;
}
std::vector<Mat> imgObj = finder.computeObjectImagePointsForSingle(image);
if ((int)imgObj[0].total() > _nMiniMatches)
{
_imagePointsForEachCamera[camera].push_back(imgObj[0]);
_objectPointsForEachCamera[camera].push_back(imgObj[1]);
timestampAvailable[camera].push_back(timestampFull[camera][imgIdx]);
}
else if ((int)imgObj[0].total() <= _nMiniMatches && _verbose)
{
std::cout << "image " << filesEachCameraFull[camera][imgIdx] <<" has too few matched points "<< std::endl;
}
}
// calibrate
Mat idx;
double rms = 0.0;
if (_camType == PINHOLE)
{
rms = cv::calibrateCamera(_objectPointsForEachCamera[camera], _imagePointsForEachCamera[camera],
image.size(), _cameraMatrix[camera], _distortCoeffs[camera], _omEachCamera[camera],
_tEachCamera[camera],_flags);
idx = Mat(1, (int)_omEachCamera[camera].size(), CV_32S);
for (int i = 0; i < (int)idx.total(); ++i)
{
idx.at<int>(i) = i;
}
}
//else if (_camType == FISHEYE)
//{
// rms = cv::fisheye::calibrate(_objectPointsForEachCamera[camera], _imagePointsForEachCamera[camera],
// image.size(), _cameraMatrix[camera], _distortCoeffs[camera], _omEachCamera[camera],
// _tEachCamera[camera], _flags);
// idx = Mat(1, (int)_omEachCamera[camera].size(), CV_32S);
// for (int i = 0; i < (int)idx.total(); ++i)
// {
// idx.at<int>(i) = i;
// }
//}
else if (_camType == OMNIDIRECTIONAL)
{
rms = cv::omnidir::calibrate(_objectPointsForEachCamera[camera], _imagePointsForEachCamera[camera],
image.size(), _cameraMatrix[camera], _xi[camera], _distortCoeffs[camera], _omEachCamera[camera],
_tEachCamera[camera], _flags, TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 300, 1e-7),
idx);
}
_cameraMatrix[camera].convertTo(_cameraMatrix[camera], CV_32F);
_distortCoeffs[camera].convertTo(_distortCoeffs[camera], CV_32F);
_xi[camera].convertTo(_xi[camera], CV_32F);
//else
//{
// CV_Error_(CV_StsOutOfRange, "Unknown camera type, use PINHOLE or OMNIDIRECTIONAL");
//}
for (int i = 0; i < (int)_omEachCamera[camera].size(); ++i)
{
int cameraVertex, timestamp, photoVertex;
cameraVertex = camera;
timestamp = timestampAvailable[camera][idx.at<int>(i)];
photoVertex = this->getPhotoVertex(timestamp);
if (_omEachCamera[camera][i].type()!=CV_32F)
{
_omEachCamera[camera][i].convertTo(_omEachCamera[camera][i], CV_32F);
}
if (_tEachCamera[camera][i].type()!=CV_32F)
{
_tEachCamera[camera][i].convertTo(_tEachCamera[camera][i], CV_32F);
}
Mat transform = Mat::eye(4, 4, CV_32F);
Mat R, T;
Rodrigues(_omEachCamera[camera][i], R);
T = (_tEachCamera[camera][i]).reshape(1, 3);
R.copyTo(transform.rowRange(0, 3).colRange(0, 3));
T.copyTo(transform.rowRange(0, 3).col(3));
this->_edgeList.push_back(edge(cameraVertex, photoVertex, idx.at<int>(i), transform));
}
std::cout << "initialized for camera " << camera << " rms = " << rms << std::endl;
std::cout << "initialized camera matrix for camera " << camera << " is" << std::endl;
std::cout << _cameraMatrix[camera] << std::endl;
std::cout << "xi for camera " << camera << " is " << _xi[camera] << std::endl;
}
}
int MultiCameraCalibration::getPhotoVertex(int timestamp)
{
int photoVertex = INVALID;
// find in existing photo vertex
for (int i = 0; i < (int)_vertexList.size(); ++i)
{
if (_vertexList[i].timestamp == timestamp)
{
photoVertex = i;
break;
}
}
// add a new photo vertex
if (photoVertex == INVALID)
{
_vertexList.push_back(vertex(Mat::eye(4, 4, CV_32F), timestamp));
photoVertex = (int)_vertexList.size() - 1;
}
return photoVertex;
}
void MultiCameraCalibration::initialize()
{
int nVertices = (int)_vertexList.size();
int nEdges = (int) _edgeList.size();
// build graph
Mat G = Mat::zeros(nVertices, nVertices, CV_32S);
for (int edgeIdx = 0; edgeIdx < nEdges; ++edgeIdx)
{
G.at<int>(this->_edgeList[edgeIdx].cameraVertex, this->_edgeList[edgeIdx].photoVertex) = edgeIdx + 1;
}
G = G + G.t();
// traverse the graph
std::vector<int> pre, order;
graphTraverse(G, 0, order, pre);
for (int i = 0; i < _nCamera; ++i)
{
if (pre[i] == INVALID)
{
std::cout << "camera" << i << "is not connected" << std::endl;
}
}
for (int i = 1; i < (int)order.size(); ++i)
{
int vertexIdx = order[i];
Mat prePose = this->_vertexList[pre[vertexIdx]].pose;
int edgeIdx = G.at<int>(vertexIdx, pre[vertexIdx]) - 1;
Mat transform = this->_edgeList[edgeIdx].transform;
if (vertexIdx < _nCamera)
{
this->_vertexList[vertexIdx].pose = transform * prePose.inv();
this->_vertexList[vertexIdx].pose.convertTo(this->_vertexList[vertexIdx].pose, CV_32F);
if (_verbose)
{
std::cout << "initial pose for camera " << vertexIdx << " is " << std::endl;
std::cout << this->_vertexList[vertexIdx].pose << std::endl;
}
}
else
{
this->_vertexList[vertexIdx].pose = prePose.inv() * transform;
this->_vertexList[vertexIdx].pose.convertTo(this->_vertexList[vertexIdx].pose, CV_32F);
}
}
}
double MultiCameraCalibration::optimizeExtrinsics()
{
// get om, t vector
int nVertex = (int)this->_vertexList.size();
Mat extrinParam(1, (nVertex-1)*6, CV_32F);
int offset = 0;
// the pose of the vertex[0] is eye
for (int i = 1; i < nVertex; ++i)
{
Mat rvec, tvec;
cv::Rodrigues(this->_vertexList[i].pose.rowRange(0,3).colRange(0, 3), rvec);
this->_vertexList[i].pose.rowRange(0,3).col(3).copyTo(tvec);
rvec.reshape(1, 1).copyTo(extrinParam.colRange(offset, offset + 3));
tvec.reshape(1, 1).copyTo(extrinParam.colRange(offset+3, offset +6));
offset += 6;
}
//double error_pre = computeProjectError(extrinParam);
// optimization
const double alpha_smooth = 0.01;
double change = 1;
for(int iter = 0; ; ++iter)
{
if ((_criteria.type == 1 && iter >= _criteria.maxCount) ||
(_criteria.type == 2 && change <= _criteria.epsilon) ||
(_criteria.type == 3 && (change <= _criteria.epsilon || iter >= _criteria.maxCount)))
break;
double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, (double)iter + 1.0);
Mat JTJ_inv, JTError;
this->computeJacobianExtrinsic(extrinParam, JTJ_inv, JTError);
Mat G = alpha_smooth2*JTJ_inv * JTError;
if (G.depth() == CV_64F)
{
G.convertTo(G, CV_32F);
}
extrinParam = extrinParam + G.reshape(1, 1);
change = norm(G) / norm(extrinParam);
//double error = computeProjectError(extrinParam);
}
double error = computeProjectError(extrinParam);
std::vector<Vec3f> rvecVertex, tvecVertex;
vector2parameters(extrinParam, rvecVertex, tvecVertex);
for (int verIdx = 1; verIdx < (int)_vertexList.size(); ++verIdx)
{
Mat R;
Mat pose = Mat::eye(4, 4, CV_32F);
Rodrigues(rvecVertex[verIdx-1], R);
R.copyTo(pose.colRange(0, 3).rowRange(0, 3));
Mat(tvecVertex[verIdx-1]).reshape(1, 3).copyTo(pose.rowRange(0, 3).col(3));
_vertexList[verIdx].pose = pose;
if (_verbose && verIdx < _nCamera)
{
std::cout << "final camera pose of camera " << verIdx << " is" << std::endl;
std::cout << pose << std::endl;
}
}
return error;
}
void MultiCameraCalibration::computeJacobianExtrinsic(const Mat& extrinsicParams, Mat& JTJ_inv, Mat& JTE)
{
int nParam = (int)extrinsicParams.total();
int nEdge = (int)_edgeList.size();
std::vector<int> pointsLocation(nEdge+1, 0);
for (int edgeIdx = 0; edgeIdx < nEdge; ++edgeIdx)
{
int nPoints = (int)_objectPointsForEachCamera[_edgeList[edgeIdx].cameraVertex][_edgeList[edgeIdx].photoIndex].total();
pointsLocation[edgeIdx+1] = pointsLocation[edgeIdx] + nPoints*2;
}
JTJ_inv = Mat(nParam, nParam, CV_64F);
JTE = Mat(nParam, 1, CV_64F);
Mat J = Mat::zeros(pointsLocation[nEdge], nParam, CV_64F);
Mat E = Mat::zeros(pointsLocation[nEdge], 1, CV_64F);
for (int edgeIdx = 0; edgeIdx < nEdge; ++edgeIdx)
{
int photoVertex = _edgeList[edgeIdx].photoVertex;
int photoIndex = _edgeList[edgeIdx].photoIndex;
int cameraVertex = _edgeList[edgeIdx].cameraVertex;
Mat objectPoints = _objectPointsForEachCamera[cameraVertex][photoIndex];
Mat imagePoints = _imagePointsForEachCamera[cameraVertex][photoIndex];
Mat rvecTran, tvecTran;
Mat R = _edgeList[edgeIdx].transform.rowRange(0, 3).colRange(0, 3);
tvecTran = _edgeList[edgeIdx].transform.rowRange(0, 3).col(3);
cv::Rodrigues(R, rvecTran);
Mat rvecPhoto = extrinsicParams.colRange((photoVertex-1)*6, (photoVertex-1)*6 + 3);
Mat tvecPhoto = extrinsicParams.colRange((photoVertex-1)*6 + 3, (photoVertex-1)*6 + 6);
Mat rvecCamera, tvecCamera;
if (cameraVertex > 0)
{
rvecCamera = extrinsicParams.colRange((cameraVertex-1)*6, (cameraVertex-1)*6 + 3);
tvecCamera = extrinsicParams.colRange((cameraVertex-1)*6 + 3, (cameraVertex-1)*6 + 6);
}
else
{
rvecCamera = Mat::zeros(3, 1, CV_32F);
tvecCamera = Mat::zeros(3, 1, CV_32F);
}
Mat jacobianPhoto, jacobianCamera, error;
computePhotoCameraJacobian(rvecPhoto, tvecPhoto, rvecCamera, tvecCamera, rvecTran, tvecTran,
objectPoints, imagePoints, this->_cameraMatrix[cameraVertex], this->_distortCoeffs[cameraVertex],
this->_xi[cameraVertex], jacobianPhoto, jacobianCamera, error);
if (cameraVertex > 0)
{
jacobianCamera.copyTo(J.rowRange(pointsLocation[edgeIdx], pointsLocation[edgeIdx+1]).
colRange((cameraVertex-1)*6, cameraVertex*6));
}
jacobianPhoto.copyTo(J.rowRange(pointsLocation[edgeIdx], pointsLocation[edgeIdx+1]).
colRange((photoVertex-1)*6, photoVertex*6));
error.copyTo(E.rowRange(pointsLocation[edgeIdx], pointsLocation[edgeIdx+1]));
}
//std::cout << J.t() * J << std::endl;
JTJ_inv = (J.t() * J + 1e-10).inv();
JTE = J.t() * E;
}
void MultiCameraCalibration::computePhotoCameraJacobian(const Mat& rvecPhoto, const Mat& tvecPhoto, const Mat& rvecCamera,
const Mat& tvecCamera, Mat& rvecTran, Mat& tvecTran, const Mat& objectPoints, const Mat& imagePoints, const Mat& K,
const Mat& distort, const Mat& xi, Mat& jacobianPhoto, Mat& jacobianCamera, Mat& E)
{
Mat drvecTran_drecvPhoto, drvecTran_dtvecPhoto,
drvecTran_drvecCamera, drvecTran_dtvecCamera,
dtvecTran_drvecPhoto, dtvecTran_dtvecPhoto,
dtvecTran_drvecCamera, dtvecTran_dtvecCamera;
MultiCameraCalibration::compose_motion(rvecPhoto, tvecPhoto, rvecCamera, tvecCamera, rvecTran, tvecTran,
drvecTran_drecvPhoto, drvecTran_dtvecPhoto, drvecTran_drvecCamera, drvecTran_dtvecCamera,
dtvecTran_drvecPhoto, dtvecTran_dtvecPhoto, dtvecTran_drvecCamera, dtvecTran_dtvecCamera);
if (rvecTran.depth() == CV_64F)
{
rvecTran.convertTo(rvecTran, CV_32F);
}
if (tvecTran.depth() == CV_64F)
{
tvecTran.convertTo(tvecTran, CV_32F);
}
float xif = 0.0f;
if (_camType == OMNIDIRECTIONAL)
{
xif= xi.at<float>(0);
}
Mat imagePoints2, jacobian, dx_drvecCamera, dx_dtvecCamera, dx_drvecPhoto, dx_dtvecPhoto;
if (_camType == PINHOLE)
{
cv::projectPoints(objectPoints, rvecTran, tvecTran, K, distort, imagePoints2, jacobian);
}
//else if (_camType == FISHEYE)
//{
// cv::fisheye::projectPoints(objectPoints, imagePoints2, rvecTran, tvecTran, K, distort, 0, jacobian);
//}
else if (_camType == OMNIDIRECTIONAL)
{
cv::omnidir::projectPoints(objectPoints, imagePoints2, rvecTran, tvecTran, K, xif, distort, jacobian);
}
if (objectPoints.depth() == CV_32F)
{
Mat(imagePoints - imagePoints2).convertTo(E, CV_64FC2);
}
else
{
E = imagePoints - imagePoints2;
}
E = E.reshape(1, (int)imagePoints.total()*2);
dx_drvecCamera = jacobian.colRange(0, 3) * drvecTran_drvecCamera + jacobian.colRange(3, 6) * dtvecTran_drvecCamera;
dx_dtvecCamera = jacobian.colRange(0, 3) * drvecTran_dtvecCamera + jacobian.colRange(3, 6) * dtvecTran_dtvecCamera;
dx_drvecPhoto = jacobian.colRange(0, 3) * drvecTran_drecvPhoto + jacobian.colRange(3, 6) * dtvecTran_drvecPhoto;
dx_dtvecPhoto = jacobian.colRange(0, 3) * drvecTran_dtvecPhoto + jacobian.colRange(3, 6) * dtvecTran_dtvecPhoto;
jacobianCamera = cv::Mat(dx_drvecCamera.rows, 6, CV_64F);
jacobianPhoto = cv::Mat(dx_drvecPhoto.rows, 6, CV_64F);
dx_drvecCamera.copyTo(jacobianCamera.colRange(0, 3));
dx_dtvecCamera.copyTo(jacobianCamera.colRange(3, 6));
dx_drvecPhoto.copyTo(jacobianPhoto.colRange(0, 3));
dx_dtvecPhoto.copyTo(jacobianPhoto.colRange(3, 6));
}
void MultiCameraCalibration::graphTraverse(const Mat& G, int begin, std::vector<int>& order, std::vector<int>& pre)
{
CV_Assert(!G.empty() && G.rows == G.cols);
int nVertex = G.rows;
order.resize(0);
pre.resize(nVertex, INVALID);
pre[begin] = -1;
std::vector<bool> visited(nVertex, false);
std::queue<int> q;
visited[begin] = true;
q.push(begin);
order.push_back(begin);
while(!q.empty())
{
int v = q.front();
q.pop();
Mat idx;
// use my findNonZero maybe
findRowNonZero(G.row(v), idx);
for(int i = 0; i < (int)idx.total(); ++i)
{
int neighbor = idx.at<int>(i);
if (!visited[neighbor])
{
visited[neighbor] = true;
q.push(neighbor);
order.push_back(neighbor);
pre[neighbor] = v;
}
}
}
}
void MultiCameraCalibration::findRowNonZero(const Mat& row, Mat& idx)
{
CV_Assert(!row.empty() && row.rows == 1 && row.channels() == 1);
Mat _row;
std::vector<int> _idx;
row.convertTo(_row, CV_32F);
for (int i = 0; i < (int)row.total(); ++i)
{
if (_row.at<float>(i) != 0)
{
_idx.push_back(i);
}
}
idx.release();
idx.create(1, (int)_idx.size(), CV_32S);
for (int i = 0; i < (int)_idx.size(); ++i)
{
idx.at<int>(i) = _idx[i];
}
}
double MultiCameraCalibration::computeProjectError(Mat& parameters)
{
int nVertex = (int)_vertexList.size();
CV_Assert((int)parameters.total() == (nVertex-1) * 6 && parameters.depth() == CV_32F);
int nEdge = (int)_edgeList.size();
// recompute the transform between photos and cameras
std::vector<edge> edgeList = this->_edgeList;
std::vector<Vec3f> rvecVertex, tvecVertex;
vector2parameters(parameters, rvecVertex, tvecVertex);
float totalError = 0;
int totalNPoints = 0;
for (int edgeIdx = 0; edgeIdx < nEdge; ++edgeIdx)
{
Mat RPhoto, RCamera, TPhoto, TCamera, transform;
int cameraVertex = edgeList[edgeIdx].cameraVertex;
int photoVertex = edgeList[edgeIdx].photoVertex;
int PhotoIndex = edgeList[edgeIdx].photoIndex;
TPhoto = Mat(tvecVertex[photoVertex - 1]).reshape(1, 3);
//edgeList[edgeIdx].transform = Mat::ones(4, 4, CV_32F);
transform = Mat::eye(4, 4, CV_32F);
cv::Rodrigues(rvecVertex[photoVertex-1], RPhoto);
if (cameraVertex == 0)
{
RPhoto.copyTo(transform.rowRange(0, 3).colRange(0, 3));
TPhoto.copyTo(transform.rowRange(0, 3).col(3));
}
else
{
TCamera = Mat(tvecVertex[cameraVertex - 1]).reshape(1, 3);
cv::Rodrigues(rvecVertex[cameraVertex - 1], RCamera);
Mat(RCamera*RPhoto).copyTo(transform.rowRange(0, 3).colRange(0, 3));
Mat(RCamera * TPhoto + TCamera).copyTo(transform.rowRange(0, 3).col(3));
}
transform.copyTo(edgeList[edgeIdx].transform);
Mat rvec, tvec;
cv::Rodrigues(transform.rowRange(0, 3).colRange(0, 3), rvec);
transform.rowRange(0, 3).col(3).copyTo(tvec);
Mat objectPoints, imagePoints, proImagePoints;
objectPoints = this->_objectPointsForEachCamera[cameraVertex][PhotoIndex];
imagePoints = this->_imagePointsForEachCamera[cameraVertex][PhotoIndex];
if (this->_camType == PINHOLE)
{
cv::projectPoints(objectPoints, rvec, tvec, _cameraMatrix[cameraVertex], _distortCoeffs[cameraVertex],
proImagePoints);
}
//else if (this->_camType == FISHEYE)
//{
// cv::fisheye::projectPoints(objectPoints, proImagePoints, rvec, tvec, _cameraMatrix[cameraVertex],
// _distortCoeffs[cameraVertex]);
//}
else if (this->_camType == OMNIDIRECTIONAL)
{
float xi = _xi[cameraVertex].at<float>(0);
cv::omnidir::projectPoints(objectPoints, proImagePoints, rvec, tvec, _cameraMatrix[cameraVertex],
xi, _distortCoeffs[cameraVertex]);
}
Mat error = imagePoints - proImagePoints;
Vec2f* ptr_err = error.ptr<Vec2f>();
for (int i = 0; i < (int)error.total(); ++i)
{
totalError += sqrt(ptr_err[i][0]*ptr_err[i][0] + ptr_err[i][1]*ptr_err[i][1]);
}
totalNPoints += (int)error.total();
}
double meanReProjError = totalError / totalNPoints;
_error = meanReProjError;
return meanReProjError;
}
void MultiCameraCalibration::compose_motion(InputArray _om1, InputArray _T1, InputArray _om2, InputArray _T2, Mat& om3, Mat& T3, Mat& dom3dom1,
Mat& dom3dT1, Mat& dom3dom2, Mat& dom3dT2, Mat& dT3dom1, Mat& dT3dT1, Mat& dT3dom2, Mat& dT3dT2)
{
Mat om1, om2, T1, T2;
_om1.getMat().convertTo(om1, CV_64F);
_om2.getMat().convertTo(om2, CV_64F);
_T1.getMat().reshape(1, 3).convertTo(T1, CV_64F);
_T2.getMat().reshape(1, 3).convertTo(T2, CV_64F);
/*Mat om2 = _om2.getMat();
Mat T1 = _T1.getMat().reshape(1, 3);
Mat T2 = _T2.getMat().reshape(1, 3);*/
//% Rotations:
Mat R1, R2, R3, dR1dom1(9, 3, CV_64FC1), dR2dom2;
cv::Rodrigues(om1, R1, dR1dom1);
cv::Rodrigues(om2, R2, dR2dom2);
/*JRodriguesMatlab(dR1dom1, dR1dom1);
JRodriguesMatlab(dR2dom2, dR2dom2);*/
dR1dom1 = dR1dom1.t();
dR2dom2 = dR2dom2.t();
R3 = R2 * R1;
Mat dR3dR2, dR3dR1;
//dAB(R2, R1, dR3dR2, dR3dR1);
matMulDeriv(R2, R1, dR3dR2, dR3dR1);
Mat dom3dR3;
cv::Rodrigues(R3, om3, dom3dR3);
//JRodriguesMatlab(dom3dR3, dom3dR3);
dom3dR3 = dom3dR3.t();
dom3dom1 = dom3dR3 * dR3dR1 * dR1dom1;
dom3dom2 = dom3dR3 * dR3dR2 * dR2dom2;
dom3dT1 = Mat::zeros(3, 3, CV_64FC1);
dom3dT2 = Mat::zeros(3, 3, CV_64FC1);
//% Translations:
Mat T3t = R2 * T1;
Mat dT3tdR2, dT3tdT1;
//dAB(R2, T1, dT3tdR2, dT3tdT1);
matMulDeriv(R2, T1, dT3tdR2, dT3tdT1);
Mat dT3tdom2 = dT3tdR2 * dR2dom2;
T3 = T3t + T2;
dT3dT1 = dT3tdT1;
dT3dT2 = Mat::eye(3, 3, CV_64FC1);
dT3dom2 = dT3tdom2;
dT3dom1 = Mat::zeros(3, 3, CV_64FC1);
}
void MultiCameraCalibration::vector2parameters(const Mat& parameters, std::vector<Vec3f>& rvecVertex, std::vector<Vec3f>& tvecVertexs)
{
int nVertex = (int)_vertexList.size();
CV_Assert((int)parameters.channels() == 1 && (int)parameters.total() == 6*(nVertex - 1));
CV_Assert(parameters.depth() == CV_32F);
parameters.reshape(1, 1);
rvecVertex.reserve(0);
tvecVertexs.resize(0);
for (int i = 0; i < nVertex - 1; ++i)
{
rvecVertex.push_back(Vec3f(parameters.colRange(i*6, i*6 + 3)));
tvecVertexs.push_back(Vec3f(parameters.colRange(i*6 + 3, i*6 + 6)));
}
}
void MultiCameraCalibration::parameters2vector(const std::vector<Vec3f>& rvecVertex, const std::vector<Vec3f>& tvecVertex, Mat& parameters)
{
CV_Assert(rvecVertex.size() == tvecVertex.size());
int nVertex = (int)rvecVertex.size();
// the pose of the first camera is known
parameters.create(1, 6*(nVertex-1), CV_32F);
for (int i = 0; i < nVertex-1; ++i)
{
Mat(rvecVertex[i]).reshape(1, 1).copyTo(parameters.colRange(i*6, i*6 + 3));
Mat(tvecVertex[i]).reshape(1, 1).copyTo(parameters.colRange(i*6 + 3, i*6 + 6));
}
}
void MultiCameraCalibration::writeParameters(const std::string& filename)
{
FileStorage fs( filename, FileStorage::WRITE );
fs << "nCameras" << _nCamera;
for (int camIdx = 0; camIdx < _nCamera; ++camIdx)
{
char num[10];
sprintf(num, "%d", camIdx);
std::string cameraMatrix = "camera_matrix_" + std::string(num);
std::string cameraPose = "camera_pose_" + std::string(num);
std::string cameraDistortion = "camera_distortion_" + std::string(num);
std::string cameraXi = "xi_" + std::string(num);
fs << cameraMatrix << _cameraMatrix[camIdx];
fs << cameraDistortion << _distortCoeffs[camIdx];
if (_camType == OMNIDIRECTIONAL)
{
fs << cameraXi << _xi[camIdx].at<float>(0);
}
fs << cameraPose << _vertexList[camIdx].pose;
}
fs << "meanReprojectError" <<_error;
for (int photoIdx = _nCamera; photoIdx < (int)_vertexList.size(); ++photoIdx)
{
char timestamp[100];
sprintf(timestamp, "%d", _vertexList[photoIdx].timestamp);
std::string photoTimestamp = "pose_timestamp_" + std::string(timestamp);
fs << photoTimestamp << _vertexList[photoIdx].pose;
}
}
}} // namespace multicalib, cv
\ No newline at end of file
modules/ccalib/src/omnidir.cpp 0 → 100644
View file @ 9011368a
/*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) 2015, Baisheng Lai (laibaisheng@gmail.com), Zhejiang University,
// all rights reserved.
//
// 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 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*/
/**
* This module was accepted as a GSoC 2015 project for OpenCV, authored by
* Baisheng Lai, mentored by Bo Li.
*
* The omnidirectional camera in this module is denoted by the catadioptric
* model. Please refer to Mei's paper for details of the camera model:
*
* C. Mei and P. Rives, "Single view point omnidirectional camera
* calibration from planar grids", in ICRA 2007.
*
* The implementation of the calibration part is based on Li's calibration
* toolbox:
*
* B. Li, L. Heng, K. Kevin and M. Pollefeys, "A Multiple-Camera System
* Calibration Toolbox Using A Feature Descriptor-Based Calibration
* Pattern", in IROS 2013.
*/
#include "precomp.hpp"
#include "opencv2/ccalib/omnidir.hpp"
#include <fstream>
#include <iostream>
namespace cv { namespace
{
struct JacobianRow
{
Matx13d dom,dT;
Matx12d df;
double ds;
Matx12d dc;
double dxi;
Matx14d dkp; // distortion k1,k2,p1,p2
};
}}
/////////////////////////////////////////////////////////////////////////////
//////// projectPoints
void cv::omnidir::projectPoints(InputArray objectPoints, OutputArray imagePoints,
InputArray rvec, InputArray tvec, InputArray K, double xi, InputArray D, OutputArray jacobian)
{
CV_Assert(objectPoints.type() == CV_64FC3 || objectPoints.type() == CV_32FC3);
CV_Assert((rvec.depth() == CV_64F || rvec.depth() == CV_32F) && rvec.total() == 3);
CV_Assert((tvec.depth() == CV_64F || tvec.depth() == CV_32F) && tvec.total() == 3);
CV_Assert((K.type() == CV_64F || K.type() == CV_32F) && K.size() == Size(3,3));
CV_Assert((D.type() == CV_64F || D.type() == CV_32F) && D.total() == 4);
imagePoints.create(objectPoints.size(), CV_MAKETYPE(objectPoints.depth(), 2));
int n = (int)objectPoints.total();
Vec3d om = rvec.depth() == CV_32F ? (Vec3d)*rvec.getMat().ptr<Vec3f>() : *rvec.getMat().ptr<Vec3d>();
Vec3d T = tvec.depth() == CV_32F ? (Vec3d)*tvec.getMat().ptr<Vec3f>() : *tvec.getMat().ptr<Vec3d>();
Vec2d f,c;
double s;
if (K.depth() == CV_32F)
{
Matx33f Kc = K.getMat();
f = Vec2f(Kc(0,0), Kc(1,1));
c = Vec2f(Kc(0,2),Kc(1,2));
s = (double)Kc(0,1);
}
else
{
Matx33d Kc = K.getMat();
f = Vec2d(Kc(0,0), Kc(1,1));
c = Vec2d(Kc(0,2),Kc(1,2));
s = Kc(0,1);
}
Vec4d kp = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr<Vec4f>() : *D.getMat().ptr<Vec4d>();
//Vec<double, 4> kp= (Vec<double,4>)*D.getMat().ptr<Vec<double,4> >();
const Vec3d* Xw_alld = objectPoints.getMat().ptr<Vec3d>();
const Vec3f* Xw_allf = objectPoints.getMat().ptr<Vec3f>();
Vec2d* xpd = imagePoints.getMat().ptr<Vec2d>();
Vec2f* xpf = imagePoints.getMat().ptr<Vec2f>();
Matx33d R;
Matx<double, 3, 9> dRdom;
Rodrigues(om, R, dRdom);
JacobianRow *Jn = 0;
if (jacobian.needed())
{
int nvars = 2+2+1+4+3+3+1; // f,c,s,kp,om,T,xi
jacobian.create(2*int(n), nvars, CV_64F);
Jn = jacobian.getMat().ptr<JacobianRow>(0);
}
double k1=kp[0],k2=kp[1];
double p1 = kp[2], p2 = kp[3];
for (int i = 0; i < n; i++)
{
// convert to camera coordinate
Vec3d Xw = objectPoints.depth() == CV_32F ? (Vec3d)Xw_allf[i] : Xw_alld[i];
Vec3d Xc = (Vec3d)(R*Xw + T);
// convert to unit sphere
Vec3d Xs = Xc/cv::norm(Xc);
// convert to normalized image plane
Vec2d xu = Vec2d(Xs[0]/(Xs[2]+xi), Xs[1]/(Xs[2]+xi));
// add distortion
Vec2d xd;
double r2 = xu[0]*xu[0]+xu[1]*xu[1];
double r4 = r2*r2;
xd[0] = xu[0]*(1+k1*r2+k2*r4) + 2*p1*xu[0]*xu[1] + p2*(r2+2*xu[0]*xu[0]);
xd[1] = xu[1]*(1+k1*r2+k2*r4) + p1*(r2+2*xu[1]*xu[1]) + 2*p2*xu[0]*xu[1];
// convert to pixel coordinate
Vec2d final;
final[0] = f[0]*xd[0]+s*xd[1]+c[0];
final[1] = f[1]*xd[1]+c[1];
if (objectPoints.depth() == CV_32F)
{
xpf[i] = final;
}
else
{
xpd[i] = final;
}
/*xpd[i][0] = f[0]*xd[0]+s*xd[1]+c[0];
xpd[i][1] = f[1]*xd[1]+c[1];*/
if (jacobian.needed())
{
double dXcdR_a[] = {Xw[0],Xw[1],Xw[2],0,0,0,0,0,0,
0,0,0,Xw[0],Xw[1],Xw[2],0,0,0,
0,0,0,0,0,0,Xw[0],Xw[1],Xw[2]};
Matx<double,3, 9> dXcdR(dXcdR_a);
Matx33d dXcdom = dXcdR * dRdom.t();
double r_1 = 1.0/norm(Xc);
double r_3 = pow(r_1,3);
Matx33d dXsdXc(r_1-Xc[0]*Xc[0]*r_3, -(Xc[0]*Xc[1])*r_3, -(Xc[0]*Xc[2])*r_3,
-(Xc[0]*Xc[1])*r_3, r_1-Xc[1]*Xc[1]*r_3, -(Xc[1]*Xc[2])*r_3,
-(Xc[0]*Xc[2])*r_3, -(Xc[1]*Xc[2])*r_3, r_1-Xc[2]*Xc[2]*r_3);
Matx23d dxudXs(1/(Xs[2]+xi), 0, -Xs[0]/(Xs[2]+xi)/(Xs[2]+xi),
0, 1/(Xs[2]+xi), -Xs[1]/(Xs[2]+xi)/(Xs[2]+xi));
// pre-compute some reusable things
double temp1 = 2*k1*xu[0] + 4*k2*xu[0]*r2;
double temp2 = 2*k1*xu[1] + 4*k2*xu[1]*r2;
Matx22d dxddxu(k2*r4+6*p2*xu[0]+2*p1*xu[1]+xu[0]*temp1+k1*r2+1, 2*p1*xu[0]+2*p2*xu[1]+xu[0]*temp2,
2*p1*xu[0]+2*p2*xu[1]+xu[1]*temp1, k2*r4+2*p2*xu[0]+6*p1*xu[1]+xu[1]*temp2+k1*r2+1);
Matx22d dxpddxd(f[0], s,
0, f[1]);
Matx23d dxpddXc = dxpddxd * dxddxu * dxudXs * dXsdXc;
// derivative of xpd respect to om
Matx23d dxpddom = dxpddXc * dXcdom;
Matx33d dXcdT(1.0,0.0,0.0,
0.0,1.0,0.0,
0.0,0.0,1.0);
// derivative of xpd respect to T
Matx23d dxpddT = dxpddXc * dXcdT;
Matx21d dxudxi(-Xs[0]/(Xs[2]+xi)/(Xs[2]+xi),
-Xs[1]/(Xs[2]+xi)/(Xs[2]+xi));
// derivative of xpd respect to xi
Matx21d dxpddxi = dxpddxd * dxddxu * dxudxi;
Matx<double,2,4> dxddkp(xu[0]*r2, xu[0]*r4, 2*xu[0]*xu[1], r2+2*xu[0]*xu[0],
xu[1]*r2, xu[1]*r4, r2+2*xu[1]*xu[1], 2*xu[0]*xu[1]);
// derivative of xpd respect to kp
Matx<double,2,4> dxpddkp = dxpddxd * dxddkp;
// derivative of xpd respect to f
Matx22d dxpddf(xd[0], 0,
0, xd[1]);
// derivative of xpd respect to c
Matx22d dxpddc(1, 0,
0, 1);
Jn[0].dom = dxpddom.row(0);
Jn[1].dom = dxpddom.row(1);
Jn[0].dT = dxpddT.row(0);
Jn[1].dT = dxpddT.row(1);
Jn[0].dkp = dxpddkp.row(0);
Jn[1].dkp = dxpddkp.row(1);
Jn[0].dxi = dxpddxi(0,0);
Jn[1].dxi = dxpddxi(1,0);
Jn[0].df = dxpddf.row(0);
Jn[1].df = dxpddf.row(1);
Jn[0].dc = dxpddc.row(0);
Jn[1].dc = dxpddc.row(1);
Jn[0].ds = xd[1];
Jn[1].ds = 0;
Jn += 2;
}
}
}
/////////////////////////////////////////////////////////////////////////////
//////// undistortPoints
void cv::omnidir::undistortPoints( InputArray distorted, OutputArray undistorted,
InputArray K, InputArray D, InputArray xi, InputArray R)
{
CV_Assert(distorted.type() == CV_64FC2 || distorted.type() == CV_32FC2);
CV_Assert(R.empty() || (!R.empty() && (R.size() == Size(3, 3) || R.total() * R.channels() == 3)
&& (R.depth() == CV_64F || R.depth() == CV_32F)));
CV_Assert((D.depth() == CV_64F || D.depth() == CV_32F) && D.total() == 4);
CV_Assert(K.size() == Size(3, 3) && (K.depth() == CV_64F || K.depth() == CV_32F));
CV_Assert(xi.total() == 1 && (xi.depth() == CV_64F || xi.depth() == CV_32F));
undistorted.create(distorted.size(), distorted.type());
cv::Vec2d f, c;
double s = 0.0;
if (K.depth() == CV_32F)
{
Matx33f camMat = K.getMat();
f = Vec2f(camMat(0,0), camMat(1,1));
c = Vec2f(camMat(0,2), camMat(1,2));
s = (double)camMat(0,1);
}
else if (K.depth() == CV_64F)
{
Matx33d camMat = K.getMat();
f = Vec2d(camMat(0,0), camMat(1,1));
c = Vec2d(camMat(0,2), camMat(1,2));
s = camMat(0,1);
}
Vec4d kp = D.depth()==CV_32F ? (Vec4d)*D.getMat().ptr<Vec4f>():(Vec4d)*D.getMat().ptr<Vec4d>();
Vec2d k = Vec2d(kp[0], kp[1]);
Vec2d p = Vec2d(kp[2], kp[3]);
double _xi = xi.depth() == CV_32F ? (double)*xi.getMat().ptr<float>() : *xi.getMat().ptr<double>();
cv::Matx33d RR = cv::Matx33d::eye();
// R is om
if(!R.empty() && R.total()*R.channels() == 3)
{
cv::Vec3d rvec;
R.getMat().convertTo(rvec, CV_64F);
cv::Rodrigues(rvec, RR);
}
else if (!R.empty() && R.size() == Size(3,3))
{
R.getMat().convertTo(RR, CV_64F);
}
const cv::Vec2d *srcd = distorted.getMat().ptr<cv::Vec2d>();
const cv::Vec2f *srcf = distorted.getMat().ptr<cv::Vec2f>();
cv::Vec2d *dstd = undistorted.getMat().ptr<cv::Vec2d>();
cv::Vec2f *dstf = undistorted.getMat().ptr<cv::Vec2f>();
int n = (int)distorted.total();
for (int i = 0; i < n; i++)
{
Vec2d pi = distorted.depth() == CV_32F ? (Vec2d)srcf[i]:(Vec2d)srcd[i]; // image point
Vec2d pp((pi[0]*f[1]-c[0]*f[1]-s*(pi[1]-c[1]))/(f[0]*f[1]), (pi[1]-c[1])/f[1]); //plane
Vec2d pu = pp; // points without distortion
// remove distortion iteratively
for (int j = 0; j < 20; j++)
{
double r2 = pu[0]*pu[0] + pu[1]*pu[1];
double r4 = r2*r2;
pu[0] = (pp[0] - 2*p[0]*pu[0]*pu[1] - p[1]*(r2+2*pu[0]*pu[0])) / (1 + k[0]*r2 + k[1]*r4);
pu[1] = (pp[1] - 2*p[1]*pu[0]*pu[1] - p[0]*(r2+2*pu[1]*pu[1])) / (1 + k[0]*r2 + k[1]*r4);
}
// project to unit sphere
double r2 = pu[0]*pu[0] + pu[1]*pu[1];
double a = (r2 + 1);
double b = 2*_xi*r2;
double cc = r2*_xi*_xi-1;
double Zs = (-b + sqrt(b*b - 4*a*cc))/(2*a);
Vec3d Xw = Vec3d(pu[0]*(Zs + _xi), pu[1]*(Zs +_xi), Zs);
// rotate
Xw = RR * Xw;
// project back to sphere
Vec3d Xs = Xw / cv::norm(Xw);
// reproject to camera plane
Vec3d ppu = Vec3d(Xs[0]/(Xs[2]+_xi), Xs[1]/(Xs[2]+_xi), 1.0);
if (undistorted.depth() == CV_32F)
{
dstf[i] = Vec2f((float)ppu[0], (float)ppu[1]);
}
else if (undistorted.depth() == CV_64F)
{
dstd[i] = Vec2d(ppu[0], ppu[1]);
}
}
}
/////////////////////////////////////////////////////////////////////////////
//////// cv::omnidir::initUndistortRectifyMap
void cv::omnidir::initUndistortRectifyMap(InputArray K, InputArray D, InputArray xi, InputArray R, InputArray P,
const cv::Size& size, int m1type, OutputArray map1, OutputArray map2, int flags)
{
CV_Assert( m1type == CV_16SC2 || m1type == CV_32F || m1type <=0 );
map1.create( size, m1type <= 0 ? CV_16SC2 : m1type );
map2.create( size, map1.type() == CV_16SC2 ? CV_16UC1 : CV_32F );
CV_Assert((K.depth() == CV_32F || K.depth() == CV_64F) && (D.depth() == CV_32F || D.depth() == CV_64F));
CV_Assert(K.size() == Size(3, 3) && (D.empty() || D.total() == 4));
CV_Assert(P.empty()|| (P.depth() == CV_32F || P.depth() == CV_64F));
CV_Assert(P.empty() || P.size() == Size(3, 3) || P.size() == Size(4, 3));
CV_Assert(R.empty() || (R.depth() == CV_32F || R.depth() == CV_64F));
CV_Assert(R.empty() || R.size() == Size(3, 3) || R.total() * R.channels() == 3);
CV_Assert(flags == RECTIFY_PERSPECTIVE || flags == RECTIFY_CYLINDRICAL || flags == RECTIFY_LONGLATI
|| flags == RECTIFY_STEREOGRAPHIC);
CV_Assert(xi.total() == 1 && (xi.depth() == CV_32F || xi.depth() == CV_64F));
cv::Vec2d f, c;
double s;
if (K.depth() == CV_32F)
{
Matx33f camMat = K.getMat();
f = Vec2f(camMat(0, 0), camMat(1, 1));
c = Vec2f(camMat(0, 2), camMat(1, 2));
s = (double)camMat(0,1);
}
else
{
Matx33d camMat = K.getMat();
f = Vec2d(camMat(0, 0), camMat(1, 1));
c = Vec2d(camMat(0, 2), camMat(1, 2));
s = camMat(0,1);
}
Vec4d kp = Vec4d::all(0);
if (!D.empty())
kp = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr<Vec4f>(): *D.getMat().ptr<Vec4d>();
double _xi = xi.depth() == CV_32F ? (double)*xi.getMat().ptr<float>() : *xi.getMat().ptr<double>();
Vec2d k = Vec2d(kp[0], kp[1]);
Vec2d p = Vec2d(kp[2], kp[3]);
cv::Matx33d RR = cv::Matx33d::eye();
if (!R.empty() && R.total() * R.channels() == 3)
{
cv::Vec3d rvec;
R.getMat().convertTo(rvec, CV_64F);
cv::Rodrigues(rvec, RR);
}
else if (!R.empty() && R.size() == Size(3, 3))
R.getMat().convertTo(RR, CV_64F);
cv::Matx33d PP = cv::Matx33d::eye();
if (!P.empty())
P.getMat().colRange(0, 3).convertTo(PP, CV_64F);
else
PP = K.getMat();
cv::Matx33d iKR = (PP*RR).inv(cv::DECOMP_SVD);
cv::Matx33d iK = PP.inv(cv::DECOMP_SVD);
cv::Matx33d iR = RR.inv(cv::DECOMP_SVD);
if (flags == omnidir::RECTIFY_PERSPECTIVE)
{
for (int i = 0; i < size.height; ++i)
{
float* m1f = map1.getMat().ptr<float>(i);
float* m2f = map2.getMat().ptr<float>(i);
short* m1 = (short*)m1f;
ushort* m2 = (ushort*)m2f;
double _x = i*iKR(0, 1) + iKR(0, 2),
_y = i*iKR(1, 1) + iKR(1, 2),
_w = i*iKR(2, 1) + iKR(2, 2);
for(int j = 0; j < size.width; ++j, _x+=iKR(0,0), _y+=iKR(1,0), _w+=iKR(2,0))
{
// project back to unit sphere
double r = sqrt(_x*_x + _y*_y + _w*_w);
double Xs = _x / r;
double Ys = _y / r;
double Zs = _w / r;
// project to image plane
double xu = Xs / (Zs + _xi),
yu = Ys / (Zs + _xi);
// add distortion
double r2 = xu*xu + yu*yu;
double r4 = r2*r2;
double xd = (1+k[0]*r2+k[1]*r4)*xu + 2*p[0]*xu*yu + p[1]*(r2+2*xu*xu);
double yd = (1+k[0]*r2+k[1]*r4)*yu + p[0]*(r2+2*yu*yu) + 2*p[1]*xu*yu;
// to image pixel
double u = f[0]*xd + s*yd + c[0];
double v = f[1]*yd + c[1];
if( m1type == CV_16SC2 )
{
int iu = cv::saturate_cast<int>(u*cv::INTER_TAB_SIZE);
int iv = cv::saturate_cast<int>(v*cv::INTER_TAB_SIZE);
m1[j*2+0] = (short)(iu >> cv::INTER_BITS);
m1[j*2+1] = (short)(iv >> cv::INTER_BITS);
m2[j] = (ushort)((iv & (cv::INTER_TAB_SIZE-1))*cv::INTER_TAB_SIZE + (iu & (cv::INTER_TAB_SIZE-1)));
}
else if( m1type == CV_32FC1 )
{
m1f[j] = (float)u;
m2f[j] = (float)v;
}
}
}
}
else if(flags == omnidir::RECTIFY_CYLINDRICAL || flags == omnidir::RECTIFY_LONGLATI ||
flags == omnidir::RECTIFY_STEREOGRAPHIC)
{
for (int i = 0; i < size.height; ++i)
{
float* m1f = map1.getMat().ptr<float>(i);
float* m2f = map2.getMat().ptr<float>(i);
short* m1 = (short*)m1f;
ushort* m2 = (ushort*)m2f;
// for RECTIFY_LONGLATI, theta and h are longittude and latitude
double theta = i*iK(0, 1) + iK(0, 2),
h = i*iK(1, 1) + iK(1, 2);
for (int j = 0; j < size.width; ++j, theta+=iK(0,0), h+=iK(1,0))
{
double _xt = 0.0, _yt = 0.0, _wt = 0.0;
if (flags == omnidir::RECTIFY_CYLINDRICAL)
{
//_xt = std::sin(theta);
//_yt = h;
//_wt = std::cos(theta);
_xt = std::cos(theta);
_yt = std::sin(theta);
_wt = h;
}
else if (flags == omnidir::RECTIFY_LONGLATI)
{
_xt = -std::cos(theta);
_yt = -std::sin(theta) * std::cos(h);
_wt = std::sin(theta) * std::sin(h);
}
else if (flags == omnidir::RECTIFY_STEREOGRAPHIC)
{
double a = theta*theta + h*h + 4;
double b = -2*theta*theta - 2*h*h;
double c2 = theta*theta + h*h -4;
_yt = (-b-std::sqrt(b*b - 4*a*c2))/(2*a);
_xt = theta*(1 - _yt) / 2;
_wt = h*(1 - _yt) / 2;
}
double _x = iR(0,0)*_xt + iR(0,1)*_yt + iR(0,2)*_wt;
double _y = iR(1,0)*_xt + iR(1,1)*_yt + iR(1,2)*_wt;
double _w = iR(2,0)*_xt + iR(2,1)*_yt + iR(2,2)*_wt;
double r = sqrt(_x*_x + _y*_y + _w*_w);
double Xs = _x / r;
double Ys = _y / r;
double Zs = _w / r;
// project to image plane
double xu = Xs / (Zs + _xi),
yu = Ys / (Zs + _xi);
// add distortion
double r2 = xu*xu + yu*yu;
double r4 = r2*r2;
double xd = (1+k[0]*r2+k[1]*r4)*xu + 2*p[0]*xu*yu + p[1]*(r2+2*xu*xu);
double yd = (1+k[0]*r2+k[1]*r4)*yu + p[0]*(r2+2*yu*yu) + 2*p[1]*xu*yu;
// to image pixel
double u = f[0]*xd + s*yd + c[0];
double v = f[1]*yd + c[1];
if( m1type == CV_16SC2 )
{
int iu = cv::saturate_cast<int>(u*cv::INTER_TAB_SIZE);
int iv = cv::saturate_cast<int>(v*cv::INTER_TAB_SIZE);
m1[j*2+0] = (short)(iu >> cv::INTER_BITS);
m1[j*2+1] = (short)(iv >> cv::INTER_BITS);
m2[j] = (ushort)((iv & (cv::INTER_TAB_SIZE-1))*cv::INTER_TAB_SIZE + (iu & (cv::INTER_TAB_SIZE-1)));
}
else if( m1type == CV_32FC1 )
{
m1f[j] = (float)u;
m2f[j] = (float)v;
}
}
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// cv::omnidir::undistortImage
void cv::omnidir::undistortImage(InputArray distorted, OutputArray undistorted,
InputArray K, InputArray D, InputArray xi, int flags, InputArray Knew, const Size& new_size, InputArray R)
{
Size size = new_size.area() != 0 ? new_size : distorted.size();
cv::Mat map1, map2;
omnidir::initUndistortRectifyMap(K, D, xi, R, Knew, size, CV_16SC2, map1, map2, flags);
cv::remap(distorted, undistorted, map1, map2, INTER_LINEAR, BORDER_CONSTANT);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// cv::omnidir::internal::initializeCalibration
void cv::omnidir::internal::initializeCalibration(InputArrayOfArrays patternPoints, InputArrayOfArrays imagePoints, Size size,
OutputArrayOfArrays omAll, OutputArrayOfArrays tAll, OutputArray K, double& xi, OutputArray idx)
{
// For details please refer to Section III from Li's IROS 2013 paper
double u0 = size.width / 2;
double v0 = size.height / 2;
int n_img = (int)imagePoints.total();
std::vector<cv::Vec3d> v_omAll(n_img), v_tAll(n_img);
std::vector<double> gammaAll(n_img);
K.create(3, 3, CV_64F);
Mat _K;
for (int image_idx = 0; image_idx < n_img; ++image_idx)
{
cv::Mat objPoints, imgPoints;
patternPoints.getMat(image_idx).copyTo(objPoints);
imagePoints.getMat(image_idx).copyTo(imgPoints);
int n_point = imgPoints.rows * imgPoints.cols;
if (objPoints.rows != n_point)
objPoints = objPoints.reshape(3, n_point);
if (imgPoints.rows != n_point)
imgPoints = imgPoints.reshape(2, n_point);
// objectPoints should be 3-channel data, imagePoints should be 2-channel data
CV_Assert(objPoints.type() == CV_64FC3 && imgPoints.type() == CV_64FC2 );
std::vector<cv::Mat> xy, uv;
cv::split(objPoints, xy);
cv::split(imgPoints, uv);
cv::Mat x = xy[0].reshape(1, n_point), y = xy[1].reshape(1, n_point),
u = uv[0].reshape(1, n_point) - u0, v = uv[1].reshape(1, n_point) - v0;
cv::Mat sqrRho = u.mul(u) + v.mul(v);
// compute extrinsic parameters
cv::Mat M(n_point, 6, CV_64F);
Mat(-v.mul(x)).copyTo(M.col(0));
Mat(-v.mul(y)).copyTo(M.col(1));
Mat(u.mul(x)).copyTo(M.col(2));
Mat(u.mul(y)).copyTo(M.col(3));
Mat(-v).copyTo(M.col(4));
Mat(u).copyTo(M.col(5));
Mat W,U,V;
cv::SVD::compute(M, W, U, V,SVD::FULL_UV);
V = V.t();
double miniReprojectError = 1e5;
// the signs of r1, r2, r3 are unknown, so they can be flipped.
for (int coef = 1; coef >= -1; coef-=2)
{
double r11 = V.at<double>(0, 5) * coef;
double r12 = V.at<double>(1, 5) * coef;
double r21 = V.at<double>(2, 5) * coef;
double r22 = V.at<double>(3, 5) * coef;
double t1 = V.at<double>(4, 5) * coef;
double t2 = V.at<double>(5, 5) * coef;
Mat roots;
double r31s;
solvePoly(Matx13d(-(r11*r12+r21*r22)*(r11*r12+r21*r22), r11*r11+r21*r21-r12*r12-r22*r22, 1), roots);
if (roots.at<Vec2d>(0)[0] > 0)
r31s = sqrt(roots.at<Vec2d>(0)[0]);
else
r31s = sqrt(roots.at<Vec2d>(1)[0]);
for (int coef2 = 1; coef2 >= -1; coef2-=2)
{
double r31 = r31s * coef2;
double r32 = -(r11*r12 + r21*r22) / r31;
cv::Vec3d r1(r11, r21, r31);
cv::Vec3d r2(r12, r22, r32);
cv::Vec3d t(t1, t2, 0);
double scale = 1 / cv::norm(r1);
r1 = r1 * scale;
r2 = r2 * scale;
t = t * scale;
// compute intrisic parameters
// Form equations in Scaramuzza's paper
// A Toolbox for Easily Calibrating Omnidirectional Cameras
Mat A(n_point*2, 3, CV_64F);
Mat((r1[1]*x + r2[1]*y + t[1])/2).copyTo(A.rowRange(0, n_point).col(0));
Mat((r1[0]*x + r2[0]*y + t[0])/2).copyTo(A.rowRange(n_point, 2*n_point).col(0));
Mat(-A.col(0).rowRange(0, n_point).mul(sqrRho)).copyTo(A.col(1).rowRange(0, n_point));
Mat(-A.col(0).rowRange(n_point, 2*n_point).mul(sqrRho)).copyTo(A.col(1).rowRange(n_point, 2*n_point));
Mat(-v).copyTo(A.rowRange(0, n_point).col(2));
Mat(-u).copyTo(A.rowRange(n_point, 2*n_point).col(2));
// Operation to avoid bad numerical-condition of A
Vec3d maxA, minA;
for (int j = 0; j < A.cols; j++)
{
cv::minMaxLoc(cv::abs(A.col(j)), &minA[j], &maxA[j]);
A.col(j) = A.col(j) / maxA[j];
}
Mat B(n_point*2 , 1, CV_64F);
Mat(v.mul(r1[2]*x + r2[2]*y)).copyTo(B.rowRange(0, n_point));
Mat(u.mul(r1[2]*x + r2[2]*y)).copyTo(B.rowRange(n_point, 2*n_point));
Mat res = A.inv(DECOMP_SVD) * B;
res = res.mul(1/Mat(maxA));
double gamma = sqrt(res.at<double>(0) / res.at<double>(1));
t[2] = res.at<double>(2);
cv::Vec3d r3 = r1.cross(r2);
Matx33d R(r1[0], r2[0], r3[0],
r1[1], r2[1], r3[1],
r1[2], r2[2], r3[2]);
Vec3d om;
Rodrigues(R, om);
// project pattern points to images
Mat projedImgPoints;
Matx33d Kc(gamma, 0, u0, 0, gamma, v0, 0, 0, 1);
// reproject error
cv::omnidir::projectPoints(objPoints, projedImgPoints, om, t, Kc, 1, Matx14d(0, 0, 0, 0), cv::noArray());
double reprojectError = omnidir::internal::computeMeanReproErr(imgPoints, projedImgPoints);
// if this reproject error is smaller
if (reprojectError < miniReprojectError)
{
miniReprojectError = reprojectError;
v_omAll[image_idx] = om;
v_tAll[image_idx] = t;
gammaAll[image_idx] = gamma;
}
}
}
}
// filter initial results whose reproject errors are too large
std::vector<double> reProjErrorFilter,v_gammaFilter;
std::vector<Vec3d> omFilter, tFilter;
double gammaFinal = 0;
// choose median value
size_t n = gammaAll.size() / 2;
std::nth_element(gammaAll.begin(), gammaAll.begin()+n, gammaAll.end());
gammaFinal = gammaAll[n];
_K = Mat(Matx33d(gammaFinal, 0, u0, 0, gammaFinal, v0, 0, 0, 1));
_K.convertTo(K, CV_64F);
std::vector<int> _idx;
// recompute reproject error using the final gamma
for (int i = 0; i< n_img; i++)
{
Mat _projected;
cv::omnidir::projectPoints(patternPoints.getMat(i), _projected, v_omAll[i], v_tAll[i], _K, 1, Matx14d(0, 0, 0, 0), cv::noArray());
double _error = omnidir::internal::computeMeanReproErr(imagePoints.getMat(i), _projected);
if(_error < 100)
{
_idx.push_back(i);
omFilter.push_back(v_omAll[i]);
tFilter.push_back(v_tAll[i]);
}
}
if (idx.needed())
{
idx.create(1, (int)_idx.size(), CV_32S);
Mat idx_m = idx.getMat();
for (int j = 0; j < (int)idx_m.total(); j++)
{
idx_m.at<int>(j) = _idx[j];
}
}
if(omAll.kind() == _InputArray::STD_VECTOR_MAT)
{
for (int i = 0; i < (int)omFilter.size(); ++i)
{
omAll.getMat(i) = Mat(omFilter[i]);
tAll.getMat(i) = Mat(tFilter[i]);
}
}
else
{
cv::Mat(omFilter).convertTo(omAll, CV_64FC3);
cv::Mat(tFilter).convertTo(tAll, CV_64FC3);
}
xi = 1;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// cv::omnidir::internal::initializeStereoCalibration
void cv::omnidir::internal::initializeStereoCalibration(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
const Size& size1, const Size& size2, OutputArray om, OutputArray T, OutputArrayOfArrays omL, OutputArrayOfArrays tL, OutputArray K1, OutputArray D1, OutputArray K2, OutputArray D2,
double &xi1, double &xi2, int flags, OutputArray idx)
{
Mat idx1, idx2;
Matx33d _K1, _K2;
Matx14d _D1, _D2;
Mat _xi1m, _xi2m;
std::vector<Vec3d> omAllTemp1, omAllTemp2, tAllTemp1, tAllTemp2;
omnidir::calibrate(objectPoints, imagePoints1, size1, _K1, _xi1m, _D1, omAllTemp1, tAllTemp1, flags, TermCriteria(3, 100, 1e-6), idx1);
omnidir::calibrate(objectPoints, imagePoints2, size2, _K2, _xi2m, _D2, omAllTemp2, tAllTemp2, flags, TermCriteria(3, 100, 1e-6), idx2);
// find the intersection idx
Mat interIdx1, interIdx2, interOri;
getInterset(idx1, idx2, interIdx1, interIdx2, interOri);
if (idx.empty())
idx.create(1, (int)interOri.total(), CV_32S);
interOri.copyTo(idx.getMat());
int n_inter = (int)interIdx1.total();
std::vector<Vec3d> omAll1(n_inter), omAll2(n_inter), tAll1(n_inter), tAll2(n_inter);
for(int i = 0; i < (int)interIdx1.total(); ++i)
{
omAll1[i] = omAllTemp1[interIdx1.at<int>(i)];
tAll1[i] = tAllTemp1[interIdx1.at<int>(i)];
omAll2[i] = omAllTemp2[interIdx2.at<int>(i)];
tAll2[i] = tAllTemp2[interIdx2.at<int>(i)];
}
// initialize R,T
Mat omEstAll(1, n_inter, CV_64FC3), tEstAll(1, n_inter, CV_64FC3);
Mat R1, R2, T1, T2, omLR, TLR, RLR;
for (int i = 0; i < n_inter; ++i)
{
Rodrigues(omAll1[i], R1);
Rodrigues(omAll2[i], R2);
T1 = Mat(tAll1[i]).reshape(1, 3);
T2 = Mat(tAll2[i]).reshape(1, 3);
RLR = R2 * R1.t();
TLR = T2 - RLR*T1;
Rodrigues(RLR, omLR);
omLR.reshape(3, 1).copyTo(omEstAll.col(i));
TLR.reshape(3, 1).copyTo(tEstAll.col(i));
}
Vec3d omEst = internal::findMedian3(omEstAll);
Vec3d tEst = internal::findMedian3(tEstAll);
Mat(omEst).copyTo(om.getMat());
Mat(tEst).copyTo(T.getMat());
if (omL.empty())
{
omL.create((int)omAll1.size(), 1, CV_64FC3);
}
if (tL.empty())
{
tL.create((int)tAll1.size(), 1, CV_64FC3);
}
if(omL.kind() == _InputArray::STD_VECTOR_MAT)
{
for(int i = 0; i < n_inter; ++i)
{
omL.create(3, 1, CV_64F, i, true);
tL.create(3, 1, CV_64F, i, true);
omL.getMat(i) = Mat(omAll1[i]);
tL.getMat(i) = Mat(tAll1[i]);
}
}
else
{
cv::Mat(omAll1).convertTo(omL, CV_64FC3);
cv::Mat(tAll1).convertTo(tL, CV_64FC3);
}
if (K1.empty())
{
K1.create(3, 3, CV_64F);
K2.create(3, 3, CV_64F);
}
if (D1.empty())
{
D1.create(1, 4, CV_64F);
D2.create(1, 4, CV_64F);
}
Mat(_K1).copyTo(K1.getMat());
Mat(_K2).copyTo(K2.getMat());
Mat(_D1).copyTo(D1.getMat());
Mat(_D2).copyTo(D2.getMat());
xi1 = _xi1m.at<double>(0);
xi2 = _xi2m.at<double>(0);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// cv::omnidir::internal::computeJacobian
void cv::omnidir::internal::computeJacobian(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags, double epsilon)
{
CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2);
int n = (int)objectPoints.total();
Mat JTJ = Mat::zeros(10 + 6*n, 10 + 6*n, CV_64F);
JTJ_inv = Mat::zeros(10 + 6*n, 10 + 6*n, CV_64F);
JTE = Mat::zeros(10 + 6*n, 1, CV_64F);
int nPointsAll = 0;
for (int i = 0; i < n; ++i)
{
nPointsAll += (int)objectPoints.getMat(i).total();
}
Mat J = Mat::zeros(2*nPointsAll, 10+6*n, CV_64F);
Mat exAll = Mat::zeros(2*nPointsAll, 10+6*n, CV_64F);
double *para = parameters.getMat().ptr<double>();
Matx33d K(para[6*n], para[6*n+2], para[6*n+3],
0, para[6*n+1], para[6*n+4],
0, 0, 1);
Matx14d D(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9]);
double xi = para[6*n+5];
for (int i = 0; i < n; i++)
{
Mat objPoints, imgPoints, om, T;
objectPoints.getMat(i).copyTo(objPoints);
imagePoints.getMat(i).copyTo(imgPoints);
objPoints = objPoints.reshape(3, objPoints.rows*objPoints.cols);
imgPoints = imgPoints.reshape(2, imgPoints.rows*imgPoints.cols);
om = parameters.getMat().colRange(i*6, i*6+3);
T = parameters.getMat().colRange(i*6+3, (i+1)*6);
Mat imgProj, jacobian;
omnidir::projectPoints(objPoints, imgProj, om, T, K, xi, D, jacobian);
Mat projError = imgPoints - imgProj;
// The intrinsic part of Jacobian
Mat JIn(jacobian.rows, 10, CV_64F);
Mat JEx(jacobian.rows, 6, CV_64F);
jacobian.colRange(6, 16).copyTo(JIn);
jacobian.colRange(0, 6).copyTo(JEx);
JTJ(Rect(6*n, 6*n, 10, 10)) = JTJ(Rect(6*n, 6*n, 10, 10)) + JIn.t()*JIn;
JTJ(Rect(i*6, i*6, 6, 6)) = JEx.t() * JEx;
Mat JExTIn = JEx.t() * JIn;
JExTIn.copyTo(JTJ(Rect(6*n, i*6, 10, 6)));
Mat(JIn.t()*JEx).copyTo(JTJ(Rect(i*6, 6*n, 6, 10)));
JTE(Rect(0, 6*n, 1, 10)) = JTE(Rect(0, 6*n,1, 10)) + JIn.t() * projError.reshape(1, 2*(int)projError.total());
JTE(Rect(0, i*6, 1, 6)) = JEx.t() * projError.reshape(1, 2*(int)projError.total());
//int nPoints = objectPoints.getMat(i).total();
//JIn.copyTo(J(Rect(6*n, i*nPoints*2, 10, nPoints*2)));
//JEx.copyTo(J(Rect(6*i, i*nPoints*2, 6, nPoints*2)));
//projError.reshape(1, 2*projError.rows).copyTo(exAll.rowRange(i*2*nPoints, (i+1)*2*nPoints));
}
//JTJ = J.t()*J;
//JTE = J.t()*exAll;
std::vector<int> _idx(6*n+10, 1);
flags2idx(flags, _idx, n);
subMatrix(JTJ, JTJ, _idx, _idx);
subMatrix(JTE, JTE, std::vector<int>(1, 1), _idx);
// in case JTJ is singular
//SVD svd(JTJ, SVD::NO_UV);
//double cond = svd.w.at<double>(0)/svd.w.at<double>(5);
//if (cond_JTJ.needed())
//{
// cond_JTJ.create(1, 1, CV_64F);
// cond_JTJ.getMat().at<double>(0) = cond;
//}
//double epsilon = 1e-4*std::exp(cond);
JTJ_inv = Mat(JTJ+epsilon).inv();
}
void cv::omnidir::internal::computeJacobianStereo(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags, double epsilon)
{
CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
CV_Assert(!imagePoints1.empty() && imagePoints1.type() == CV_64FC2);
CV_Assert(!imagePoints2.empty() && imagePoints2.type() == CV_64FC2);
CV_Assert((imagePoints1.total() == imagePoints2.total()) && (imagePoints1.total() == objectPoints.total()));
// compute Jacobian matrix by naive way
int n_img = (int)objectPoints.total();
int n_points = (int)objectPoints.getMat(0).total();
Mat J = Mat::zeros(4 * n_points * n_img, 20 + 6 * (n_img + 1), CV_64F);
Mat exAll = Mat::zeros(4 * n_points * n_img, 1, CV_64F);
double *para = parameters.getMat().ptr<double>();
int offset1 = (n_img + 1) * 6;
int offset2 = offset1 + 10;
Matx33d K1(para[offset1], para[offset1+2], para[offset1+3],
0, para[offset1+1], para[offset1+4],
0, 0, 1);
Matx14d D1(para[offset1+6], para[offset1+7], para[offset1+8], para[offset1+9]);
double xi1 = para[offset1+5];
Matx33d K2(para[offset2], para[offset2+2], para[offset2+3],
0, para[offset2+1], para[offset2+4],
0, 0, 1);
Matx14d D2(para[offset2+6], para[offset2+7], para[offset2+8], para[offset2+9]);
double xi2 = para[offset2+5];
Mat om = parameters.getMat().reshape(1, 1).colRange(0, 3);
Mat T = parameters.getMat().reshape(1, 1).colRange(3, 6);
for (int i = 0; i < n_img; i++)
{
Mat objPointsi, imgPoints1i, imgPoints2i, om1, T1;
objectPoints.getMat(i).copyTo(objPointsi);
imagePoints1.getMat(i).copyTo(imgPoints1i);
imagePoints2.getMat(i).copyTo(imgPoints2i);
objPointsi = objPointsi.reshape(3, objPointsi.rows*objPointsi.cols);
imgPoints1i = imgPoints1i.reshape(2, imgPoints1i.rows*imgPoints1i.cols);
imgPoints2i = imgPoints2i.reshape(2, imgPoints2i.rows*imgPoints2i.cols);
om1 = parameters.getMat().reshape(1, 1).colRange((1 + i) * 6, (1 + i) * 6 + 3);
T1 = parameters.getMat().reshape(1, 1).colRange((1 + i) * 6 + 3, (i + 1) * 6 + 6);
Mat imgProj1, imgProj2, jacobian1, jacobian2;
// jacobian for left image
cv::omnidir::projectPoints(objPointsi, imgProj1, om1, T1, K1, xi1, D1, jacobian1);
Mat projError1 = imgPoints1i - imgProj1;
//Mat JIn1(jacobian1.rows, 10, CV_64F);
//Mat JEx1(jacobian1.rows, 6, CV_64F);
jacobian1.colRange(6, 16).copyTo(J(Rect(6*(n_img+1), i*n_points*4, 10, n_points*2)));
jacobian1.colRange(0, 6).copyTo(J(Rect(6+i*6, i*n_points*4, 6, n_points*2)));
projError1.reshape(1, 2*n_points).copyTo(exAll.rowRange(i*4*n_points, (i*4+2)*n_points));
//jacobian for right image
Mat om2, T2, dom2dom1, dom2dT1, dom2dom, dom2dT, dT2dom1, dT2dT1, dT2dom, dT2dT;
cv::omnidir::internal::compose_motion(om1, T1, om, T, om2, T2, dom2dom1, dom2dT1, dom2dom, dom2dT, dT2dom1, dT2dT1, dT2dom, dT2dT);
cv::omnidir::projectPoints(objPointsi, imgProj2, om2, T2, K2, xi2, D2, jacobian2);
Mat projError2 = imgPoints2i - imgProj2;
projError2.reshape(1, 2*n_points).copyTo(exAll.rowRange((i*4+2)*n_points, (i*4+4)*n_points));
Mat dxrdom = jacobian2.colRange(0, 3) * dom2dom + jacobian2.colRange(3, 6) * dT2dom;
Mat dxrdT = jacobian2.colRange(0, 3) * dom2dT + jacobian2.colRange(3, 6) * dT2dT;
Mat dxrdom1 = jacobian2.colRange(0, 3) * dom2dom1 + jacobian2.colRange(3, 6) * dT2dom1;
Mat dxrdT1 = jacobian2.colRange(0, 3) * dom2dT1 + jacobian2.colRange(3, 6) * dT2dT1;
dxrdom.copyTo(J(Rect(0, (i*4+2)*n_points, 3, n_points*2)));
dxrdT.copyTo(J(Rect(3, (i*4+2)*n_points, 3, n_points*2)));
dxrdom1.copyTo(J(Rect(6+i*6, (i*4+2)*n_points, 3, n_points*2)));
dxrdT1.copyTo(J(Rect(6+i*6+3, (i*4+2)*n_points, 3, n_points*2)));
jacobian2.colRange(6, 16).copyTo(J(Rect(6*(n_img+1)+10, (4*i+2)*n_points, 10, n_points*2)));
}
std::vector<int> _idx(6*(n_img+1)+20, 1);
flags2idxStereo(flags, _idx, n_img);
Mat JTJ = J.t()*J;
JTE = J.t()*exAll;
subMatrix(JTJ, JTJ, _idx, _idx);
subMatrix(JTE, JTE, std::vector<int>(1, 1), _idx);
JTJ_inv = Mat(JTJ+epsilon).inv();
}
// This function is from fisheye.cpp
void cv::omnidir::internal::compose_motion(InputArray _om1, InputArray _T1, InputArray _om2, InputArray _T2, Mat& om3, Mat& T3, Mat& dom3dom1,
Mat& dom3dT1, Mat& dom3dom2, Mat& dom3dT2, Mat& dT3dom1, Mat& dT3dT1, Mat& dT3dom2, Mat& dT3dT2)
{
Mat om1 = _om1.getMat();
Mat om2 = _om2.getMat();
Mat T1 = _T1.getMat().reshape(1, 3);
Mat T2 = _T2.getMat().reshape(1, 3);
//% Rotations:
Mat R1, R2, R3, dR1dom1(9, 3, CV_64FC1), dR2dom2;
Rodrigues(om1, R1, dR1dom1);
Rodrigues(om2, R2, dR2dom2);
//JRodriguesMatlab(dR1dom1, dR1dom1);
//JRodriguesMatlab(dR2dom2, dR2dom2);
dR1dom1 = dR1dom1.t();
dR2dom2 = dR2dom2.t();
R3 = R2 * R1;
Mat dR3dR2, dR3dR1;
//dAB(R2, R1, dR3dR2, dR3dR1);
matMulDeriv(R2, R1, dR3dR2, dR3dR1);
Mat dom3dR3;
Rodrigues(R3, om3, dom3dR3);
//JRodriguesMatlab(dom3dR3, dom3dR3);
dom3dR3 = dom3dR3.t();
dom3dom1 = dom3dR3 * dR3dR1 * dR1dom1;
dom3dom2 = dom3dR3 * dR3dR2 * dR2dom2;
dom3dT1 = Mat::zeros(3, 3, CV_64FC1);
dom3dT2 = Mat::zeros(3, 3, CV_64FC1);
//% Translations:
Mat T3t = R2 * T1;
Mat dT3tdR2, dT3tdT1;
//dAB(R2, T1, dT3tdR2, dT3tdT1);
matMulDeriv(R2, T1, dT3tdR2, dT3tdT1);
Mat dT3tdom2 = dT3tdR2 * dR2dom2;
T3 = T3t + T2;
dT3dT1 = dT3tdT1;
dT3dT2 = Mat::eye(3, 3, CV_64FC1);
dT3dom2 = dT3tdom2;
dT3dom1 = Mat::zeros(3, 3, CV_64FC1);
}
double cv::omnidir::calibrate(InputArray patternPoints, InputArray imagePoints, Size size,
InputOutputArray K, InputOutputArray xi, InputOutputArray D, OutputArrayOfArrays omAll, OutputArrayOfArrays tAll,
int flags, TermCriteria criteria, OutputArray idx)
{
CV_Assert(!patternPoints.empty() && !imagePoints.empty() && patternPoints.total() == imagePoints.total());
CV_Assert((patternPoints.type() == CV_64FC3 && imagePoints.type() == CV_64FC2) ||
(patternPoints.type() == CV_32FC3 && imagePoints.type() == CV_32FC2));
CV_Assert(patternPoints.getMat(0).channels() == 3 && imagePoints.getMat(0).channels() == 2);
CV_Assert((!K.empty() && K.size() == Size(3,3)) || K.empty());
CV_Assert((!D.empty() && D.total() == 4) || D.empty());
CV_Assert((!xi.empty() && xi.total() == 1) || xi.empty());
CV_Assert((!omAll.empty() && omAll.depth() == patternPoints.depth()) || omAll.empty());
CV_Assert((!tAll.empty() && tAll.depth() == patternPoints.depth()) || tAll.empty());
int depth = patternPoints.depth();
std::vector<Mat> _patternPoints, _imagePoints;
for (int i = 0; i < (int)patternPoints.total(); ++i)
{
_patternPoints.push_back(patternPoints.getMat(i));
_imagePoints.push_back(imagePoints.getMat(i));
if (depth == CV_32F)
{
_patternPoints[i].convertTo(_patternPoints[i], CV_64FC3);
_imagePoints[i].convertTo(_imagePoints[i], CV_64FC2);
}
}
double _xi;
// initialization
std::vector<Vec3d> _omAll, _tAll;
Matx33d _K;
Matx14d _D;
Mat _idx;
cv::omnidir::internal::initializeCalibration(_patternPoints, _imagePoints, size, _omAll, _tAll, _K, _xi, _idx);
std::vector<Mat> _patternPointsTmp = _patternPoints;
std::vector<Mat> _imagePointsTmp = _imagePoints;
_patternPoints.clear();
_imagePoints.clear();
// erase
for (int i = 0; i < (int)_idx.total(); i++)
{
_patternPoints.push_back(_patternPointsTmp[_idx.at<int>(i)]);
_imagePoints.push_back(_imagePointsTmp[_idx.at<int>(i)]);
}
int n = (int)_patternPoints.size();
Mat finalParam(1, 10 + 6*n, CV_64F);
Mat currentParam(1, 10 + 6*n, CV_64F);
cv::omnidir::internal::encodeParameters(_K, _omAll, _tAll, Mat::zeros(1,4,CV_64F), _xi, currentParam);
// optimization
const double alpha_smooth = 0.01;
//const double thresh_cond = 1e6;
double change = 1;
for(int iter = 0; ; ++iter)
{
if ((criteria.type == 1 && iter >= criteria.maxCount) ||
(criteria.type == 2 && change <= criteria.epsilon) ||
(criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount)))
break;
double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, (double)iter + 1.0);
Mat JTJ_inv, JTError;
double epsilon = 0.01 * std::pow(0.9, (double)iter/10);
cv::omnidir::internal::computeJacobian(_patternPoints, _imagePoints, currentParam, JTJ_inv, JTError, flags, epsilon);
// GaussCNewton
Mat G = alpha_smooth2*JTJ_inv * JTError;
omnidir::internal::fillFixed(G, flags, n);
finalParam = currentParam + G.t();
change = norm(G) / norm(currentParam);
currentParam = finalParam.clone();
cv::omnidir::internal::decodeParameters(currentParam, _K, _omAll, _tAll, _D, _xi);
//double repr = internal::computeMeanReproErr(_patternPoints, _imagePoints, _K, _D, _xi, _omAll, _tAll);
}
cv::omnidir::internal::decodeParameters(currentParam, _K, _omAll, _tAll, _D, _xi);
//double repr = internal::computeMeanReproErr(_patternPoints, _imagePoints, _K, _D, _xi, _omAll, _tAll);
if (omAll.needed())
{
omAll.create((int)_omAll.size(), 1, CV_64FC3);
}
if (tAll.needed())
{
tAll.create((int)_tAll.size(), 1, CV_64FC3);
}
if (omAll.kind() == _InputArray::STD_VECTOR_MAT)
{
for (int i = 0; i < n; ++i)
{
omAll.create(3, 1, CV_64F, i, true);
tAll.create(3, 1, CV_64F, i, true);
Mat tmpom = Mat(_omAll[i]);
Mat tmpt = Mat(_tAll[i]);
tmpom.convertTo(tmpom, CV_64F);
tmpt.convertTo(tmpt, CV_64F);
tmpom.copyTo(omAll.getMat(i));
tmpt.copyTo(tAll.getMat(i));
}
}
else
{
Mat(_omAll).convertTo(omAll, CV_64FC3);
Mat(_tAll).convertTo(tAll, CV_64FC3);
}
if(K.empty())
{
K.create(3, 3, CV_64F);
}
if (D.empty())
{
D.create(1, 4, CV_64F);
}
Mat(_K).convertTo(K.getMat(), K.empty()? CV_64F : K.type());
Mat(_D).convertTo(D.getMat(), D.empty() ? CV_64F: D.type());
if (xi.empty())
{
xi.create(1, 1, CV_64F);
}
Mat xi_m = Mat(1, 1, CV_64F);
xi_m.at<double>(0) = _xi;
xi_m.convertTo(xi.getMat(), xi.empty() ? CV_64F : xi.type());
if (idx.needed())
{
idx.create(1, (int)_idx.total(), CV_32S);
_idx.copyTo(idx.getMat());
}
Vec2d std_error;
double rms;
Mat errors;
cv::omnidir::internal::estimateUncertainties(_patternPoints, _imagePoints, finalParam, errors, std_error, rms, flags);
return rms;
}
double cv::omnidir::stereoCalibrate(InputOutputArrayOfArrays objectPoints, InputOutputArrayOfArrays imagePoints1, InputOutputArrayOfArrays imagePoints2,
const Size& imageSize1, const Size& imageSize2, InputOutputArray K1, InputOutputArray xi1, InputOutputArray D1, InputOutputArray K2, InputOutputArray xi2,
InputOutputArray D2, OutputArray om, OutputArray T, OutputArrayOfArrays omL, OutputArrayOfArrays tL, int flags, TermCriteria criteria, OutputArray idx)
{
CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_64FC3 || objectPoints.type() == CV_32FC3));
CV_Assert(!imagePoints1.empty() && (imagePoints1.type() == CV_64FC2 || imagePoints1.type() == CV_32FC2));
CV_Assert(!imagePoints2.empty() && (imagePoints2.type() == CV_64FC2 || imagePoints2.type() == CV_32FC2));
CV_Assert(((flags & CALIB_USE_GUESS) && !K1.empty() && !D1.empty() && !K2.empty() && !D2.empty()) || !(flags & CALIB_USE_GUESS));
int depth = objectPoints.depth();
std::vector<Mat> _objectPoints, _imagePoints1, _imagePoints2,
_objectPointsFilt, _imagePoints1Filt, _imagePoints2Filt;
for (int i = 0; i < (int)objectPoints.total(); ++i)
{
_objectPoints.push_back(objectPoints.getMat(i));
_imagePoints1.push_back(imagePoints1.getMat(i));
_imagePoints2.push_back(imagePoints2.getMat(i));
if (depth == CV_32F)
{
_objectPoints[i].convertTo(_objectPoints[i], CV_64FC3);
_imagePoints1[i].convertTo(_imagePoints1[i], CV_64FC2);
_imagePoints2[i].convertTo(_imagePoints2[i], CV_64FC2);
}
}
Matx33d _K1, _K2;
Matx14d _D1, _D2;
double _xi1, _xi2;
std::vector<Vec3d> _omL, _TL;
Vec3d _om, _T;
// initializaition
Mat _idx;
internal::initializeStereoCalibration(_objectPoints, _imagePoints1, _imagePoints2, imageSize1, imageSize2, _om, _T, _omL, _TL, _K1, _D1, _K2, _D2, _xi1, _xi2, flags, _idx);
if(idx.needed())
{
idx.create(1, (int)_idx.total(), CV_32S);
_idx.copyTo(idx.getMat());
}
for (int i = 0; i < (int)_idx.total(); ++i)
{
_objectPointsFilt.push_back(_objectPoints[_idx.at<int>(i)]);
_imagePoints1Filt.push_back(_imagePoints1[_idx.at<int>(i)]);
_imagePoints2Filt.push_back(_imagePoints2[_idx.at<int>(i)]);
}
int n = (int)_objectPointsFilt.size();
Mat finalParam(1, 10 + 6*n, CV_64F);
Mat currentParam(1, 10 + 6*n, CV_64F);
//double repr1 = internal::computeMeanReproErrStereo(_objectPoints, _imagePoints1, _imagePoints2, _K1, _K2, _D1, _D2, _xi1, _xi2, _om,
// _T, _omL, _TL);
cv::omnidir::internal::encodeParametersStereo(_K1, _K2, _om, _T, _omL, _TL, _D1, _D2, _xi1, _xi2, currentParam);
// optimization
const double alpha_smooth = 0.01;
double change = 1;
for(int iter = 0; ; ++iter)
{
if ((criteria.type == 1 && iter >= criteria.maxCount) ||
(criteria.type == 2 && change <= criteria.epsilon) ||
(criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount)))
break;
double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, (double)iter + 1.0);
Mat JTJ_inv, JTError;
double epsilon = 0.01 * std::pow(0.9, (double)iter/10);
cv::omnidir::internal::computeJacobianStereo(_objectPointsFilt, _imagePoints1Filt, _imagePoints2Filt, currentParam,
JTJ_inv, JTError, flags, epsilon);
// GaussCNewton
Mat G = alpha_smooth2*JTJ_inv * JTError;
omnidir::internal::fillFixedStereo(G, flags, n);
finalParam = currentParam + G.t();
change = norm(G) / norm(currentParam);
currentParam = finalParam.clone();
cv::omnidir::internal::decodeParametersStereo(currentParam, _K1, _K2, _om, _T, _omL, _TL, _D1, _D2, _xi1, _xi2);
//double repr = internal::computeMeanReproErrStereo(_objectPoints, _imagePoints1, _imagePoints2, _K1, _K2, _D1, _D2, _xi1, _xi2, _om,
// _T, _omL, _TL);
}
cv::omnidir::internal::decodeParametersStereo(finalParam, _K1, _K2, _om, _T, _omL, _TL, _D1, _D2, _xi1, _xi2);
//double repr = internal::computeMeanReproErrStereo(_objectPoints, _imagePoints1, _imagePoints2, _K1, _K2, _D1, _D2, _xi1, _xi2, _om,
// _T, _omL, _TL);
if (K1.empty())
{
K1.create(3, 3, CV_64F);
D1.create(1, 4, CV_64F);
K2.create(3, 3, CV_64F);
D2.create(1, 4, CV_64F);
}
if (om.empty())
{
om.create(1, 3, CV_64F);
T.create(1, 3, CV_64F);
}
if (omL.empty())
{
omL.create(1, n, CV_64FC3);
tL.create(1, n, CV_64FC3);
}
Mat(_K1).convertTo(K1.getMat(), K1.empty() ? CV_64F : K1.type());
Mat(_D1).convertTo(D1.getMat(), D1.empty() ? CV_64F : D1.type());
Mat(_K2).convertTo(K2.getMat(), K2.empty() ? CV_64F : K2.type());
Mat(_D2).convertTo(D2.getMat(), D2.empty() ? CV_64F : D2.type());
Mat(_om).convertTo(om.getMat(), om.empty() ? CV_64F: om.type());
Mat(_T).convertTo(T.getMat(), T.empty() ? CV_64F: T.type());
if (omL.needed())
{
omL.create((int)_omL.size(), 1, CV_64FC3);
}
if (tL.needed())
{
tL.create((int)_TL.size(), 1, CV_64FC3);
}
if (omL.kind() == _InputArray::STD_VECTOR_MAT)
{
for (int i = 0; i < n; ++i)
{
omL.create(3, 1, CV_64F, i, true);
tL.create(3, 1, CV_64F, i, true);
Mat(_omL[i]).copyTo(omL.getMat(i));
Mat(_TL[i]).copyTo(tL.getMat(i));
}
}
else
{
Mat(_omL).convertTo(omL, omL.empty() ? CV_64FC3 : omL.type());
Mat(_TL).convertTo(tL, tL.empty() ? CV_64FC3 : tL.type());
}
Mat xi1_m = Mat(1, 1, CV_64F),
xi2_m = Mat(1, 1, CV_64F);
xi1_m.at<double>(0) = _xi1;
xi2_m.at<double>(0) = _xi2;
if (xi1.empty())
{
xi1.create(1, 1, CV_64F);
}
if (xi2.empty())
{
xi2.create(1, 1, CV_64F);
}
xi1_m.convertTo(xi1, xi1.empty() ? CV_64F : xi1.type());
xi2_m.convertTo(xi2, xi2.empty() ? CV_64F : xi2.type());
// compute uncertainty
Vec2d std_error;
double rms;
Mat errors;
cv::omnidir::internal::estimateUncertaintiesStereo(_objectPointsFilt, _imagePoints1Filt, _imagePoints2Filt,
finalParam, errors, std_error, rms, flags);
return rms;
}
void cv::omnidir::stereoReconstruct(InputArray image1, InputArray image2, InputArray K1, InputArray D1,
InputArray xi1, InputArray K2, InputArray D2, InputArray xi2, InputArray R, InputArray T, int flag,
int numDisparities, int SADWindowSize, OutputArray disparity, OutputArray image1Rec, OutputArray image2Rec,
const Size& newSize, InputArray Knew, OutputArray pointCloud, int pointType)
{
CV_Assert(!K1.empty() && K1.size() == Size(3,3) && (K1.type() == CV_64F || K1.type() == CV_32F));
CV_Assert(!K2.empty() && K2.size() == Size(3,3) && (K2.type() == CV_64F || K2.type() == CV_32F));
CV_Assert(!D1.empty() && D1.total() == 4 && (D1.type() == CV_64F || D1.type() == CV_32F));
CV_Assert(!D2.empty() && D2.total() == 4 && (D2.type() == CV_64F || D2.type() == CV_32F));
CV_Assert(!R.empty() && (R.size() == Size(3,3) || R.total() == 3) && (R.type() == CV_64F || R.type() == CV_32F));
CV_Assert(!T.empty() && T.total() == 3 && (T.type() == CV_64F || T.type() == CV_32F));
CV_Assert(!image1.empty() && (image1.type() == CV_8U || image1.type() == CV_8UC3));
CV_Assert(!image2.empty() && (image2.type() == CV_8U || image2.type() == CV_8UC3));
CV_Assert(flag == omnidir::RECTIFY_LONGLATI || flag == omnidir::RECTIFY_PERSPECTIVE);
Mat _K1, _D1, _K2, _D2, _R, _T;
K1.getMat().convertTo(_K1, CV_64F);
K2.getMat().convertTo(_K2, CV_64F);
D1.getMat().convertTo(_D1, CV_64F);
D2.getMat().convertTo(_D2, CV_64F);
T.getMat().reshape(1, 3).convertTo(_T, CV_64F);
if (R.size() == Size(3, 3))
{
R.getMat().convertTo(_R, CV_64F);
}
else if (R.total() == 3)
{
Rodrigues(R.getMat(), _R);
_R.convertTo(_R, CV_64F);
}
// stereo rectify so that stereo matching can be applied in one line
Mat R1, R2;
stereoRectify(_R, _T, R1, R2);
Mat undis1, undis2;
Matx33d _Knew = Matx33d(_K1);
if (!Knew.empty())
{
Knew.getMat().convertTo(_Knew, CV_64F);
}
undistortImage(image1.getMat(), undis1, _K1, _D1, xi1, flag, _Knew, newSize, R1);
undistortImage(image2.getMat(), undis2, _K2, _D2, xi2, flag, _Knew, newSize, R2);
undis1.copyTo(image1Rec);
undis2.copyTo(image2Rec);
// stereo matching by semi-global
Mat _disMap;
int channel = image1.channels();
//cv::StereoSGBM matching(0, numDisparities, SADWindowSize, 8*channel*SADWindowSize*SADWindowSize, 32*channel*SADWindowSize*SADWindowSize);
//matching(undis1, undis2, _depthMap);
Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, numDisparities, SADWindowSize, 8 * channel*SADWindowSize*SADWindowSize, 32 * channel*SADWindowSize*SADWindowSize);
sgbm->compute(undis1, undis2, _disMap);
// some regions of image1 is black, the corresponding regions of disparity map is also invalid.
Mat realDis;
_disMap.convertTo(_disMap, CV_32F);
Mat(_disMap/16.0f).convertTo(realDis, CV_32F);
Mat grayImg, binaryImg, idx;
if (undis1.channels() == 3)
{
cvtColor(undis1, grayImg, COLOR_RGB2GRAY);
}
else
{
grayImg = undis1;
}
binaryImg = (grayImg <= 0);
findNonZero(binaryImg, idx);
for (int i = 0; i < (int)idx.total(); ++i)
{
Vec2i _idx = idx.at<Vec2i>(i);
realDis.at<float>(_idx[1], _idx[0]) = 0.0f;
}
disparity.create(realDis.size(), realDis.type());
realDis.copyTo(disparity.getMat());
std::vector<Vec3f> _pointCloud;
std::vector<Vec6f> _pointCloudColor;
double baseline = cv::norm(T);
double f = _Knew(0, 0);
Matx33d K_inv = _Knew.inv();
std::vector<Mat> rgb;
if (undis1.channels() == 3)
{
split(undis1, rgb);
}
if (pointCloud.needed())
{
for (int i = 0; i < newSize.width; ++i)
{
for(int j = 0; j < newSize.height; ++j)
{
Vec3f point;
Vec6f pointColor;
if (realDis.at<float>(j, i) > 15)
{
float depth = float(baseline * f /realDis.at<float>(j, i));
// for RECTIFY_PERSPECTIVE, (x,y) are image plane points,
// for RECTIFY_LONGLATI, (x,y) are (theta, phi) angles
float x = float(K_inv(0,0) * i + K_inv(0,1) * j + K_inv(0,2));
float y = float(K_inv(1,0) * i + K_inv(1,1) * j + K_inv(1,2));
if (flag == omnidir::RECTIFY_LONGLATI)
{
point = Vec3f((float)-std::cos(x), (float)(-std::sin(x)*std::cos(y)), (float)(std::sin(x)*std::sin(y))) * depth;
}
else if(flag == omnidir::RECTIFY_PERSPECTIVE)
{
point = Vec3f(float(x), float(y), 1.0f) * depth;
}
if (pointType == XYZ)
{
_pointCloud.push_back(point);
}
else if (pointType == XYZRGB)
{
pointColor[0] = point[0];
pointColor[1] = point[1];
pointColor[2] = point[2];
if (undis1.channels() == 1)
{
pointColor[3] = float(undis1.at<uchar>(j, i));
pointColor[4] = pointColor[3];
pointColor[5] = pointColor[3];
}
else if (undis1.channels() == 3)
{
pointColor[3] = rgb[0].at<uchar>(j, i);
pointColor[4] = rgb[1].at<uchar>(j, i);
pointColor[5] = rgb[2].at<uchar>(j, i);
}
_pointCloudColor.push_back(pointColor);
}
}
}
}
if (pointType == XYZ)
{
Mat(_pointCloud).convertTo(pointCloud, CV_MAKE_TYPE(CV_32F, 3));
}
else if (pointType == XYZRGB)
{
Mat(_pointCloudColor).convertTo(pointCloud, CV_MAKE_TYPE(CV_32F, 6));
}
}
}
void cv::omnidir::internal::encodeParameters(InputArray K, InputArrayOfArrays omAll, InputArrayOfArrays tAll, InputArray distoaration, double xi, OutputArray parameters)
{
CV_Assert(K.type() == CV_64F && K.size() == Size(3,3));
CV_Assert(distoaration.total() == 4 && distoaration.type() == CV_64F);
int n = (int)omAll.total();
Mat _omAll = omAll.getMat(), _tAll = tAll.getMat();
Matx33d _K = K.getMat();
Vec4d _D = (Vec4d)distoaration.getMat();
parameters.create(1, 10+6*n,CV_64F);
Mat _params = parameters.getMat();
for (int i = 0; i < n; i++)
{
Mat(_omAll.at<Vec3d>(i)).reshape(1, 1).copyTo(_params.colRange(i*6, i*6+3));
Mat(_tAll.at<Vec3d>(i)).reshape(1, 1).copyTo(_params.colRange(i*6+3, (i+1)*6));
}
_params.at<double>(0, 6*n) = _K(0,0);
_params.at<double>(0, 6*n+1) = _K(1,1);
_params.at<double>(0, 6*n+2) = _K(0,1);
_params.at<double>(0, 6*n+3) = _K(0,2);
_params.at<double>(0, 6*n+4) = _K(1,2);
_params.at<double>(0, 6*n+5) = xi;
_params.at<double>(0, 6*n+6) = _D[0];
_params.at<double>(0, 6*n+7) = _D[1];
_params.at<double>(0, 6*n+8) = _D[2];
_params.at<double>(0, 6*n+9) = _D[3];
}
void cv::omnidir::internal::encodeParametersStereo(InputArray K1, InputArray K2, InputArray om, InputArray T, InputArrayOfArrays omL, InputArrayOfArrays tL,
InputArray D1, InputArray D2, double xi1, double xi2, OutputArray parameters)
{
CV_Assert(!K1.empty() && K1.type() == CV_64F && K1.size() == Size(3,3));
CV_Assert(!K2.empty() && K2.type() == CV_64F && K2.size() == Size(3,3));
CV_Assert(!om.empty() && om.type() == CV_64F && om.total() == 3);
CV_Assert(!T.empty() && T.type() == CV_64F && T.total() == 3);
CV_Assert(omL.total() == tL.total() && omL.type() == CV_64FC3 && tL.type() == CV_64FC3);
CV_Assert(D1.type() == CV_64F && D1.total() == 4 && D2.type() == CV_64F && D2.total() == 4);
int n = (int)omL.total();
// om, T, omL, tL, intrinsic left, intrinsic right
parameters.create(1, 20 + 6 * (n + 1), CV_64F);
Mat _params = parameters.getMat();
om.getMat().reshape(1, 1).copyTo(_params.colRange(0, 3));
T.getMat().reshape(1, 1).copyTo(_params.colRange(3, 6));
for(int i = 0; i < n; ++i)
{
Mat(omL.getMat().at<Vec3d>(i)).reshape(1, 1).copyTo(_params.colRange(6 + i*6, 6 + i*6 + 3));
Mat(tL.getMat().at<Vec3d>(i)).reshape(1, 1).copyTo(_params.colRange(6 + i*6 + 3, 6 + i*6 + 6));
}
Matx33d _K1 = K1.getMat();
Matx33d _K2 = K2.getMat();
Vec4d _D1 = D1.getMat();
Vec4d _D2 = D2.getMat();
_params.at<double>(0, 6*(n+1)) = _K1(0,0);
_params.at<double>(0, 6*(n+1)+1) = _K1(1,1);
_params.at<double>(0, 6*(n+1)+2) = _K1(0,1);
_params.at<double>(0, 6*(n+1)+3) = _K1(0,2);
_params.at<double>(0, 6*(n+1)+4) = _K1(1,2);
_params.at<double>(0, 6*(n+1)+5) = xi1;
_params.at<double>(0, 6*(n+1)+6) = _D1[0];
_params.at<double>(0, 6*(n+1)+7) = _D1[1];
_params.at<double>(0, 6*(n+1)+8) = _D1[2];
_params.at<double>(0, 6*(n+1)+9) = _D1[3];
_params.at<double>(0, 6*(n+1)+10) = _K2(0,0);
_params.at<double>(0, 6*(n+1)+11) = _K2(1,1);
_params.at<double>(0, 6*(n+1)+12) = _K2(0,1);
_params.at<double>(0, 6*(n+1)+13) = _K2(0,2);
_params.at<double>(0, 6*(n+1)+14) = _K2(1,2);
_params.at<double>(0, 6*(n+1)+15) = xi2;
_params.at<double>(0, 6*(n+1)+16) = _D2[0];
_params.at<double>(0, 6*(n+1)+17) = _D2[1];
_params.at<double>(0, 6*(n+1)+18) = _D2[2];
_params.at<double>(0, 6*(n+1)+19) = _D2[3];
}
void cv::omnidir::internal::decodeParameters(InputArray parameters, OutputArray K, OutputArrayOfArrays omAll, OutputArrayOfArrays tAll, OutputArray distoration, double& xi)
{
if(K.empty())
K.create(3,3,CV_64F);
Matx33d _K;
int n = (int)(parameters.total()-10)/6;
if(omAll.empty())
omAll.create(1, n, CV_64FC3);
if(tAll.empty())
tAll.create(1, n, CV_64FC3);
if(distoration.empty())
distoration.create(1, 4, CV_64F);
Matx14d _D = distoration.getMat();
Mat param = parameters.getMat();
double *para = param.ptr<double>();
_K = Matx33d(para[6*n], para[6*n+2], para[6*n+3],
0, para[6*n+1], para[6*n+4],
0, 0, 1);
_D = Matx14d(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9]);
xi = para[6*n+5];
std::vector<Vec3d> _omAll(n), _tAll(n);
for (int i = 0; i < n; i++)
{
_omAll[i] = Vec3d(param.colRange(i*6, i*6+3));
_tAll[i] = Vec3d(param.colRange(i*6+3, i*6+6));
}
Mat(_D).convertTo(distoration, CV_64F);
Mat(_K).convertTo(K, CV_64F);
if (omAll.kind() == _InputArray::STD_VECTOR_MAT)
{
for (int i = 0; i < n; ++i)
{
Mat(_omAll[i]).copyTo(omAll.getMat(i));
Mat(_tAll[i]).copyTo(tAll.getMat(i));
}
}
else
{
Mat(_omAll).convertTo(omAll, CV_64FC3);
Mat(_tAll).convertTo(tAll, CV_64FC3);
}
}
void cv::omnidir::internal::decodeParametersStereo(InputArray parameters, OutputArray K1, OutputArray K2, OutputArray om, OutputArray T, OutputArrayOfArrays omL,
OutputArrayOfArrays tL, OutputArray D1, OutputArray D2, double& xi1, double& xi2)
{
if(K1.empty())
K1.create(3, 3, CV_64F);
if(K2.empty())
K2.create(3, 3, CV_64F);
if(om.empty())
om.create(3, 1, CV_64F);
if(T.empty())
T.create(3, 1, CV_64F);
int n = ((int)parameters.total() - 20) / 6 - 1;
if(omL.empty())
omL.create(1, n, CV_64FC3);
if(tL.empty())
tL.create(1, n, CV_64FC3);
if(D1.empty())
D1.create(1, 4, CV_64F);
if(D2.empty())
D2.create(1, 4, CV_64F);
Mat param = parameters.getMat().reshape(1, 1);
param.colRange(0, 3).reshape(1, 3).copyTo(om.getMat());
param.colRange(3, 6).reshape(1, 3).copyTo(T.getMat());
std::vector<Vec3d> _omL, _tL;
for(int i = 0; i < n; i++)
{
_omL.push_back(Vec3d(param.colRange(6 + i*6, 6 + i*6 + 3)));
_tL.push_back(Vec3d(param.colRange(6 + i*6 + 3, 6 + i*6 + 6)));
}
double* para = param.ptr<double>();
int offset1 = (n + 1)*6;
Matx33d _K1(para[offset1], para[offset1+2], para[offset1+3],
0, para[offset1+1], para[offset1+4],
0, 0, 1);
xi1 = para[offset1+5];
Matx14d _D1(para[offset1+6], para[offset1+7], para[offset1+8], para[offset1+9]);
int offset2 = (n + 1)*6 + 10;
Matx33d _K2(para[offset2], para[offset2+2], para[offset2+3],
0, para[offset2+1], para[offset2+4],
0, 0, 1);
xi2 = para[offset2+5];
Matx14d _D2(para[offset2+6], para[offset2+7], para[offset2+8], para[offset2+9]);
Mat(_K1).convertTo(K1, CV_64F);
Mat(_D1).convertTo(D1, CV_64F);
Mat(_K2).convertTo(K2, CV_64F);
Mat(_D2).convertTo(D2, CV_64F);
if(omL.kind() == _InputArray::STD_VECTOR_MAT)
{
for(int i = 0; i < n; ++i)
{
Mat(_omL[i]).copyTo(omL.getMat(i));
Mat(_tL[i]).copyTo(tL.getMat(i));
}
}
else
{
Mat(_omL).convertTo(omL, CV_64FC3);
Mat(_tL).convertTo(tL, CV_64FC3);
}
}
void cv::omnidir::internal::estimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray parameters,
Mat& errors, Vec2d& std_error, double& rms, int flags)
{
CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2);
CV_Assert(!parameters.empty() && parameters.type() == CV_64F);
int n = (int) objectPoints.total();
// assume every image has the same number of objectpoints
int nPointsAll = 0;
for (int i = 0; i < n; ++i)
{
nPointsAll += (int)objectPoints.getMat(i).total();
}
Mat reprojError = Mat(nPointsAll, 1, CV_64FC2);
double* para = parameters.getMat().ptr<double>();
Matx33d K(para[6*n], para[6*n+2], para[6*n+3],
0, para[6*n+1], para[6*n+4],
0, 0, 1);
Matx14d D(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9]);
double xi = para[6*n+5];
int nPointsAccu = 0;
for(int i=0; i < n; ++i)
{
Mat imgPoints, objPoints;
imagePoints.getMat(i).copyTo(imgPoints);
objectPoints.getMat(i).copyTo(objPoints);
imgPoints = imgPoints.reshape(2, imgPoints.rows*imgPoints.cols);
objPoints = objPoints.reshape(3, objPoints.rows*objPoints.cols);
Mat om = parameters.getMat().colRange(i*6, i*6+3);
Mat T = parameters.getMat().colRange(i*6+3, (i+1)*6);
Mat x;
omnidir::projectPoints(objPoints, x, om, T, K, xi, D, cv::noArray());
Mat errorx = (imgPoints - x);
//reprojError.rowRange(errorx.rows*i, errorx.rows*(i+1)) = errorx.clone();
errorx.copyTo(reprojError.rowRange(nPointsAccu, nPointsAccu + (int)errorx.total()));
nPointsAccu += (int)errorx.total();
}
meanStdDev(reprojError, noArray(), std_error);
std_error *= sqrt((double)reprojError.total()/((double)reprojError.total() - 1.0));
Mat sigma_x;
meanStdDev(reprojError.reshape(1,1), noArray(), sigma_x);
sigma_x *= sqrt(2.0*(double)reprojError.total()/(2.0*(double)reprojError.total() - 1.0));
double s = sigma_x.at<double>(0);
Mat _JTJ_inv, _JTE;
computeJacobian(objectPoints, imagePoints, parameters, _JTJ_inv, _JTE, flags, 0.0);
sqrt(_JTJ_inv, _JTJ_inv);
errors = 3 * s * _JTJ_inv.diag();
checkFixed(errors, flags, n);
rms = 0;
const Vec2d* ptr_ex = reprojError.ptr<Vec2d>();
for (int i = 0; i < (int)reprojError.total(); i++)
{
rms += ptr_ex[i][0] * ptr_ex[i][0] + ptr_ex[i][1] * ptr_ex[i][1];
}
rms /= (double)reprojError.total();
rms = sqrt(rms);
}
// estimateUncertaintiesStereo
void cv::omnidir::internal::estimateUncertaintiesStereo(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
InputArray parameters, Mat& errors, Vec2d& std_error, double& rms, int flags)
{
CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
CV_Assert(!imagePoints1.empty() && imagePoints1.type() == CV_64FC2 && imagePoints1.total() == objectPoints.total());
CV_Assert(!imagePoints2.empty() && imagePoints2.type() == CV_64FC2 && imagePoints1.total() == imagePoints2.total());
int n_img = (int)objectPoints.total();
CV_Assert((int)parameters.total() == (6*(n_img+1)+20));
Mat _K1, _K2, _D1, _D2;
Vec3d _om, _T;
std::vector<Vec3d> _omL(n_img), _tL(n_img);
Mat _parameters = parameters.getMat().reshape(1, 1);
double _xi1, _xi2;
internal::decodeParametersStereo(_parameters, _K1, _K2, _om, _T, _omL, _tL, _D1, _D2, _xi1, _xi2);
int n_points = (int)objectPoints.getMat(0).total();
Mat reprojErrorAll = Mat::zeros(2*n_points*n_img, 1, CV_64FC2);
// error for left image
for (int i = 0; i < n_img; ++i)
{
Mat objPointsi, imgPointsi;
objectPoints.getMat(i).copyTo(objPointsi);
imagePoints1.getMat(i).copyTo(imgPointsi);
objPointsi = objPointsi.reshape(3, objPointsi.rows*objPointsi.cols);
imgPointsi = imgPointsi.reshape(2, imgPointsi.rows*imgPointsi.cols);
Mat x;
omnidir::projectPoints(objPointsi, x, _omL[i], _tL[i], _K1, _xi1, _D1, cv::noArray());
Mat errorx = imgPointsi - x;
errorx.copyTo(reprojErrorAll.rowRange(i*2*n_points, (i*2+1)*n_points));
}
// error for right image
for (int i = 0; i < n_img; ++i)
{
Mat objPointsi, imgPointsi;
objectPoints.getMat(i).copyTo(objPointsi);
imagePoints2.getMat(i).copyTo(imgPointsi);
objPointsi = objPointsi.reshape(3, objPointsi.rows*objPointsi.cols);
imgPointsi = imgPointsi.reshape(2, imgPointsi.rows*imgPointsi.cols);
Mat x;
Mat _R, _R2, _R1, _T2, _T1, _om2;
Rodrigues(_om, _R);
Rodrigues(_omL[i], _R1);
_T1 = Mat(_tL[i]);
_R2 = _R * _R1;
_T2 = _R * _T1 + Mat(_T);
Rodrigues(_R2, _om2);
omnidir::projectPoints(objPointsi, x, _om2, _T2, _K2, _xi2, _D2, cv::noArray());
Mat errorx = imgPointsi - x;
errorx.copyTo(reprojErrorAll.rowRange((i*2+1)*n_points, (i+1)*2*n_points));
}
meanStdDev(reprojErrorAll, cv::noArray(), std_error);
std_error *= sqrt((double)reprojErrorAll.total()/((double)reprojErrorAll.total() - 1.0));
Mat sigma_x;
meanStdDev(reprojErrorAll.reshape(1,1), noArray(), sigma_x);
sigma_x *= sqrt(2.0*(double)reprojErrorAll.total()/(2.0*(double)reprojErrorAll.total() - 1.0));
double s = sigma_x.at<double>(0);
Mat _JTJ_inv, _JTE;
computeJacobianStereo(objectPoints, imagePoints1, imagePoints2, _parameters, _JTJ_inv, _JTE, flags, 0.0);
cv::sqrt(_JTJ_inv, _JTJ_inv);
errors = 3 * s * _JTJ_inv.diag();
rms = 0;
const Vec2d* ptr_ex = reprojErrorAll.ptr<Vec2d>();
for (int i = 0; i < (int)reprojErrorAll.total(); i++)
{
rms += ptr_ex[i][0] * ptr_ex[i][0] + ptr_ex[i][1] * ptr_ex[i][1];
}
rms /= (double)reprojErrorAll.total();
rms = sqrt(rms);
}
//
double cv::omnidir::internal::computeMeanReproErr(InputArrayOfArrays imagePoints, InputArrayOfArrays proImagePoints)
{
CV_Assert(!imagePoints.empty() && imagePoints.type()==CV_64FC2);
CV_Assert(!proImagePoints.empty() && proImagePoints.type() == CV_64FC2);
CV_Assert(imagePoints.total() == proImagePoints.total());
int n = (int)imagePoints.total();
double reprojError = 0;
int totalPoints = 0;
if (imagePoints.kind() == _InputArray::STD_VECTOR_MAT)
{
for (int i = 0; i < n; i++)
{
Mat x, proj_x;
imagePoints.getMat(i).copyTo(x);
proImagePoints.getMat(i).copyTo(proj_x);
Mat errorI = x.reshape(2, x.rows*x.cols) - proj_x.reshape(2, proj_x.rows*proj_x.cols);
//Mat errorI = imagePoints.getMat(i) - proImagePoints.getMat(i);
totalPoints += (int)errorI.total();
Vec2d* ptr_err = errorI.ptr<Vec2d>();
for (int j = 0; j < (int)errorI.total(); j++)
{
reprojError += sqrt(ptr_err[j][0]*ptr_err[j][0] + ptr_err[j][1]*ptr_err[j][1]);
}
}
}
else
{
Mat x, proj_x;
imagePoints.getMat().copyTo(x);
proImagePoints.getMat().copyTo(proj_x);
Mat errorI = x.reshape(2, x.rows*x.cols) - proj_x.reshape(2, proj_x.rows*proj_x.cols);
//Mat errorI = imagePoints.getMat() - proImagePoints.getMat();
totalPoints += (int)errorI.total();
Vec2d* ptr_err = errorI.ptr<Vec2d>();
for (int j = 0; j < (int)errorI.total(); j++)
{
reprojError += sqrt(ptr_err[j][0]*ptr_err[j][0] + ptr_err[j][1]*ptr_err[j][1]);
}
}
double meanReprojError = reprojError / totalPoints;
return meanReprojError;
}
double cv::omnidir::internal::computeMeanReproErr(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray K, InputArray D, double xi, InputArrayOfArrays omAll,
InputArrayOfArrays tAll)
{
CV_Assert(objectPoints.total() == imagePoints.total());
CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2);
std::vector<Mat> proImagePoints;
int n = (int)objectPoints.total();
Mat _omAll = omAll.getMat();
Mat _tAll = tAll.getMat();
for(int i = 0; i < n; ++i)
{
Mat imgPoint;
//cv::omnidir::projectPoints(objetPoints.getMat(i), imgPoint, omAll.getMat(i), tAll.getMat(i), K.getMat(), xi, D.getMat(), noArray());
cv::omnidir::projectPoints(objectPoints.getMat(i), imgPoint, _omAll.at<Vec3d>(i), _tAll.at<Vec3d>(i), K.getMat(), xi, D.getMat(), noArray());
proImagePoints.push_back(imgPoint);
}
return internal::computeMeanReproErr(imagePoints, proImagePoints);
}
double cv::omnidir::internal::computeMeanReproErrStereo(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, InputArray K1, InputArray K2,
InputArray D1, InputArray D2, double xi1, double xi2, InputArray om, InputArray T, InputArrayOfArrays omL, InputArrayOfArrays TL)
{
CV_Assert(objectPoints.total() == imagePoints1.total() && imagePoints1.total() == imagePoints2.total());
CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
CV_Assert(!imagePoints1.empty() && imagePoints1.type() == CV_64FC2);
CV_Assert(!imagePoints2.empty() && imagePoints2.type() == CV_64FC2);
std::vector<Mat> proImagePoints1, proImagePoints2;
int n = (int)objectPoints.total();
Mat _omL = omL.getMat(), _TL = TL.getMat();
Mat _om = om.getMat(), _R, _T = T.getMat();
Rodrigues(_om, _R);
Mat _K1 = K1.getMat(), _K2 = K2.getMat();
Mat _D1 = D1.getMat(), _D2 = D2.getMat();
// reprojection error for left image
for (int i = 0; i < n; ++i)
{
Mat imgPoints;
cv::omnidir::projectPoints(objectPoints.getMat(i), imgPoints, _omL.at<Vec3d>(i), _TL.at<Vec3d>(i), _K1, xi1, _D1, cv::noArray());
proImagePoints1.push_back(imgPoints);
}
// reprojection error for right image
for (int i = 0; i < n; ++i)
{
Mat imgPoints;
Mat _omRi,_RRi,_TRi,_RLi, _TLi;
Rodrigues(_omL.at<Vec3d>(i), _RLi);
_TLi = Mat(_TL.at<Vec3d>(i)).reshape(1, 3);
_RRi = _R * _RLi;
_TRi = _R * _TLi + _T;
Rodrigues(_RRi, _omRi);
cv::omnidir::projectPoints(objectPoints.getMat(i), imgPoints, _omRi, _TRi, _K2, xi2, _D2, cv::noArray());
proImagePoints2.push_back(imgPoints);
}
double reProErr1 = internal::computeMeanReproErr(imagePoints1, proImagePoints1);
double reProErr2 = internal::computeMeanReproErr(imagePoints2, proImagePoints2);
double reProErr = (reProErr1 + reProErr2) / 2.0;
//double reProErr = reProErr1*reProErr1 + reProErr2* reProErr2;
return reProErr;
}
void cv::omnidir::internal::checkFixed(Mat& G, int flags, int n)
{
int _flags = flags;
if(_flags >= omnidir::CALIB_FIX_CENTER)
{
G.at<double>(6*n+3) = 0;
G.at<double>(6*n+4) = 0;
_flags -= omnidir::CALIB_FIX_CENTER;
}
if(_flags >= omnidir::CALIB_FIX_GAMMA)
{
G.at<double>(6*n) = 0;
G.at<double>(6*n+1) = 0;
_flags -= omnidir::CALIB_FIX_GAMMA;
}
if(_flags >= omnidir::CALIB_FIX_XI)
{
G.at<double>(6*n + 5) = 0;
_flags -= omnidir::CALIB_FIX_XI;
}
if(_flags >= omnidir::CALIB_FIX_P2)
{
G.at<double>(6*n + 9) = 0;
_flags -= omnidir::CALIB_FIX_P2;
}
if(_flags >= omnidir::CALIB_FIX_P1)
{
G.at<double>(6*n + 8) = 0;
_flags -= omnidir::CALIB_FIX_P1;
}
if(_flags >= omnidir::CALIB_FIX_K2)
{
G.at<double>(6*n + 7) = 0;
_flags -= omnidir::CALIB_FIX_K2;
}
if(_flags >= omnidir::CALIB_FIX_K1)
{
G.at<double>(6*n + 6) = 0;
_flags -= omnidir::CALIB_FIX_K1;
}
if(_flags >= omnidir::CALIB_FIX_SKEW)
{
G.at<double>(6*n + 2) = 0;
}
}
// This function is from fisheye.cpp
void cv::omnidir::internal::subMatrix(const Mat& src, Mat& dst, const std::vector<int>& cols, const std::vector<int>& rows)
{
CV_Assert(src.type() == CV_64FC1);
int nonzeros_cols = cv::countNonZero(cols);
Mat tmp(src.rows, nonzeros_cols, CV_64FC1);
for (int i = 0, j = 0; i < (int)cols.size(); i++)
{
if (cols[i])
{
src.col(i).copyTo(tmp.col(j++));
}
}
int nonzeros_rows = cv::countNonZero(rows);
Mat tmp1(nonzeros_rows, nonzeros_cols, CV_64FC1);
for (int i = 0, j = 0; i < (int)rows.size(); i++)
{
if (rows[i])
{
tmp.row(i).copyTo(tmp1.row(j++));
}
}
dst = tmp1.clone();
}
void cv::omnidir::internal::flags2idx(int flags, std::vector<int>& idx, int n)
{
idx = std::vector<int>(6*n+10,1);
int _flags = flags;
if(_flags >= omnidir::CALIB_FIX_CENTER)
{
idx[6*n+3] = 0;
idx[6*n+4] = 0;
_flags -= omnidir::CALIB_FIX_CENTER;
}
if(_flags >= omnidir::CALIB_FIX_GAMMA)
{
idx[6*n] = 0;
idx[6*n+1] = 0;
_flags -= omnidir::CALIB_FIX_GAMMA;
}
if(_flags >= omnidir::CALIB_FIX_XI)
{
idx[6*n + 5] = 0;
_flags -= omnidir::CALIB_FIX_XI;
}
if(_flags >= omnidir::CALIB_FIX_P2)
{
idx[6*n + 9] = 0;
_flags -= omnidir::CALIB_FIX_P2;
}
if(_flags >= omnidir::CALIB_FIX_P1)
{
idx[6*n + 8] = 0;
_flags -= omnidir::CALIB_FIX_P1;
}
if(_flags >= omnidir::CALIB_FIX_K2)
{
idx[6*n + 7] = 0;
_flags -= omnidir::CALIB_FIX_K2;
}
if(_flags >= omnidir::CALIB_FIX_K1)
{
idx[6*n + 6] = 0;
_flags -= omnidir::CALIB_FIX_K1;
}
if(_flags >= omnidir::CALIB_FIX_SKEW)
{
idx[6*n + 2] = 0;
}
}
void cv::omnidir::internal::flags2idxStereo(int flags, std::vector<int>& idx, int n)
{
idx = std::vector<int>(6*(n+1)+20, 1);
int _flags = flags;
int offset1 = 6*(n+1);
int offset2 = offset1 + 10;
if(_flags >= omnidir::CALIB_FIX_CENTER)
{
idx[offset1+3] = 0;
idx[offset1+4] = 0;
idx[offset2+3] = 0;
idx[offset2+4] = 0;
_flags -= omnidir::CALIB_FIX_CENTER;
}
if(_flags >= omnidir::CALIB_FIX_GAMMA)
{
idx[offset1] = 0;
idx[offset1+1] = 0;
idx[offset2] = 0;
idx[offset2+1] = 0;
_flags -= omnidir::CALIB_FIX_GAMMA;
}
if(_flags >= omnidir::CALIB_FIX_XI)
{
idx[offset1 + 5] = 0;
idx[offset2 + 5] = 0;
_flags -= omnidir::CALIB_FIX_XI;
}
if(_flags >= omnidir::CALIB_FIX_P2)
{
idx[offset1 + 9] = 0;
idx[offset2 + 9] = 0;
_flags -= omnidir::CALIB_FIX_P2;
}
if(_flags >= omnidir::CALIB_FIX_P1)
{
idx[offset1 + 8] = 0;
idx[offset2 + 8] = 0;
_flags -= omnidir::CALIB_FIX_P1;
}
if(_flags >= omnidir::CALIB_FIX_K2)
{
idx[offset1 + 7] = 0;
idx[offset2 + 7] = 0;
_flags -= omnidir::CALIB_FIX_K2;
}
if(_flags >= omnidir::CALIB_FIX_K1)
{
idx[offset1 + 6] = 0;
idx[offset2 + 6] = 0;
_flags -= omnidir::CALIB_FIX_K1;
}
if(_flags >= omnidir::CALIB_FIX_SKEW)
{
idx[offset1 + 2] = 0;
idx[offset2 + 2] = 0;
}
}
// fill in zerso for fixed parameters
void cv::omnidir::internal::fillFixed(Mat&G, int flags, int n)
{
Mat tmp = G.clone();
std::vector<int> idx(6*n + 10, 1);
flags2idx(flags, idx, n);
G.release();
G.create(6*n +10, 1, CV_64F);
G = cv::Mat::zeros(6*n +10, 1, CV_64F);
for (int i = 0,j=0; i < (int)idx.size(); i++)
{
if (idx[i])
{
G.at<double>(i) = tmp.at<double>(j++);
}
}
}
void cv::omnidir::internal::fillFixedStereo(Mat& G, int flags, int n)
{
Mat tmp = G.clone();
std::vector<int> idx(6*(n+1)+20, 1);
flags2idxStereo(flags, idx, n);
G.release();
G.create(6 * (n+1) + 20, 1, CV_64F);
G = cv::Mat::zeros(6* (n + 1) + 20, 1, CV_64F);
for (int i = 0,j=0; i < (int)idx.size(); i++)
{
if (idx[i])
{
G.at<double>(i) = tmp.at<double>(j++);
}
}
}
double cv::omnidir::internal::findMedian(const Mat& row)
{
CV_Assert(!row.empty() && row.rows == 1 && row.type() == CV_64F);
Mat tmp = row.clone();
cv::sort(tmp, tmp, 0);
if((int)tmp.total()%2 == 0)
return tmp.at<double>((int)tmp.total() / 2);
else
return 0.5 * (tmp.at<double>((int)tmp.total() / 2) + tmp.at<double>((int)tmp.total()/2 - 1));
}
cv::Vec3d cv::omnidir::internal::findMedian3(InputArray mat)
{
CV_Assert(mat.depth() == CV_64F && mat.getMat().rows == 1);
Mat M = Mat(mat.getMat().t()).reshape(1).t();
return Vec3d(findMedian(M.row(0)), findMedian(M.row(1)), findMedian(M.row(2)));
}
void cv::omnidir::stereoRectify(InputArray R, InputArray T, OutputArray R1, OutputArray R2)
{
CV_Assert((R.size() == Size(3,3) || R.total() == 3) && (R.depth() == CV_32F || R.depth() == CV_64F));
CV_Assert(T.total() == 3 && (T.depth() == CV_32F || T.depth() == CV_64F));
Mat _R, _T;
if (R.size() == Size(3, 3))
{
R.getMat().convertTo(_R, CV_64F);
}
else if (R.total() == 3)
{
Rodrigues(R.getMat(), _R);
_R.convertTo(_R, CV_64F);
}
T.getMat().reshape(1, 3).convertTo(_T, CV_64F);
R1.create(3, 3, CV_64F);
Mat _R1 = R1.getMat();
R2.create(3, 3, CV_64F);
Mat _R2 = R2.getMat();
Mat R21 = _R.t();
Mat T21 = -_R.t() * _T;
Mat e1, e2, e3;
e1 = T21.t() / norm(T21);
e2 = Mat(Matx13d(T21.at<double>(1)*-1, T21.at<double>(0), 0.0));
e2 = e2 / norm(e2);
e3 = e1.cross(e2);
e3 = e3 / norm(e3);
e1.copyTo(_R1.row(0));
e2.copyTo(_R1.row(1));
e3.copyTo(_R1.row(2));
_R2 = R21 * _R1;
}
void cv::omnidir::internal::getInterset(InputArray idx1, InputArray idx2, OutputArray inter1, OutputArray inter2,
OutputArray inter_ori)
{
Mat _idx1 = idx1.getMat();
Mat _idx2 = idx2.getMat();
int n1 = (int)idx1.total();
int n2 = (int)idx2.total();
std::vector<int> _inter1, _inter2, _inter_ori;
for (int i = 0; i < n1; ++i)
{
for (int j = 0; j < n2; ++j)
{
if(_idx1.at<int>(i) == _idx2.at<int>(j))
{
_inter1.push_back(i);
_inter2.push_back(j);
_inter_ori.push_back(_idx1.at<int>(i));
}
}
}
inter1.create(1, (int)_inter1.size(), CV_32S);
inter2.create(1, (int)_inter2.size(), CV_32S);
inter_ori.create(1, (int)_inter_ori.size(), CV_32S);
for (int i = 0; i < (int)_inter1.size(); ++i)
{
inter1.getMat().at<int>(i) = _inter1[i];
}
for (int i = 0; i < (int)_inter2.size(); ++i)
{
inter2.getMat().at<int>(i) = _inter2[i];
}
for (int i = 0; i < (int)_inter_ori.size(); ++i)
{
inter_ori.getMat().at<int>(i) = _inter_ori[i];
}
}
\ No newline at end of file
modules/ccalib/src/precomp.hpp
View file @ 9011368a
/*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) 2014, OpenCV Foundation, 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 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*/
//
// 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) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage 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 name 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*/
#ifndef __OPENCV_CCALIB_PRECOMP__
#define __OPENCV_CCALIB_PRECOMP__
#ifndef __OPENCV_PRECOMP_H__
#define __OPENCV_PRECOMP_H__
#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/features2d.hpp>
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include <vector>
#endif
modules/ccalib/src/randpattern.cpp 0 → 100644
View file @ 9011368a
/*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) 2015, Baisheng Lai (laibaisheng@gmail.com), Zhejiang University,
// all rights reserved.
//
// 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 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*/
/**
* This module was accepted as a GSoC 2015 project for OpenCV, authored by
* Baisheng Lai, mentored by Bo Li.
* This module implements points extraction from a "random" pattern image.
* It returns 3D object points and 2D image pixels that can be used for calibration.
* Compared with traditional calibration object like chessboard, it is useful when pattern is
* partly occluded or only a part of pattern can be observed in multiple cameras calibration.
* For detail, refer to this paper
* B. Li, L. Heng, K. Kevin and M. Pollefeys, "A Multiple-Camera System
* Calibration Toolbox Using A Feature Descriptor-Based Calibration
* Pattern", in IROS 2013.
*/
#include "precomp.hpp"
#include "opencv2/ccalib/randpattern.hpp"
#include <iostream>
using namespace std;
namespace cv { namespace randpattern {
RandomPatternCornerFinder::RandomPatternCornerFinder(float patternWidth, float patternHeight,
int nminiMatch, int depth, int verbose, int showExtraction, Ptr<FeatureDetector> detector, Ptr<DescriptorExtractor> descriptor,
Ptr<DescriptorMatcher> matcher)
{
_patternHeight = patternHeight;
_patternWidth = patternWidth;
_nminiMatch = nminiMatch;
_objectPonits.resize(0);
_imagePoints.resize(0);
_depth = depth;
_detector = detector;
_descriptor = descriptor;
_matcher = matcher;
_showExtraction = showExtraction;
_verbose = verbose;
}
void RandomPatternCornerFinder::computeObjectImagePoints(std::vector<cv::Mat> inputImages)
{
CV_Assert(!_patternImage.empty());
CV_Assert(inputImages.size() > 0);
int nImages = (int)inputImages.size();
std::vector<Mat> imageObjectPoints;
for (int i = 0; i < nImages; ++i)
{
imageObjectPoints = computeObjectImagePointsForSingle(inputImages[i]);
if ((int)imageObjectPoints[0].total() > _nminiMatch)
{
_imagePoints.push_back(imageObjectPoints[0]);
_objectPonits.push_back(imageObjectPoints[1]);
}
}
}
void RandomPatternCornerFinder::keyPoints2MatchedLocation(const std::vector<cv::KeyPoint>& imageKeypoints,
const std::vector<cv::KeyPoint>& patternKeypoints, const std::vector<cv::DMatch> matchces,
cv::Mat& matchedImagelocation, cv::Mat& matchedPatternLocation)
{
matchedImagelocation.release();
matchedPatternLocation.release();
std::vector<Vec2d> image, pattern;
for (int i = 0; i < (int)matchces.size(); ++i)
{
Point2f imgPt = imageKeypoints[matchces[i].queryIdx].pt;
Point2f patPt = patternKeypoints[matchces[i].trainIdx].pt;
image.push_back(Vec2d(imgPt.x, imgPt.y));
pattern.push_back(Vec2d(patPt.x, patPt.y));
}
Mat(image).convertTo(matchedImagelocation, CV_64FC2);
Mat(pattern).convertTo(matchedPatternLocation, CV_64FC2);
}
void RandomPatternCornerFinder::getFilteredLocation(cv::Mat& imageKeypoints, cv::Mat& patternKeypoints, const cv::Mat mask)
{
Mat tmpKeypoint, tmpPattern;
imageKeypoints.copyTo(tmpKeypoint);
patternKeypoints.copyTo(tmpPattern);
imageKeypoints.release();
patternKeypoints.release();
std::vector<cv::Vec2d> vecKeypoint, vecPattern;
for(int i = 0; i < (int)mask.total(); ++i)
{
if (mask.at<uchar>(i) == 1)
{
vecKeypoint.push_back(tmpKeypoint.at<Vec2d>(i));
vecPattern.push_back(tmpPattern.at<Vec2d>(i));
}
}
Mat(vecKeypoint).convertTo(imageKeypoints, CV_64FC2);
Mat(vecPattern).convertTo(patternKeypoints, CV_64FC2);
}
void RandomPatternCornerFinder::getObjectImagePoints(const cv::Mat& imageKeypoints, const cv::Mat& patternKeypoints)
{
Mat imagePoints_i, objectPoints_i;
int imagePointsType = CV_MAKETYPE(_depth, 2);
int objectPointsType = CV_MAKETYPE(_depth, 3);
imageKeypoints.convertTo(imagePoints_i, imagePointsType);
_imagePoints.push_back(imagePoints_i);
if (patternKeypoints.total()!=CV_64FC2)
patternKeypoints.convertTo(patternKeypoints, CV_64FC2);
std::vector<Vec3d> objectPoints;
for (int i = 0; i < (int)patternKeypoints.total(); ++i)
{
double x = patternKeypoints.at<Vec2d>(i)[0];
double y = patternKeypoints.at<Vec2d>(i)[1];
x = x / _patternImageSize.width * _patternWidth;
y = y / _patternImageSize.height * _patternHeight;
objectPoints.push_back(Vec3d(x, y, 0));
}
Mat(objectPoints).convertTo(objectPoints_i, objectPointsType);
_objectPonits.push_back(objectPoints_i);
}
void RandomPatternCornerFinder::crossCheckMatching( Ptr<DescriptorMatcher>& descriptorMatcher,
const Mat& descriptors1, const Mat& descriptors2,
std::vector<DMatch>& filteredMatches12, int knn )
{
filteredMatches12.clear();
std::vector<std::vector<DMatch> > matches12, matches21;
descriptorMatcher->knnMatch( descriptors1, descriptors2, matches12, knn );
descriptorMatcher->knnMatch( descriptors2, descriptors1, matches21, knn );
for( size_t m = 0; m < matches12.size(); m++ )
{
bool findCrossCheck = false;
for( size_t fk = 0; fk < matches12[m].size(); fk++ )
{
DMatch forward = matches12[m][fk];
for( size_t bk = 0; bk < matches21[forward.trainIdx].size(); bk++ )
{
DMatch backward = matches21[forward.trainIdx][bk];
if( backward.trainIdx == forward.queryIdx )
{
filteredMatches12.push_back(forward);
findCrossCheck = true;
break;
}
}
if( findCrossCheck ) break;
}
}
}
void RandomPatternCornerFinder::drawCorrespondence(const Mat& image1, const std::vector<cv::KeyPoint> keypoint1,
const Mat& image2, const std::vector<cv::KeyPoint> keypoint2, const std::vector<cv::DMatch> matchces,
const Mat& mask1, const Mat& mask2, const int step)
{
Mat img_corr;
if(step == 1)
{
drawMatches(image1, keypoint1, image2, keypoint2, matchces, img_corr);
}
else if(step == 2)
{
std::vector<cv::DMatch> matchesFilter;
for (int i = 0; i < (int)mask1.total(); ++i)
{
if (!mask1.empty() && mask1.at<uchar>(i) == 1)
{
matchesFilter.push_back(matchces[i]);
}
}
drawMatches(image1, keypoint1, image2, keypoint2, matchesFilter, img_corr);
}
else if(step == 3)
{
std::vector<cv::DMatch> matchesFilter;
int j = 0;
for (int i = 0; i < (int)mask1.total(); ++i)
{
if (mask1.at<uchar>(i) == 1)
{
if (!mask2.empty() && mask2.at<uchar>(j) == 1)
{
matchesFilter.push_back(matchces[i]);
}
j++;
}
}
drawMatches(image1, keypoint1, image2, keypoint2, matchesFilter, img_corr);
}
imshow("correspondence", img_corr);
waitKey(0);
}
std::vector<cv::Mat> RandomPatternCornerFinder::getObjectPoints()
{
return _objectPonits;
}
std::vector<cv::Mat> RandomPatternCornerFinder::getImagePoints()
{
return _imagePoints;
}
void RandomPatternCornerFinder::loadPattern(cv::Mat patternImage)
{
_patternImage = patternImage.clone();
if (_patternImage.type()!= CV_8U)
_patternImage.convertTo(_patternImage, CV_8U);
_patternImageSize = _patternImage.size();
_detector->detect(patternImage, _keypointsPattern);
_descriptor->compute(patternImage, _keypointsPattern, _descriptorPattern);
_descriptorPattern.convertTo(_descriptorPattern, CV_32F);
}
std::vector<cv::Mat> RandomPatternCornerFinder::computeObjectImagePointsForSingle(cv::Mat inputImage)
{
CV_Assert(!_patternImage.empty());
std::vector<cv::Mat> r(2);
Mat descriptorImage1, descriptorImage2,descriptorImage;
std::vector<cv::KeyPoint> keypointsImage1, keypointsImage2, keypointsImage;
if (inputImage.type()!=CV_8U)
{
inputImage.convertTo(inputImage, CV_8U);
}
Mat imageEquHist;
equalizeHist(inputImage, imageEquHist);
_detector->detect(inputImage, keypointsImage1);
_descriptor->compute(inputImage, keypointsImage1, descriptorImage1);
_detector->detect(imageEquHist, keypointsImage2);
_descriptor->compute(imageEquHist, keypointsImage2, descriptorImage2);
descriptorImage1.convertTo(descriptorImage1, CV_32F);
descriptorImage2.convertTo(descriptorImage2, CV_32F);
// match with pattern
std::vector<DMatch> matchesImgtoPat, matchesImgtoPat1, matchesImgtoPat2;
cv::Mat keypointsImageLocation, keypointsPatternLocation;
crossCheckMatching(this->_matcher, descriptorImage1, this->_descriptorPattern, matchesImgtoPat1, 1);
crossCheckMatching(this->_matcher, descriptorImage2, this->_descriptorPattern, matchesImgtoPat2, 1);
if ((int)matchesImgtoPat1.size() > (int)matchesImgtoPat2.size())
{
matchesImgtoPat = matchesImgtoPat1;
keypointsImage = keypointsImage1;
}
else
{
matchesImgtoPat = matchesImgtoPat2;
keypointsImage = keypointsImage2;
}
keyPoints2MatchedLocation(keypointsImage, this->_keypointsPattern, matchesImgtoPat,
keypointsImageLocation, keypointsPatternLocation);
Mat img_corr;
// innerMask is CV_8U type
Mat innerMask1, innerMask2;
// draw raw correspondence
if(this->_showExtraction)
{
drawCorrespondence(inputImage, keypointsImage, _patternImage, _keypointsPattern, matchesImgtoPat,
innerMask1, innerMask2, 1);
}
if (_verbose)
{
std::cout << "number of matched points " << (int)keypointsImageLocation.total() << std::endl;
}
// outlier remove
findFundamentalMat(keypointsImageLocation, keypointsPatternLocation,
FM_RANSAC, 1, 0.995, innerMask1);
getFilteredLocation(keypointsImageLocation, keypointsPatternLocation, innerMask1);
if (this->_showExtraction)
{
drawCorrespondence(inputImage, keypointsImage, _patternImage, _keypointsPattern, matchesImgtoPat,
innerMask1, innerMask2, 2);
}
findHomography(keypointsImageLocation, keypointsPatternLocation, RANSAC, 30*inputImage.cols/1000, innerMask2);
getFilteredLocation(keypointsImageLocation, keypointsPatternLocation, innerMask2);
if (_verbose)
{
std::cout << "number of filtered points " << (int)keypointsImageLocation.total() << std::endl;
}
// draw filtered correspondence
if (this->_showExtraction)
{
drawCorrespondence(inputImage, keypointsImage, _patternImage, _keypointsPattern, matchesImgtoPat,
innerMask1, innerMask2, 3);
}
std::vector<Vec3d> objectPoints;
int imagePointsType = CV_MAKETYPE(_depth, 2);
int objectPointsType = CV_MAKETYPE(_depth, 3);
keypointsImageLocation.convertTo(r[0], imagePointsType);
for (int i = 0; i < (int)keypointsPatternLocation.total(); ++i)
{
double x = keypointsPatternLocation.at<Vec2d>(i)[0];
double y = keypointsPatternLocation.at<Vec2d>(i)[1];
x = x / _patternImageSize.width * _patternWidth;
y = y / _patternImageSize.height * _patternHeight;
objectPoints.push_back(Vec3d(x, y, 0));
}
Mat(objectPoints).convertTo(r[1], objectPointsType);
return r;
}
RandomPatternGenerator::RandomPatternGenerator(int imageWidth, int imageHeight)
{
_imageWidth = imageWidth;
_imageHeight = imageHeight;
}
void RandomPatternGenerator::generatePattern()
{
Mat pattern = Mat(_imageHeight, _imageWidth, CV_32F, Scalar(0.));
int m = 5;
int count = 0;
while(m < _imageWidth)
{
int n = (int)std::floor(double(_imageHeight) / double(_imageWidth) * double(m)) + 1;
Mat r = Mat(n, m, CV_32F);
cv::randn(r, Scalar::all(0), Scalar::all(1));
cv::resize(r, r, Size(_imageWidth ,_imageHeight));
double min_r, max_r;
minMaxLoc(r, &min_r, &max_r);
r = (r - (float)min_r) / float(max_r - min_r);
pattern += r;
count += 1;
m *= 2;
}
pattern = pattern / count * 255;
pattern.convertTo(pattern, CV_8U);
equalizeHist(pattern, pattern);
pattern.copyTo(_pattern);
}
cv::Mat RandomPatternGenerator::getPattern()
{
return _pattern;
}
}} //namespace randpattern, cv
\ No newline at end of file
modules/ccalib/tutorial/data/omni_calib_data.xml 0 → 100644
View file @ 9011368a
<?xml version="1.0"?>
<opencv_storage>
<objectPoints>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"3d"</dt>
<data>
0. 0. 0. 2.0000000000000001e-001 0. 0. 4.0000000000000002e-001 0.
0. 6.0000000000000009e-001 0. 0. 8.0000000000000004e-001 0. 0. 1.
0. 0. 0. 2.0000000000000001e-001 0. 2.0000000000000001e-001
2.0000000000000001e-001 0. 4.0000000000000002e-001
2.0000000000000001e-001 0. 6.0000000000000009e-001
2.0000000000000001e-001 0. 8.0000000000000004e-001
2.0000000000000001e-001 0. 1. 2.0000000000000001e-001 0. 0.
4.0000000000000002e-001 0. 2.0000000000000001e-001
4.0000000000000002e-001 0. 4.0000000000000002e-001
4.0000000000000002e-001 0. 6.0000000000000009e-001
4.0000000000000002e-001 0. 8.0000000000000004e-001
4.0000000000000002e-001 0. 1. 4.0000000000000002e-001 0. 0.
6.0000000000000009e-001 0. 2.0000000000000001e-001
6.0000000000000009e-001 0. 4.0000000000000002e-001
6.0000000000000009e-001 0. 6.0000000000000009e-001
6.0000000000000009e-001 0. 8.0000000000000004e-001
6.0000000000000009e-001 0. 1. 6.0000000000000009e-001 0. 0.
8.0000000000000004e-001 0. 2.0000000000000001e-001
8.0000000000000004e-001 0. 4.0000000000000002e-001
8.0000000000000004e-001 0. 6.0000000000000009e-001
8.0000000000000004e-001 0. 8.0000000000000004e-001
8.0000000000000004e-001 0. 1. 8.0000000000000004e-001 0. 0. 1. 0.
2.0000000000000001e-001 1. 0. 4.0000000000000002e-001 1. 0.
6.0000000000000009e-001 1. 0. 8.0000000000000004e-001 1. 0. 1. 1.
0. 0. 1.2000000000000002e+000 0. 2.0000000000000001e-001
1.2000000000000002e+000 0. 4.0000000000000002e-001
1.2000000000000002e+000 0. 6.0000000000000009e-001
1.2000000000000002e+000 0. 8.0000000000000004e-001
1.2000000000000002e+000 0. 1. 1.2000000000000002e+000 0. 0.
1.4000000000000001e+000 0. 2.0000000000000001e-001
1.4000000000000001e+000 0. 4.0000000000000002e-001
1.4000000000000001e+000 0. 6.0000000000000009e-001
1.4000000000000001e+000 0. 8.0000000000000004e-001
1.4000000000000001e+000 0. 1. 1.4000000000000001e+000 0. 0.
1.6000000000000001e+000 0. 2.0000000000000001e-001
1.6000000000000001e+000 0. 4.0000000000000002e-001
1.6000000000000001e+000 0. 6.0000000000000009e-001
1.6000000000000001e+000 0. 8.0000000000000004e-001
1.6000000000000001e+000 0. 1. 1.6000000000000001e+000 0.</data></_></objectPoints>
<imagePoints>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
6.75490112e+002 2.58054169e+002 6.56950867e+002 2.73248138e+002
6.36838318e+002 2.90432434e+002 6.16385193e+002 3.13483246e+002
5.91500000e+002 337. 569. 359. 6.61358582e+002 2.31272415e+002
6.40136780e+002 2.45905136e+002 6.18555176e+002 2.64595276e+002
5.92727356e+002 2.85703613e+002 5.65221863e+002 3.12442780e+002
5.42423889e+002 3.41452393e+002 6.43457520e+002 2.03086868e+002
6.21402466e+002 2.17740417e+002 5.93600830e+002 2.37447601e+002
5.65232788e+002 2.59174164e+002 5.37838196e+002 2.84915863e+002
5.12503052e+002 3.16757507e+002 6.24005432e+002 1.74736618e+002
5.97496338e+002 1.88732834e+002 5.67288635e+002 2.07220291e+002
5.38015198e+002 2.30461014e+002 5.07095947e+002 2.58994263e+002
4.77491852e+002 2.92051727e+002 6.01037720e+002 1.46273209e+002
5.73469177e+002 1.59748337e+002 5.40935669e+002 1.77509201e+002
5.06503784e+002 2.02032761e+002 4.74068848e+002 2.32182053e+002
4.42500122e+002 2.66570831e+002 5.77138550e+002 1.19448029e+002
5.45628052e+002 1.32241302e+002 5.11336334e+002 1.50559265e+002
4.76117249e+002 1.75316269e+002 4.40502075e+002 2.05725952e+002
4.07965607e+002 2.41970963e+002 5.51863831e+002 9.43849182e+001
5.18395569e+002 1.07406387e+002 4.82542755e+002 1.26161377e+002
4.44942627e+002 1.51338882e+002 4.08494537e+002 1.82473099e+002
3.74957062e+002 2.19434235e+002 5.26199463e+002 7.13332672e+001
4.90725861e+002 8.47979507e+001 4.54244202e+002 1.03585014e+002
4.15386475e+002 1.29469131e+002 3.77955872e+002 1.61422897e+002
3.44593353e+002 1.99354340e+002 5.00305573e+002 4.98030853e+001
4.63683289e+002 6.37438354e+001 4.25043182e+002 8.33283768e+001
3.87471771e+002 1.09548264e+002 3.49389191e+002 1.41874283e+002
3.16625092e+002 1.79468613e+002</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
7.84090210e+002 3.88017731e+002 7.58447571e+002 3.71531097e+002
7.31788330e+002 3.55191376e+002 7.01500000e+002 341. 674. 329.
6.47506470e+002 3.17759216e+002 8.03502258e+002 3.63574219e+002
7.75521851e+002 3.44961121e+002 746. 328. 715. 3.12500000e+002
6.84290955e+002 3.00195953e+002 655. 289. 8.23624084e+002
3.35837616e+002 7.93792664e+002 3.16025818e+002 7.60739319e+002
2.97872772e+002 7.29195923e+002 2.81551361e+002 6.95991028e+002
2.68595581e+002 6.64197266e+002 2.57892395e+002 8.43718933e+002
3.07062378e+002 8.12469604e+002 2.84912384e+002 7.79037659e+002
2.66707306e+002 7.41777527e+002 2.49394852e+002 7.07556702e+002
2.34234406e+002 6.72774292e+002 2.27675980e+002 8.61286499e+002
2.75579224e+002 8.30026917e+002 2.53900146e+002 7.93825806e+002
2.32928696e+002 7.57441345e+002 2.16481247e+002 7.20349548e+002
2.02759094e+002 6.83531921e+002 1.94480438e+002 8.79620789e+002
2.44113449e+002 8.46151184e+002 2.20349777e+002 8.09496887e+002
2.00622604e+002 7.70929443e+002 1.82847610e+002 7.30988708e+002
1.70821991e+002 6.93835693e+002 1.63439514e+002 8.94767029e+002
2.13258896e+002 8.60505676e+002 1.89982834e+002 8.22427734e+002
1.68008087e+002 7.82129700e+002 1.51629501e+002 7.41957092e+002
1.40314484e+002 7.03475403e+002 1.33016800e+002 9.08190430e+002
1.83071915e+002 8.72554749e+002 1.58984772e+002 8.33662354e+002
1.38905655e+002 7.93456970e+002 1.22534683e+002 7.52483276e+002
1.12002495e+002 7.12622009e+002 1.05611115e+002 9.20126831e+002
1.53868591e+002 8.83432190e+002 1.30497757e+002 8.43475952e+002
1.10962593e+002 8.02609802e+002 9.46659164e+001 7.62433105e+002
8.44985809e+001 7.22421509e+002 7.95673294e+001</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
7.92344788e+002 1.23433441e+002 7.83113831e+002 1.56123978e+002
7.73464417e+002 1.87453384e+002 7.63411438e+002 2.17998337e+002
7.53567810e+002 2.45984650e+002 7.44753601e+002 2.71632080e+002
7.60217590e+002 1.08415695e+002 7.52620911e+002 1.42628448e+002
7.44526428e+002 1.76912338e+002 7.35988892e+002 2.09661407e+002
7.28117737e+002 2.39632019e+002 7.19922302e+002 2.67545227e+002
7.22557617e+002 9.39710464e+001 7.16941467e+002 1.30629669e+002
7.10926208e+002 1.67611130e+002 7.04934631e+002 2.02370895e+002
6.98490540e+002 2.34473831e+002 6.93224060e+002 2.63872314e+002
6.80145203e+002 8.36722336e+001 6.76818970e+002 1.22316780e+002
6.73215637e+002 1.60653091e+002 6.70026489e+002 1.97099380e+002
6.66568665e+002 2.31376785e+002 6.62879944e+002 2.62516785e+002
6.32677307e+002 7.81240692e+001 6.32470459e+002 1.17437523e+002
6.32117493e+002 1.57589417e+002 6.32748596e+002 1.95813293e+002
6.30779907e+002 2.31080444e+002 6.31525696e+002 2.61573303e+002
5.83167114e+002 7.81999664e+001 5.86345337e+002 1.18803757e+002
5.89284363e+002 1.58127457e+002 5.92464355e+002 1.96512299e+002
5.95569031e+002 2.31983124e+002 599. 2.63500000e+002
5.34004700e+002 8.45869598e+001 5.39235229e+002 1.25196678e+002
5.46483337e+002 1.64360748e+002 5.52699951e+002 2.01743942e+002
5.59695923e+002 2.36428940e+002 5.66886475e+002 2.66694885e+002
4.87107574e+002 9.68924789e+001 4.96528351e+002 1.36307755e+002
5.05490631e+002 1.73577209e+002 5.15388550e+002 2.10275955e+002
5.25177612e+002 2.43547882e+002 5.34730652e+002 2.73751343e+002
4.44378540e+002 1.13022438e+002 4.55947266e+002 1.51598907e+002
4.68412659e+002 1.86861725e+002 4.80615326e+002 2.21379471e+002
4.92385834e+002 2.51911041e+002 5.05001953e+002 2.81006958e+002</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
4.81378052e+002 3.22624115e+002 4.80406860e+002 3.48465851e+002
4.79837738e+002 3.75912994e+002 4.81462616e+002 4.04819092e+002
4.83980530e+002 4.33634308e+002 4.87793243e+002 4.60171875e+002
4.54013733e+002 3.18167145e+002 4.51625885e+002 3.45765320e+002
4.50526794e+002 3.75426971e+002 4.51838287e+002 4.06206909e+002
4.53470581e+002 4.38935760e+002 4.58468933e+002 4.69474152e+002
4.23284729e+002 3.13804901e+002 4.19377289e+002 3.43510986e+002
4.17511902e+002 3.76080963e+002 4.18134125e+002 4.09680328e+002
4.21365753e+002 4.44091339e+002 4.26660858e+002 4.77992554e+002
3.89624908e+002 3.10471985e+002 3.84562958e+002 3.42330597e+002
3.81837799e+002 3.76874817e+002 3.82294586e+002 4.13676239e+002
3.85513824e+002 4.50580383e+002 3.92000275e+002 4.87093964e+002
3.53710693e+002 3.08246033e+002 3.47404022e+002 3.42408508e+002
3.44254639e+002 3.79191742e+002 3.43830841e+002 4.18523499e+002
3.48014771e+002 4.58329376e+002 3.55914246e+002 4.96030151e+002
3.17335358e+002 3.06980377e+002 3.09772308e+002 3.42725769e+002
3.05612762e+002 3.82554993e+002 3.05464905e+002 4.24428253e+002
3.09292694e+002 4.66695251e+002 3.18388306e+002 5.07142700e+002
2.80019928e+002 3.07089233e+002 2.71911407e+002 3.44642975e+002
2.66643463e+002 3.86568085e+002 2.65566803e+002 4.30820862e+002
2.70939453e+002 4.74563324e+002 2.81078247e+002 5.16716125e+002
2.40957367e+002 3.08758362e+002 2.31442734e+002 3.48210541e+002
2.27068802e+002 3.91975769e+002 2.25384995e+002 4.37844238e+002
2.31651993e+002 4.83715027e+002 2.42571533e+002 5.28483398e+002
2.02110733e+002 3.09812042e+002 1.94327621e+002 3.51957764e+002
1.87146088e+002 3.98251556e+002 1.87980118e+002 4.44944000e+002
1.91585983e+002 4.91924042e+002 2.04436539e+002 5.38972961e+002</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
2.98078094e+002 2.37883362e+002 3.35515320e+002 2.48331757e+002
3.71690491e+002 2.59992523e+002 4.05996399e+002 2.71813202e+002
4.38327240e+002 2.84377747e+002 4.67610535e+002 2.96125549e+002
2.76246307e+002 2.72641418e+002 3.17697021e+002 2.81506042e+002
3.55573639e+002 2.91446899e+002 3.93156860e+002 3.01325012e+002
4.27950684e+002 3.12004181e+002 4.59678680e+002 3.22413574e+002
2.56363251e+002 3.13856415e+002 2.99295868e+002 3.21094360e+002
3.40440948e+002 3.28280487e+002 3.80945679e+002 3.35933075e+002
4.18308960e+002 3.43917114e+002 4.52452362e+002 3.51947723e+002
2.39919037e+002 3.61791626e+002 2.85563354e+002 3.65437958e+002
3.28859955e+002 3.69890686e+002 3.70934082e+002 3.74754761e+002
4.11297394e+002 3.79269104e+002 4.46561798e+002 3.84470093e+002
2.29331482e+002 4.13949615e+002 2.76502441e+002 4.14469330e+002
3.21892761e+002 4.15404633e+002 3.65014740e+002 4.16746887e+002
4.05875916e+002 4.18226929e+002 4.42373169e+002 4.19298706e+002
2.25439499e+002 4.66823242e+002 2.73527618e+002 4.65384979e+002
3.19782501e+002 4.63428619e+002 3.62731781e+002 4.60680084e+002
4.04263794e+002 4.57940735e+002 4.40636688e+002 4.55776001e+002
2.30218567e+002 5.20786621e+002 2.77071594e+002 5.14983215e+002
3.23104034e+002 5.08725159e+002 3.65094299e+002 5.03015442e+002
4.05360077e+002 4.97127014e+002 4.41966370e+002 4.91036346e+002
2.40612778e+002 5.70426086e+002 2.85953003e+002 5.62237854e+002
3.30600311e+002 5.52684509e+002 3.71975464e+002 5.42622925e+002
4.09923431e+002 5.33989502e+002 4.44944977e+002 5.25030884e+002
2.55944458e+002 6.14003845e+002 2.99916656e+002 6.03268311e+002
3.41513885e+002 5.91473389e+002 3.80401947e+002 5.79480225e+002
4.16935638e+002 5.67436584e+002 4.50069733e+002 5.56018372e+002</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
3.50848297e+002 7.14228760e+002 3.39568451e+002 6.80527710e+002
3.31520844e+002 6.47583862e+002 3.25584290e+002 6.16500488e+002
3.22707947e+002 5.87258911e+002 3.21184540e+002 5.60773315e+002
3.77261963e+002 7.01518982e+002 3.65311646e+002 6.68576538e+002
3.55806183e+002 6.36336304e+002 3.49578125e+002 6.06026062e+002
3.45301941e+002 5.77650146e+002 3.42621765e+002 5.51476318e+002
4.04504059e+002 6.87412781e+002 3.91320770e+002 6.55220886e+002
3.80983765e+002 6.24722473e+002 3.73301453e+002 5.95179749e+002
3.68130768e+002 5.67810120e+002 3.65298218e+002 5.42442078e+002
4.31546967e+002 6.72458313e+002 4.17646942e+002 6.42048096e+002
4.06438263e+002 6.12466675e+002 3.98071136e+002 5.83806885e+002
3.92286835e+002 5.57391174e+002 3.88171814e+002 5.32996887e+002
4.58466370e+002 6.56727600e+002 4.44212433e+002 6.27420959e+002
4.32595306e+002 5.99160889e+002 4.23473755e+002 5.71937195e+002
4.16379791e+002 5.47057983e+002 4.11460785e+002 5.23456848e+002
4.85465790e+002 6.40406189e+002 4.70502625e+002 6.12528503e+002
4.58568359e+002 5.85643738e+002 4.48552917e+002 5.59496521e+002
4.39335297e+002 5.35315491e+002 4.35449188e+002 5.11826019e+002
5.11575684e+002 6.23271973e+002 4.96589050e+002 5.97307983e+002
4.83748962e+002 5.71929810e+002 4.74580475e+002 5.48551331e+002
4.64500000e+002 5.25500000e+002 4.57500000e+002 504.
5.36280518e+002 6.06423767e+002 5.21435181e+002 5.81569031e+002
5.08335693e+002 5.58358887e+002 4.96972076e+002 5.35987671e+002
4.87500000e+002 515. 480. 4.95500000e+002 5.58491699e+002
5.88809082e+002 5.44518311e+002 5.66903748e+002 5.31299805e+002
5.45354065e+002 5.19564819e+002 5.24556335e+002 5.09763062e+002
5.04446228e+002 4.99442291e+002 4.86084625e+002</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
8.81713440e+002 3.86197113e+002 9.17583252e+002 4.03987152e+002
9.53123840e+002 4.25079559e+002 9.87299316e+002 4.48395782e+002
1.01957056e+003 4.71871674e+002 1.05150195e+003 4.96505127e+002
8.70195496e+002 4.18432007e+002 9.06274963e+002 4.37890503e+002
9.40723938e+002 4.60578949e+002 9.75081177e+002 4.86557098e+002
1.00749420e+003 5.12476074e+002 1.03804028e+003 5.39053772e+002
8.52960571e+002 4.51070099e+002 8.88932251e+002 4.74545105e+002
9.23249329e+002 5.00817688e+002 9.57479492e+002 5.26468933e+002
9.88569580e+002 5.54295715e+002 1.01776630e+003 5.80369690e+002
833. 4.85500000e+002 8.68537170e+002 5.11270020e+002
9.01563538e+002 5.37908203e+002 9.33960876e+002 5.67073425e+002
9.63848145e+002 5.94500244e+002 9.92547302e+002 6.23060425e+002
8.09514282e+002 5.19066772e+002 8.41934021e+002 5.47015015e+002
8.74326538e+002 5.76352844e+002 9.05180908e+002 6.05454163e+002
9.33955200e+002 6.33015564e+002 9.61550537e+002 6.60031067e+002
7.83425110e+002 5.48695862e+002 8.13460266e+002 5.79241699e+002
8.43861572e+002 6.08925354e+002 8.73789917e+002 6.38604370e+002
9.00273804e+002 6.66994873e+002 926. 691. 7.55429077e+002
5.78604431e+002 7.84420837e+002 6.06109802e+002 8.11835876e+002
6.36899536e+002 8.39316772e+002 6.66466309e+002 8.65523132e+002
6.94579712e+002 887. 721. 7.31811340e+002 6.02392456e+002
7.56072571e+002 6.29946777e+002 7.80895203e+002 6.59628906e+002
8.05592163e+002 6.88606934e+002 8.30125122e+002 7.16589966e+002
8.53550293e+002 7.42749756e+002 7.06514648e+002 6.19389526e+002
7.29003296e+002 6.47576904e+002 7.52428772e+002 6.77560120e+002
7.73979187e+002 7.05406006e+002 7.96596558e+002 7.32817749e+002
8.19155334e+002 7.58741821e+002</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
9.62623657e+002 1.43474075e+002 9.87476990e+002 1.86780289e+002
1.00622748e+003 2.27544540e+002 1.01850989e+003 2.68001343e+002
1.02580457e+003 3.05497681e+002 1.02905554e+003 3.40061096e+002
9.39946655e+002 1.60036179e+002 9.65124207e+002 2.02795746e+002
9.82815247e+002 2.44572601e+002 9.95420349e+002 2.84801941e+002
1.00243457e+003 3.22694733e+002 1.00672827e+003 3.55448639e+002
9.13736206e+002 1.78822800e+002 9.39209473e+002 2.21480820e+002
9.58125244e+002 2.62399445e+002 9.70636292e+002 3.02435455e+002
9.78465759e+002 3.38327209e+002 9.82429932e+002 3.70877167e+002
8.85564148e+002 1.99267883e+002 9.12097656e+002 2.40570816e+002
9.31139771e+002 2.81464874e+002 9.44167786e+002 3.20086060e+002
9.52656250e+002 3.55026093e+002 9.57049866e+002 3.86803772e+002
8.54976013e+002 2.21390503e+002 8.81532410e+002 2.62056274e+002
9.01685364e+002 3.01690674e+002 9.15212036e+002 3.39471344e+002
9.24049683e+002 3.71807831e+002 9.30585632e+002 4.01065582e+002
8.22988953e+002 2.45276154e+002 8.49244141e+002 2.84704559e+002
8.69523010e+002 3.22591766e+002 8.84555115e+002 3.57831940e+002
8.93988586e+002 3.89629944e+002 9.01408630e+002 4.16973846e+002
7.90107666e+002 2.72643127e+002 8.14884399e+002 3.09990326e+002
8.36442383e+002 3.43821808e+002 8.52355164e+002 3.76016418e+002
8.63413513e+002 4.05272797e+002 8.71420044e+002 4.30495575e+002
7.55500000e+002 2.99500000e+002 7.83207214e+002 3.32013153e+002
8.02516418e+002 3.64541840e+002 8.20689148e+002 3.95600006e+002
8.30940125e+002 4.21514069e+002 8.41085144e+002 4.42547974e+002
7.23500000e+002 3.25500000e+002 7.49073181e+002 3.55019714e+002
7.68561096e+002 3.82762939e+002 7.88711731e+002 4.10029236e+002
8.00077393e+002 4.34538788e+002 8.13500000e+002 455.</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
1.02610925e+003 5.69419373e+002 1018. 6.08500000e+002
1.00757269e+003 6.50474792e+002 9.90500000e+002 690.
9.69713196e+002 7.28559875e+002 945. 760. 9.96910217e+002
5.55883240e+002 9.88487000e+002 5.95331909e+002 9.74262878e+002
6.34519043e+002 9.59772949e+002 6.74763550e+002 9.40404785e+002
7.11471130e+002 9.15623962e+002 7.47316528e+002 9.66300659e+002
5.43581970e+002 9.58032898e+002 5.81726501e+002 9.45526917e+002
6.19927490e+002 9.29846252e+002 6.57364746e+002 9.07986694e+002
6.93463928e+002 8.85528625e+002 7.26559448e+002 9.36298889e+002
5.31255432e+002 9.27100647e+002 5.67005432e+002 9.14340454e+002
6.04617249e+002 8.97536072e+002 6.41583191e+002 8.77466370e+002
6.75493286e+002 8.55136047e+002 7.06785339e+002 9.05193909e+002
5.19209595e+002 8.96645874e+002 5.53356995e+002 8.81072083e+002
5.87715820e+002 8.66478516e+002 6.23560242e+002 8.47192688e+002
6.56474304e+002 8.25451843e+002 6.86781494e+002 8.74048035e+002
5.06574799e+002 8.63423889e+002 5.38618835e+002 8.52353210e+002
5.72285339e+002 8.35347961e+002 6.05874939e+002 8.17006592e+002
6.37074524e+002 7.96157104e+002 6.66649353e+002 8.43320862e+002
4.94560669e+002 8.31992737e+002 5.26001587e+002 8.20451355e+002
5.58649658e+002 8.04538635e+002 5.88254333e+002 7.86978882e+002
6.16805054e+002 7.68332642e+002 6.44677917e+002 8.13489441e+002
4.84499451e+002 8.03744141e+002 5.13451721e+002 7.89310547e+002
5.41404175e+002 7.76207275e+002 5.71436951e+002 7.59780945e+002
5.98410828e+002 7.42307495e+002 6.24391235e+002 7.86433960e+002
4.74941956e+002 776. 5.00500000e+002 7.62905396e+002
5.25627258e+002 7.48652344e+002 5.54555420e+002 7.34200806e+002
5.79552063e+002 7.18498901e+002 6.04499268e+002</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
9.81431458e+002 1.50723282e+002 1.01354156e+003 1.89045822e+002
1.03995068e+003 2.30531693e+002 1057. 272. 1073. 322.
1.08372375e+003 3.59986786e+002 9.53933044e+002 1.70431076e+002
9.85864075e+002 2.09328415e+002 1.01058606e+003 2.48266327e+002
1.03053076e+003 2.91816650e+002 1.04453125e+003 3.33481750e+002
1.05310962e+003 3.73423157e+002 9.26057983e+002 1.91534622e+002
9.55485291e+002 2.28094772e+002 9.79567627e+002 2.67866821e+002
9.99413330e+002 3.09235443e+002 1.01355310e+003 3.50008392e+002
1.02151367e+003 3.88853394e+002 8.96944275e+002 2.13524841e+002
9.25470215e+002 2.48956131e+002 9.49139709e+002 2.87351990e+002
9.67360901e+002 3.26701294e+002 9.81027039e+002 3.65454712e+002
9.89696350e+002 4.02052551e+002 8.67878784e+002 2.35145157e+002
8.93921936e+002 2.69948578e+002 9.17444458e+002 3.06666321e+002
9.35262207e+002 3.44310059e+002 9.49273499e+002 3.80840088e+002
9.57196228e+002 4.15376526e+002 8.37211060e+002 2.57732269e+002
8.63078918e+002 2.90968628e+002 8.84456726e+002 3.25454163e+002
9.02282166e+002 3.60872833e+002 9.15394653e+002 3.95404449e+002
9.25000916e+002 4.27231750e+002 8.07257935e+002 2.80493073e+002
8.31424500e+002 3.11445557e+002 8.52416260e+002 3.43626923e+002
8.69502930e+002 3.76587524e+002 8.83034119e+002 4.08043854e+002
8.92384033e+002 4.37808990e+002 7.79647278e+002 3.02607025e+002
8.01500000e+002 331. 8.21078003e+002 3.61130402e+002
8.36605713e+002 3.90487549e+002 8.50583069e+002 4.20562988e+002
8.61375000e+002 4.47132935e+002 7.53991333e+002 3.22964294e+002
7.74386963e+002 3.48028870e+002 7.92334412e+002 3.76588531e+002
8.08500000e+002 4.03500000e+002 8.21683960e+002 4.30331360e+002
8.32711182e+002 4.55293457e+002</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
943. 665. 9.16278015e+002 6.53599182e+002 8.89926086e+002
6.39973694e+002 8.63574951e+002 6.26499451e+002 8.37769043e+002
6.12583801e+002 8.12686646e+002 5.98407715e+002 9.62436462e+002
6.41479614e+002 9.34012207e+002 6.28588379e+002 9.06015320e+002
6.16000183e+002 8.78138000e+002 6.02532837e+002 8.50593689e+002
5.89413879e+002 8.23956055e+002 5.75843201e+002 9.79901611e+002
6.12453613e+002 9.50447876e+002 5.99915649e+002 9.21525208e+002
5.87731018e+002 892. 5.75562622e+002 8.62920715e+002
5.62961060e+002 8.34826538e+002 5.50825378e+002 9.96176331e+002
5.78932007e+002 9.65559021e+002 5.67786194e+002 9.34909790e+002
5.56707581e+002 9.04852844e+002 5.45350830e+002 8.74290710e+002
5.34053772e+002 8.44917908e+002 5.23278564e+002 1.00932385e+003
5.42889587e+002 9.77957153e+002 5.32451477e+002 9.46471741e+002
5.22616333e+002 9.15226868e+002 5.12911560e+002 8.83610840e+002
5.02954590e+002 8.53250366e+002 4.93492188e+002 1.01908594e+003
5.04993866e+002 9.87409180e+002 4.95473785e+002 9.55026733e+002
4.86801392e+002 9.22781494e+002 4.78766449e+002 8.91075867e+002
4.70492035e+002 8.59416992e+002 4.62671021e+002 1.02507336e+003
4.65049469e+002 9.92578613e+002 4.57474152e+002 9.60626831e+002
4.50082123e+002 9.28285034e+002 4.43053711e+002 8.95946228e+002
4.36700684e+002 8.64128967e+002 4.31439453e+002 1.02649512e+003
4.25967773e+002 9.94566406e+002 4.19460480e+002 9.62663879e+002
4.14105835e+002 9.30171631e+002 4.08365509e+002 8.97982483e+002
4.04065765e+002 8.66648926e+002 4.00205078e+002 1.02453589e+003
3.87660034e+002 9.92254456e+002 3.82540924e+002 9.61000061e+002
3.78366821e+002 9.29085388e+002 3.74990723e+002 8.96591003e+002
3.72973572e+002 8.67031006e+002 3.71491272e+002</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
9.92850464e+002 4.18888062e+002 9.59441101e+002 4.31965637e+002
9.27529907e+002 4.42658875e+002 8.95500000e+002 4.54500000e+002
8.64757568e+002 4.62105591e+002 8.36500000e+002 470.
9.92081848e+002 3.90383728e+002 9.56924988e+002 4.05250916e+002
9.22091003e+002 4.20337341e+002 8.87402649e+002 4.33017029e+002
8.56380310e+002 4.44641022e+002 829. 455. 9.86754822e+002
3.57046844e+002 9.51323853e+002 3.75313019e+002 9.13420715e+002
3.93538635e+002 8.77389587e+002 4.08586243e+002 8.42382935e+002
4.23987030e+002 8.14038635e+002 4.34158386e+002 9.78456604e+002
3.15776184e+002 9.40002991e+002 3.39462708e+002 9.02653076e+002
3.60630981e+002 8.62497986e+002 3.81124268e+002 8.26435364e+002
3.97412628e+002 7.95500000e+002 413. 9.62976929e+002
2.68506958e+002 9.23128662e+002 2.96603394e+002 8.83157654e+002
3.23671356e+002 8.46422974e+002 3.46435120e+002 8.07500000e+002
3.69500000e+002 7.75500000e+002 3.87500000e+002 9.37054321e+002
2.19495621e+002 8.98032654e+002 2.50468826e+002 8.59119385e+002
2.81512115e+002 8.18123779e+002 3.12852264e+002 7.82500000e+002
3.38500000e+002 7.49654114e+002 3.60178528e+002 8.99865540e+002
1.68641479e+002 8.62528259e+002 2.03040527e+002 8.23181213e+002
2.38275269e+002 7.83826294e+002 2.74359985e+002 7.51240540e+002
3.08780212e+002 721. 335. 8.50886414e+002 1.21732185e+002
8.15732605e+002 1.59313904e+002 7.80988892e+002 1.99837677e+002
7.45621826e+002 2.38860184e+002 7.17436951e+002 2.75985657e+002
690. 309. 7.96992249e+002 8.39213028e+001 7.64863586e+002
1.24419228e+002 7.32131287e+002 1.67438324e+002 7.03050720e+002
2.09443497e+002 6.77796021e+002 2.50547577e+002 6.56559998e+002
2.84677460e+002</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
6.21648621e+002 2.64615540e+002 6.24922363e+002 2.37001770e+002
6.27736938e+002 2.07531189e+002 6.31982788e+002 1.76568405e+002
6.36301697e+002 1.45738327e+002 6.41479370e+002 1.12997490e+002
6.49474060e+002 2.68830536e+002 6.54831299e+002 2.42088898e+002
6.59611450e+002 2.12451355e+002 6.66164429e+002 1.82008011e+002
6.72218506e+002 1.50188156e+002 6.78809082e+002 1.18253792e+002
6.76446716e+002 2.75682434e+002 6.84066223e+002 2.48810486e+002
6.91582581e+002 2.19964661e+002 6.99929382e+002 1.89697372e+002
7.08432861e+002 1.58282516e+002 7.16680298e+002 1.26765770e+002
7.02703430e+002 2.81968536e+002 7.13080933e+002 2.57524872e+002
7.22746216e+002 2.29195480e+002 7.33006653e+002 2.00202789e+002
7.43558472e+002 1.69573883e+002 7.53214417e+002 1.38527176e+002
7.29548523e+002 2.92423767e+002 7.39874817e+002 2.66248413e+002
7.51979431e+002 2.41330765e+002 7.64206543e+002 2.12827515e+002
7.76047668e+002 1.83567627e+002 7.87649597e+002 1.54007019e+002
7.53089783e+002 3.01017059e+002 7.67395508e+002 2.78472321e+002
7.79268372e+002 2.53931122e+002 7.92513916e+002 2.27305573e+002
8.05449707e+002 1.99512634e+002 8.18565918e+002 1.71618118e+002
7.76721497e+002 3.11713715e+002 7.89275085e+002 2.91075226e+002
8.03490906e+002 2.67230316e+002 8.17492493e+002 2.42576752e+002
8.31731934e+002 2.16472717e+002 8.45407410e+002 1.89744308e+002
7.95295349e+002 3.22685059e+002 8.09476746e+002 3.02655273e+002
8.24271973e+002 2.81040771e+002 8.39294617e+002 2.57719421e+002
8.54225525e+002 2.33317703e+002 8.68694580e+002 2.09246872e+002
8.12529663e+002 3.32871613e+002 8.27758423e+002 3.14300171e+002
8.42943970e+002 2.94153076e+002 8.57932678e+002 2.72886414e+002
8.73576050e+002 2.50439499e+002 8.88212158e+002 2.27181015e+002</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
4.87553070e+002 3.33081940e+002 4.62614197e+002 3.02489258e+002
4.37576416e+002 2.69530884e+002 4.12060089e+002 2.35439728e+002
3.88464539e+002 2.01847916e+002 3.67124573e+002 1.69329208e+002
5.19967590e+002 3.11576843e+002 4.97410217e+002 2.78322418e+002
4.73922424e+002 2.43532043e+002 4.50377716e+002 2.06583496e+002
4.28351135e+002 1.72629303e+002 4.08012329e+002 1.37952560e+002
5.54011902e+002 2.91292267e+002 5.34618408e+002 2.56294556e+002
5.14143921e+002 2.19427399e+002 4.93516510e+002 1.81965973e+002
4.74155090e+002 1.46653778e+002 4.54185089e+002 1.11583130e+002
5.90221252e+002 2.72753845e+002 5.74075134e+002 2.38453003e+002
5.56422119e+002 2.00955048e+002 5.38824219e+002 1.63484421e+002
5.19426758e+002 1.27573326e+002 5.01227142e+002 9.22989960e+001
6.25049805e+002 2.60661194e+002 6.10513733e+002 2.25872940e+002
5.96240234e+002 1.87419449e+002 5.81616028e+002 1.50480957e+002
5.65533997e+002 1.14384705e+002 5.49490784e+002 8.06278229e+001
6.54708191e+002 2.51049423e+002 6.46017090e+002 2.16017548e+002
6.34464661e+002 1.80174149e+002 6.21993286e+002 1.43330460e+002
6.08453613e+002 1.07988892e+002 5.94316040e+002 7.44691238e+001
6.83734497e+002 2.42870544e+002 6.76964294e+002 2.10122986e+002
6.68720886e+002 1.76243042e+002 6.58802124e+002 1.40617599e+002
6.47013855e+002 1.06712463e+002 6.34501831e+002 7.43235016e+001
7.08225525e+002 2.38328049e+002 7.03408813e+002 2.06709641e+002
6.98146729e+002 1.74235916e+002 6.89880737e+002 1.41212967e+002
6.80672668e+002 1.08417931e+002 6.70395264e+002 7.71330414e+001
7.29351624e+002 2.34441971e+002 7.26809326e+002 2.05535583e+002
7.22509155e+002 1.75092453e+002 7.16651672e+002 1.43296677e+002
7.09192444e+002 1.12335365e+002 7.01085388e+002 8.23730545e+001</data></_>
<_ type_id="opencv-matrix">
<rows>54</rows>
<cols>1</cols>
<dt>"2f"</dt>
<data>
7.44175171e+002 3.41905121e+002 7.81475098e+002 3.30542725e+002
8.21084900e+002 3.18639160e+002 8.59480896e+002 3.07950623e+002
8.97504517e+002 2.99007721e+002 9.32098755e+002 2.91761475e+002
7.56500000e+002 379. 7.94500000e+002 370. 8.33592896e+002
3.60368561e+002 8.72396912e+002 3.52087860e+002 9.09731689e+002
3.43209076e+002 9.44365051e+002 3.36201935e+002 7.66500000e+002
416. 8.02858948e+002 4.08045868e+002 8.43303467e+002
4.00438141e+002 8.80331055e+002 3.93918793e+002 9.17367004e+002
3.86928528e+002 9.51384460e+002 3.80909027e+002 7.73453308e+002
4.50313354e+002 8.09500000e+002 445. 8.47631348e+002
4.40441315e+002 8.84517883e+002 4.33624207e+002 9.19723938e+002
4.28626343e+002 9.52826843e+002 4.22249237e+002 779.
4.80500000e+002 8.13571777e+002 4.78707336e+002 8.48882446e+002
4.74345764e+002 8.84045044e+002 4.70790955e+002 9.18399536e+002
4.65920532e+002 9.50262146e+002 4.60167633e+002 7.81162781e+002
5.08493103e+002 8.14266479e+002 5.06445465e+002 8.47477173e+002
5.05494507e+002 8.81002136e+002 5.02996338e+002 9.14179688e+002
4.99080872e+002 9.45393311e+002 4.93634796e+002 7.82686035e+002
5.32574951e+002 8.13157410e+002 5.31836975e+002 8.44857605e+002
5.31897461e+002 8.76928589e+002 5.30486755e+002 9.08286499e+002
5.27373474e+002 935. 527. 7.84048706e+002 5.52915894e+002
8.12160828e+002 5.54131714e+002 8.41816040e+002 5.54021057e+002
8.71936523e+002 5.53551270e+002 9.01274780e+002 5.51488403e+002
9.28989075e+002 5.49090820e+002 7.83927795e+002 5.70247925e+002
8.10133240e+002 5.71974304e+002 8.37997986e+002 5.73809448e+002
8.66329834e+002 5.73433228e+002 8.93925415e+002 5.72344299e+002
9.20939758e+002 5.70050781e+002</data></_></imagePoints>
<imageSize>
1280 960</imageSize>
</opencv_storage>
modules/ccalib/tutorial/data/omni_stereocalib_data.xml 0 → 100644
View file @ 9011368a
This source diff could not be displayed because it is too large. You can view the blob instead.
modules/ccalib/tutorial/multi_camera_tutorial.markdown 0 → 100644
View file @ 9011368a
Multi-camera Calibration {#tutorial_multi_camera_main}
====================================
This tutorial will show how to use the multiple camera calibration toolbox. This toolbox is based on the usage of "random" pattern calibration object, so the tutorial is mainly two parts: an introduction to "random" pattern and multiple camera calibration.
Random Pattern Calibration Object
-------------------------------
The random pattern is an image that is randomly generated. It is "random" so that it has many feature points. After generating it, one print it out and use it as a calibration object. The following two images are random pattern and a photo taken for it.
![image](img/random_pattern.jpg)
![image](img/pattern_img.jpg)
To generate a random pattern, use the class ```cv::randpattern::RandomPatternGenerator``` in ```ccalib``` module. Run it as
```
cv::randpattern::RandomPatternGenerator generator(width, height);
generator.generatePattern();
pattern = generator.getPattern();
```
Here ```width``` and ```height``` are width and height of pattern image. After getting the pattern, print it out and take some photos of it.
Now we can use these images to calibrate camera. First, ```objectPoints``` and ```imagePoints``` need to be detected. Use class ```cv::randpattern::RandomPatternCornerFinder``` to detect them. A sample code can be
```
cv::randpattern::RandomPatternCornerFinder finder(patternWidth, patternHeight, nMiniMatches);
finder.loadPattern(pattern);
finder.computeObjectImagePoints(vecImg);
vector<Mat> objectPoints = finder.getObjectPoints();
vector<Mat> imagePoints = finder.getImagePoints();
```
Here variable ```patternWidth``` and ```patternHeight``` are physical pattern width and height with some user defined unit. ```vecImg``` is a vector of images that stores calibration images.
Second, use calibration functions like ```cv::calibrateCamera``` or ```cv::omnidir::calibrate``` to calibrate camera.
Multiple Cameras Calibration
-------------------------------
Now we move to multiple camera calibration, so far this toolbox must use random pattern object.
To calibrate multiple cameras, we first need to take some photos of random pattern. Of cause, to calibrate the extrinsic parameters, one pattern need to be viewed by multiple cameras (at least two) at the same time. Another thing is that to help the program know which camera and which pattern the photo is taken, the image file should be named as "cameraIdx-timestamp.*". Photos with same timestamp means that they are the same object taken by several cameras. In addition, cameraIdx should start from 0. Some examples of files names are "0-129.png", "0-187.png", "1-187", "2-129".
Then, we can run multiple cameras calibration as
```
cv::multicalib::MultiCameraCalibration multiCalib(cameraType, nCamera, inputFilename,patternWidth, patternHeight, showFeatureExtraction, nMiniMatches);
multiCalib.run();
multiCalib.writeParameters(outputFilename);
```
Here ```cameraType``` indicates the camera type, ```multicalib::MultiCameraCalibration::PINHOLE``` and ```multicalib::MultiCameraCalibration::OMNIDIRECTIONAL``` are supported. For omnidirectional camera, you can refer to ```cv::omnidir``` module for detail. ```nCamera``` is the number of camers. ```inputFilename``` is the name of a file generated by ```imagelist_creator``` from ```opencv/sample```. It stores names of random pattern and calibration images, the first file name is the name of random pattern. ```patternWidth``` and ```patternHeight``` are physical width and height of pattern. ```showFeatureExtraction``` is a flags to indicate whether show feature extraction process. ```nMiniMatches``` is a minimal points that should be detected in each frame, otherwise this frame will be abandoned. ```outputFilename``` is a xml file name to store parameters.
modules/ccalib/tutorial/omnidir_tutorial.markdown 0 → 100644
View file @ 9011368a
Omnidirectional Cameara Calibration {#tutorial_omnidir_calib_main}
======================
This module includes calibration, rectification and stereo reconstruction of omnidirectional camearas. The camera model is described in this paper:
*C. Mei and P. Rives, Single view point omnidirectional camera calibration from planar grids, in ICRA 2007.*
The model is capable of modeling catadioptric cameras and fisheye cameras, which may both have very large field of view.
The implementation of the calibration part is based on Li's calibration toolbox:
*B. Li, L. Heng, K. Kevin and M. Pollefeys, "A Multiple-Camera System Calibration Toolbox Using A Feature Descriptor-Based Calibration Pattern", in IROS 2013.*
This tutorial will introduce the following parts of omnidirectional camera calibartion module:
- calibrate a single camera.
- calibrate a stereo pair of cameras.
- rectify images so that large distoration is removed.
- reconstruct 3D from two stereo images, with large filed of view.
- comparison with fisheye model in opencv/calib3d/
Single Camera Calibration
---------------------
The first step to calibrate camera is to get a calibration pattern and take some photos. Several kinds of patterns are supported by OpenCV, like checkerborad and circle grid. A new pattern named random pattern can also be used, you can refer to opencv_contrib/modules/ccalib for more details.
Next step is to extract corners from calibration pattern. For checkerboard, use OpenCV function ```cv::findChessboardCorners```; for circle grid, use ```cv::findCirclesGrid```, for random pattern, use the ```randomPatternCornerFinder``` class in opencv_contrib/modules/ccalib/src/randomPattern.hpp. Save the positions of corners in images in a variable like ```imagePoints```. The type of ```imagePoints``` may be ```std::vector<std::vector<cv::Vec2f>>```, the first vector stores corners in each frame, the second vector stores corners in an individual frame. The type can also be ```std::vector<cv::Mat>``` where the ```cv::Mat``` is ```CV_32FC2```.
Also, the corresponding 3D points in world (pattern) coordinate are required. You can compute they for yourself if you know the physical size of your pattern. Save 3D points in ```objectPoints```, similar to ```imagePoints```, it can be ```std::vector<std::vector<Vec3f>>``` or ```std::vector<cv::Mat>``` where ```cv::Mat``` is of type ```CV_32FC3```. Note the size of ```objectPoints``` and ```imagePoints``` must be the same because they are corresponding to each other.
Another thing you should input is the size of images. The file opencv_contrib/modules/ccalib/tutorial/data/omni_calib_data.xml stores an example of objectPoints, imagePoints and imageSize. Use the following code to load them:
```
cv::FileStorage fs("omni_calib_data.xml", cv::FileStorage::READ);
std::vector<cv::Mat> objectPoints, imagePoints;
cv::Size imgSize;
fs["objectPoints"] >> objectPoints;
fs["imagePoints"] >> imagePoints;
fs["imageSize"] >> imgSize;
```
Then define some variables to store the output parameters and run the calibration function like:
```
cv::Mat K, xi, D, idx;
int flags = 0;
cv::TermCriteria critia(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, 0.0001);
std::vector<cv::Mat> rvecs, tvecs;
double rms = cv::omnidir::calibrate(objectPoints, imagePoints, imgSize, K, xi, D, rvecs, tvecs, flags, critia, idx);
```
```K```, ```xi```, ```D``` are internal parameters and ```rvecs```, ```tvecs``` are external parameters that store the pose of patterns. All of them have depth of ```CV_64F```. The ```xi``` is a single value variable of Mei's model. ```idx``` is a ```CV_32S``` Mat that stores indices of images that are really used in calibration. This is due to some images are failed in the initialization step so they are not used in the final optimization. The returned value *rms* is the root mean square of reprojection errors.
The calibration supports some features, *flags* is a enumeration for some features, including:
- cv::omnidir::CALIB_FIX_SKEW
- cv::omnidir::CALIB_FIX_K1
- cv::omnidir::CALIB_FIX_K2
- cv::omnidir::CALIB_FIX_P1
- cv::omnidir::CALIB_FIX_P2
- cv::omnidir::CALIB_FIX_XI
- cv::omnidir::CALIB_FIX_GAMMA
- cv::omnidir::CALIB_FIX_CENTER
Your can specify ```flags``` to fix parameters during calibration. Use 'plus' operator to set multiple features. For example, ```CALIB_FIX_SKEW+CALIB_FIX_K1``` means fixing skew and K1.
```critia``` is the stopping critia during optimization, set it to be, for example, cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, 0.0001), which means using 200 iterations and stopping when relative change is smaller than 0.0001.
Stereo Calibration
--------------------------
Stereo calibration is to calibrate two cameras together. The output parameters include camera parameters of two cameras and the relative pose of them. To recover the relative pose, two cameras must observe the same pattern at the same time, so the ```objectPoints``` of two cameras are the same.
Now detect image corners for both cameras as discussed above to get ```imagePoints1``` and ```imagePoints2```. Then compute the shared ```objectPoints```.
An example of of stereo calibration data is stored in opencv_contrib/modules/ccalib/tutorial/data/omni_stereocalib_data.xml. Load the data by
```
cv::FileStorage fs("omni_stereocalib_data.xml", cv::FileStorage::READ);
std::vector<cv::Mat> objectPoints, imagePoints1, imagePoints2;
cv::Size imgSize1, imgSize2;
fs["objectPoints"] >> objectPoints;
fs["imagePoints1"] >> imagePoints1;
fs["imagePoints2"] >> imagePoints2;
fs["imageSize1"] >> imgSize1;
fs["imageSize2"] >> imgSize2;
```
Then do stereo calibration by
```
cv::Mat K1, K2, xi1, xi2, D1, D2;
int flags = 0;
cv::TermCriteria critia(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, 0.0001);
std::vector<cv::Mat> rvecsL, tvecsL;
cv::Mat rvec, tvec;
double rms = cv::omnidir::calibrate(objectPoints, imagePoints1, imagePoints2, imgSize1, imgSize2, K1, xi1, D1, K2, xi2, D2, rvec, tvec, rvecsL, tvecsL, flags, critia, idx);
```
Here ```rvec``` and ```tvec``` are the transform between the first and the second camera. ```rvecsL``` and ```tvecsL``` are the transforms between patterns and the first camera.
Image Rectificaiton
---------------------------
Omnidirectional images have very large distortion, so it is not compatible with human's eye balls. For better view, rectification can be applied if camera parameters are known. Here is an example of omnidirectional image of 360 degrees of horizontal field of view.
![image](img/sample.jpg)
After rectification, a perspective like view is generated. Here is one example to run image rectification in this module:
```
cv::omnidir::undistortImage(distorted, undistorted, K, D, xi, int flags, Knew, new_size)
```
The variable *distorted* and *undistorted* are the origional image and rectified image perspectively. *K*, *D*, *xi* are camera parameters. *KNew* and *new_size* are the camera matrix and image size for rectified image. *flags* is the rectification type, it can be:
- RECTIFY_PERSPECTIVE: rectify to perspective images, which will lose some filed of view.
- RECTIFY_CYLINDRICAL: rectify to cylindrical images that preserve all view.
- RECTIFY_STEREOGRAPHIC: rectify to stereographic images that may lose a little view.
- RECTIFY_LONGLATI: rectify to longitude-latitude map like a world map of the earth. This rectification can be used to stereo reconstruction but may not be friendly for view. This map is described in paper:
*Li S. Binocular spherical stereo[J]. Intelligent Transportation Systems, IEEE Transactions on, 2008, 9(4): 589-600.*
The following four images are four types of rectified images discribed above:
![image](img/sample_rec_per.jpg)
![image](img/sample_rec_cyl.jpg)
![image](img/sample_rec_ste.jpg)
![image](img/sample_rec_log.jpg)
It can be observed that perspective rectified image perserves only a little field of view and is not goodlooking. Cylindrical rectification preserves all field of view and scene is unnatural only in the middle of bottom. The distortion of stereographic in the middle of bottom is smaller than cylindrical but the distortion of other places are larger, and it can not preserve all field of view. For images with very large distortion, the longitude-latitude rectification does not give a good result, but it is available to make epipolar constraint in a line so that stereo matching can be applied in omnidirectional images.
**Note**: To have a better result, you should carefully choose ```Knew``` and it is related to your camera. In general, a smaller focal length leads to a smaller field of view and vice versa. Here are recommonded settings.
For RECTIFY_PERSPECTIVE
```
Knew = Matx33f(new_size.width/4, 0, new_size.width/2,
0, new_size.height/4, new_size.height/2,
0, 0, 1);
```
For RECTIFY_CYLINDRICAL, RECTIFY_STEREOGRAPHIC, RECTIFY_LONGLATI
```
Knew = Matx33f(new_size.width/3.1415, 0, 0,
0, new_size.height/3.1415, 0,
0, 0, 1);
```
Maybe you need to change ```(u0, v0)``` to get a better view.
Stereo Reconstruction
------------------------------
Stereo reconstruction is to reconstruct 3D points from a calibrated stereo camera pair. It is a basic problem of computer vison. However, for omnidirectional camera, it is not very popular because of the large distortion make it a little difficult. Conventional methods rectify images to perspective ones and do stereo reconstruction in perspective images. However, the last section shows that recifying to perspective images lose too much field of view, which waste the advantage of omnidirectional camera, i.e. large field of view.
The first step of stereo reconstruction is stereo rectification so that epipolar lines are horizontal lines. Here, we use longitude-latitude rectification to preserve all filed of view, or perspective rectification which is available but is not recommended. The second step is stereo matching to get a disparity map. At last, 3D points can be generated from disparity map.
The API of stereo reconstruction for omnidrectional camera is ```omnidir::stereoReconstruct```. Here we use an example to show how it works.
First, calibrate a stereo pair of cameras as describe above and get parameters like ```K1```, ```D1```, ```xi1```, ```K2```, ```D2```, ```xi2```, ```rvec```, ```tvec```. Then read two images from the first and second camera respectively, for instance, ```image1``` and ```image2```, which are shown below.
![image](img/imgs.jpg)
Second, run ```omnidir::stereoReconstruct``` like:
```
cv::Size imgSize = img1.size();
int numDisparities = 16*5;
int SADWindowSize = 5;
cv::Mat disMap;
int flag = cv::omnidir::RECTIFY_LONGLATI;
int pointType = omnidir::XYZRGB;
// the range of theta is (0, pi) and the range of phi is (0, pi)
cv::Matx33d KNew(imgSize.width / 3.1415, 0, 0, 0, imgSize.height / 3.1415, 0, 0, 0, 1);
Mat imageRec1, imageRec2, pointCloud;
cv::omnidir::stereoReconstruct(img1, img2, K1, D1, xi1, K2, D2, xi2, R, T, flag, numDisparities, SADWindowSize, imageRec1, imageRec2, disMap, imgSize, KNew, pointCloud);
```
Here variable ```flag``` indicates the recectify type, only ```RECTIFY_LONGLATI```(recommend) and ```RECTIFY_PERSPECTIVE``` make sense. ```numDisparities``` is the max disparity value and ```SADWindowSize``` is the window size of ```cv::StereoSGBM```. ```pointType``` is a flag to define the type of point cloud, ```omnidir::XYZRGB``` each point is a 6-dimensional vector, the first three elements are xyz coordinate, the last three elements are rgb color information. Another type ```omnidir::XYZ``` means each point is 3-dimensional and has only xyz coordiante.
Moreover, ```imageRec1``` and ```imagerec2``` are rectified versions of the first and second images. The epipolar lines of them have the same y-coordinate so that stereo matching becomes easy. Here are an example of them:
![image](img/lines.jpg)
It can be observed that they are well aligned. The variable ```disMap``` is the disparity map computed by ```cv::StereoSGBM``` from ```imageRec1``` and ```imageRec2```. The disparity map of the above two images is:
![image](img/disparity.jpg)
After we have disparity, we can compute 3D location for each pixel. The point cloud is stored in ```pointCloud```, which is a 3-channel or 6-channel ```cv::Mat```. We show the point cloud in the following image.
![image](img/pointCloud.jpg)
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment