WebSVN – seema-scanner – Diff – /src/cvtools.cpp


#include "cvtools.h"
 
#ifdef _WIN32
#include <cstdint>
#endif
 
#include <stdio.h>
 
namespace cvtools{
 
// Convert a 3xN matrix to a vector of Point3fs.
void matToPoints3f(const cv::Mat &mat, std::vector<cv::Point3f> &points){
 
    unsigned int nPoints = mat.cols;
    points.resize(nPoints);
 
    for(unsigned int i=0; i<nPoints; i++)
        points[i] = cv::Point3f(mat.at<float>(0, i), mat.at<float>(1, i), mat.at<float>(2, i));
}
 
// Convert a (Dim+1)xN matrix of homogenous points to a DimxN matrix of points in non-homogenous coordinates.
void convertMatFromHomogeneous(cv::Mat &src, cv::Mat &dst){
    unsigned int N = src.cols;
    unsigned int Dim = src.rows-1;
    dst.create(Dim, N, src.type());
    for(unsigned int i=0; i<N; i++){
        for(unsigned int j=0; j<Dim; j++)
            dst.at<float>(j,i) = src.at<float>(j,i)/src.at<float>(Dim,i);
    }
 
}
 
// Function to solve the hand-eye (or eye-in-hand) calibration problem.
// Finds [Omega | tau], to minimize ||[R_mark | t_mark][Omega | tau] - [Omega | tau][R | t]||^2
// Algorithm according to Tsai, Lenz, A new technique for fully autonomous and efficient 3d robotics hand-eye calibration
// DTU, 2014, Jakob Wilm
void handEyeCalibrationTsai(const std::vector<cv::Matx33f> R, const std::vector<cv::Vec3f> t, const std::vector<cv::Matx33f> R_mark, const std::vector<cv::Vec3f> t_mark, cv::Matx33f &Omega, cv::Vec3f &tau){
 
 
    int N = R.size();
    assert(N == R_mark.size());
    assert(N == t.size());
    assert(N == t_mark.size());
 
    // construct equations for rotation
    cv::Mat A(3*N, 3, CV_32F);
    cv::Mat b(3*N, 1, CV_32F);
    for(int i=0; i<N; i++){
        // angle axis representations
        cv::Vec3f rot;
        cv::Vec3f rot_mark;
        cv::Rodrigues(R[i], rot);
        cv::Rodrigues(R_mark[i], rot_mark);
 
        cv::Vec3f P = 2.0*sin(cv::norm(rot)/2.0)*cv::normalize(rot);
//std::cout << "P: " << std::endl << P << std::endl;
        cv::Vec3f P_mark = 2.0*sin(cv::norm(rot_mark)/2.0)*cv::normalize(rot_mark);
//std::cout << "P_mark: " << std::endl << P_mark << std::endl;
        cv::Vec3f sum = P+P_mark;
        cv::Mat crossProduct = (cv::Mat_<float>(3,3) << 0.0, -sum(2), sum(1), sum(2), 0.0, -sum(0), -sum(1), sum(0), 0.0);
//std::cout << "crossProduct: " << std::endl << crossProduct << std::endl;
        crossProduct.copyTo(A.rowRange(i*3, i*3+3));
 
        cv::Mat(P-P_mark).copyTo(b.rowRange(i*3, i*3+3));
    }
 
    // solve for rotation
    cv::Vec3f P_prime;
    cv::solve(A, b, P_prime, cv::DECOMP_SVD);
    cv::Vec3f P = 2.0*P_prime/(cv::sqrt(1.0 + cv::norm(P_prime)*cv::norm(P_prime)));
    float nP = cv::norm(P);
    cv::Mat crossProduct = (cv::Mat_<float>(3,3) << 0.0, -P(2), P(1), P(2), 0.0, -P(0), -P(1), P(0), 0.0);
    cv::Mat OmegaMat = (1.0-nP*nP/2.0)*cv::Mat::eye(3,3,CV_32F) + 0.5*(cv::Mat(P)*cv::Mat(P).t() + cv::sqrt(4.0 - nP*nP)*crossProduct);
    Omega = cv::Matx33f(OmegaMat);
 
    // construct equations for translation
    A.setTo(0.0);
    b.setTo(0.0);
    for(int i=0; i<N; i++){
 
        cv::Mat diff = cv::Mat(R_mark[i]) - cv::Mat::eye(3, 3, CV_32F);
        diff.copyTo(A.rowRange(i*3, i*3+3));
 
        cv::Mat diff_mark = cv::Mat(Omega*t[i] - t_mark[i]);
        diff_mark.copyTo(b.rowRange(i*3, i*3+3));
    }
 
    // solve for translation
    cv::solve(A, b, tau, cv::DECOMP_SVD);
}
 
// Function to fit two sets of corresponding pose data.
// Finds [Omega | tau], to minimize ||[R_mark | t_mark] - [Omega | tau][R | t]||^2
// Algorithm and notation according to Mili Shah, Comparing two sets of corresponding six degree of freedom data, CVIU 2011.
// DTU, 2013, Oline V. Olesen, Jakob Wilm
void fitSixDofData(const std::vector<cv::Matx33f> R, const std::vector<cv::Vec3f> t, const std::vector<cv::Matx33f> R_mark, const std::vector<cv::Vec3f> t_mark, cv::Matx33f &Omega, cv::Vec3f &tau){
 
    int N = R.size();
    assert(N == R_mark.size());
    assert(N == t.size());
    assert(N == t_mark.size());
 
    // Mean translations
    cv::Vec3f t_mean;
    cv::Vec3f t_mark_mean;
    for(int i=0; i<N; i++){
        t_mean += 1.0/N * t[i];
        t_mark_mean += 1.0/N * t_mark[i];
    }
 
    // Data with mean adjusted translations
    cv::Mat X_bar(3, 4*N, CV_32F);
    cv::Mat X_mark_bar(3, 4*N, CV_32F);
    for(int i=0; i<N; i++){
        cv::Mat(R[i]).copyTo(X_bar.colRange(i*4,i*4+3));
        cv::Mat(t[i] - t_mean).copyTo(X_bar.col(i*4+3));
        cv::Mat(R_mark[i]).copyTo(X_mark_bar.colRange(i*4,i*4+3));
        cv::Mat(t_mark[i] - t_mark_mean).copyTo(X_mark_bar.col(i*4+3));
    }
    //std::cout << X_bar << std::endl;
    // SVD
    cv::Mat W, U, VT;
    cv::SVDecomp(X_bar*X_mark_bar.t(), W, U, VT);
 
    cv::Matx33f D = cv::Matx33f::eye();
    if(cv::determinant(VT*U) < 0)
        D(3,3) = -1;
 
    // Best rotation
    Omega = cv::Matx33f(cv::Mat(VT.t()))*D*cv::Matx33f(cv::Mat(U.t()));
 
    // Best translation
    tau = t_mark_mean - Omega*t_mean;
 
}
 
// Forward distortion of points. The inverse of the undistortion in cv::initUndistortRectifyMap().
// Inspired by Pascal Thomet, http://code.opencv.org/issues/1387#note-11
// Convention for distortion parameters: http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html
void initDistortMap(const cv::Matx33f cameraMatrix, const cv::Vec<float, 5> distCoeffs, const cv::Size size, cv::Mat &map1, cv::Mat &map2){
 
    float fx = cameraMatrix(0,0);
    float fy = cameraMatrix(1,1);
    float ux = cameraMatrix(0,2);
    float uy = cameraMatrix(1,2);
 
    float k1 = distCoeffs[0];
    float k2 = distCoeffs[1];
    float p1 = distCoeffs[2];
    float p2 = distCoeffs[3];
    float k3 = distCoeffs[4];
 
    map1.create(size, CV_32F);
    map2.create(size, CV_32F);
 
    for(int col = 0; col < size.width; col++){
        for(int row = 0; row < size.height; row++){
 
            // move origo to principal point and convert using focal length
            float x = (col-ux)/fx;
            float y = (row-uy)/fy;
 
            float xCorrected, yCorrected;
 
            //Step 1 : correct distortion
            float r2 = x*x + y*y;
            //radial
            xCorrected = x * (1. + k1*r2 + k2*r2*r2 + k3*r2*r2*r2);
            yCorrected = y * (1. + k1*r2 + k2*r2*r2 + k3*r2*r2*r2);
            //tangential
            xCorrected = xCorrected + (2.*p1*x*y + p2*(r2+2.*x*x));
            yCorrected = yCorrected + (p1*(r2+2.*y*y) + 2.*p2*x*y);
 
            //convert back to pixel coordinates
            float col_displaced = xCorrected * fx + ux;
            float row_displaced = yCorrected * fy + uy;
 
            // correct the vector in the opposite direction
            map1.at<float>(row,col) = col+(col-col_displaced);
            map2.at<float>(row,col) = row +(row-row_displaced);
        }
    }
}
 
// Downsample a texture which was created in virtual column/row space for a diamond pixel array projector
cv::Mat diamondDownsample(cv::Mat &pattern){
 
    cv::Mat pattern_diamond(pattern.rows,pattern.cols/2,CV_8UC3);
 
    for(unsigned int col = 0; col < pattern_diamond.cols; col++){
        for(unsigned int row = 0; row < pattern_diamond.rows; row++){
 
            pattern_diamond.at<cv::Vec3b>(row,col)=pattern.at<cv::Vec3b>(row,col*2+row%2);
        }
    }
 
    return pattern_diamond;
 
}
 
 
void mouseCallback(int evt, int x, int y, int flags, void* param){
    cv::Mat *im = (cv::Mat*) param;
    if (evt == CV_EVENT_LBUTTONDOWN) {
        if(im->type() == CV_8UC3){
            printf("%d %d: %d, %d, %d\n",
                   x, y,
                   (int)(*im).at<cv::Vec3b>(y, x)[0],
                    (int)(*im).at<cv::Vec3b>(y, x)[1],
                    (int)(*im).at<cv::Vec3b>(y, x)[2]);
        } else if (im->type() == CV_32F) {
            printf("%d %d: %f\n",
                   x, y,
                   im->at<float>(y, x));
        }
    }
}
 
void imshow(const char *windowName, cv::Mat im, unsigned int x, unsigned int y){
 
    // Imshow
    if(!cvGetWindowHandle(windowName)){
        int windowFlags = CV_GUI_EXPANDED | CV_WINDOW_KEEPRATIO;
        cv::namedWindow(windowName, windowFlags);
        cv::moveWindow(windowName, x, y);
    }
    cv::imshow(windowName, im);
}
 
void imagesc(const char *windowName, cv::Mat im){
 
    // Imshow with scaled image
 
 
}
 
cv::Mat histimage(cv::Mat histogram){
 
    cv::Mat histImage(512, 640, CV_8UC3, cv::Scalar(0));
 
    // Normalize the result to [ 2, histImage.rows-2 ]
    cv::normalize(histogram, histogram, 2, histImage.rows-2, cv::NORM_MINMAX, -1, cv::Mat());
 
    float bin_w = (float)histImage.cols/(float)histogram.rows;
 
    // Draw main histogram
    for(int i = 1; i < histogram.rows-10; i++){
        cv::line(histImage, cv::Point( bin_w*(i-1), histImage.rows - cvRound(histogram.at<float>(i-1)) ),
                 cv::Point( bin_w*(i), histImage.rows - cvRound(histogram.at<float>(i)) ),
                 cv::Scalar(255, 255, 255), 2, 4);
    }
 
    // Draw red max
    for(int i = histogram.rows-10; i < histogram.rows; i++){
        cv::line(histImage, cv::Point( bin_w*(i-1), histImage.rows - cvRound(histogram.at<float>(i-1)) ),
                 cv::Point( bin_w*(i), histImage.rows - cvRound(histogram.at<float>(i)) ),
                 cv::Scalar(0, 0, 255), 2, 4);
    }
 
    return histImage;
}
 
void hist(const char *windowName, cv::Mat histogram, unsigned int x, unsigned int y){
 
    // Display
    imshow(windowName, histimage(histogram), x, y);
    cv::Point(1,2);
}
 
 
void writeMat(cv::Mat const& mat, const char* filename, const char* varName, bool bgr2rgb){
    /*!
         *  \author Philip G. Lee <rocketman768@gmail.com>
         *  Write \b mat into \b filename
         *  in uncompressed .mat format (Level 5 MATLAB) for Matlab.
         *  The variable name in matlab will be \b varName. If
         *  \b bgr2rgb is true and there are 3 channels, swaps 1st and 3rd
         *  channels in the output. This is needed because OpenCV matrices
         *  are bgr, while Matlab is rgb. This has been tested to work with
         *  3-channel single-precision floating point matrices, and I hope
         *  it works on other types/channels, but not exactly sure.
         *  Documentation at <http://www.mathworks.com/help/pdf_doc/matlab/matfile_format.pdf>
         */
    int textLen = 116;
    char* text;
    int subsysOffsetLen = 8;
    char* subsysOffset;
    int verLen = 2;
    char* ver;
    char flags;
    int bytes;
    int padBytes;
    int bytesPerElement;
    int i,j,k,k2;
    bool doBgrSwap;
    char mxClass;
    int32_t miClass;
    uchar const* rowPtr;
    uint32_t tmp32;
    float tmp;
    FILE* fp;
 
    // Matlab constants.
    const uint16_t MI = 0x4d49; // Contains "MI" in ascii.
    const int32_t miINT8 = 1;
    const int32_t miUINT8 = 2;
    const int32_t miINT16 = 3;
    const int32_t miUINT16 = 4;
    const int32_t miINT32 = 5;
    const int32_t miUINT32 = 6;
    const int32_t miSINGLE = 7;
    const int32_t miDOUBLE = 9;
    const int32_t miMATRIX = 14;
    const char mxDOUBLE_CLASS = 6;
    const char mxSINGLE_CLASS = 7;
    const char mxINT8_CLASS = 8;
    const char mxUINT8_CLASS = 9;
    const char mxINT16_CLASS = 10;
    const char mxUINT16_CLASS = 11;
    const char mxINT32_CLASS = 12;
    const char mxUINT32_CLASS = 13;
    const uint64_t zero = 0; // Used for padding.
 
    fp = fopen( filename, "wb" );
 
    if( fp == 0 )
        return;
 
    const int rows = mat.rows;
    const int cols = mat.cols;
    const int chans = mat.channels();
 
    doBgrSwap = (chans==3) && bgr2rgb;
 
    // I hope this mapping is right :-/
    switch( mat.depth() ){
    case CV_8U:
        mxClass = mxUINT8_CLASS;
        miClass = miUINT8;
        bytesPerElement = 1;
        break;
    case CV_8S:
        mxClass = mxINT8_CLASS;
        miClass = miINT8;
        bytesPerElement = 1;
        break;
    case CV_16U:
        mxClass = mxUINT16_CLASS;
        miClass = miUINT16;
        bytesPerElement = 2;
        break;
    case CV_16S:
        mxClass = mxINT16_CLASS;
        miClass = miINT16;
        bytesPerElement = 2;
        break;
    case CV_32S:
        mxClass = mxINT32_CLASS;
        miClass = miINT32;
        bytesPerElement = 4;
        break;
    case CV_32F:
        mxClass = mxSINGLE_CLASS;
        miClass = miSINGLE;
        bytesPerElement = 4;
        break;
    case CV_64F:
        mxClass = mxDOUBLE_CLASS;
        miClass = miDOUBLE;
        bytesPerElement = 8;
        break;
    default:
        return;
    }
 
    //==================Mat-file header (128 bytes, page 1-5)==================
    text = new char[textLen]; // Human-readable text.
    memset( text, ' ', textLen );
    text[textLen-1] = '\0';
    const char* t = "MATLAB 5.0 MAT-file, Platform: PCWIN";
    memcpy( text, t, strlen(t) );
 
    subsysOffset = new char[subsysOffsetLen]; // Zeros for us.
    memset( subsysOffset, 0x00, subsysOffsetLen );
    ver = new char[verLen];
    ver[0] = 0x00;
    ver[1] = 0x01;
 
    fwrite( text, 1, textLen, fp );
    fwrite( subsysOffset, 1, subsysOffsetLen, fp );
    fwrite( ver, 1, verLen, fp );
    // Endian indicator. MI will show up as "MI" on big-endian
    // systems and "IM" on little-endian systems.
    fwrite( &MI, 2, 1, fp );
    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
    //===================Data element tag (8 bytes, page 1-8)==================
    bytes = 16 + 24 + (8 + strlen(varName) + (8-(strlen(varName)%8))%8)
            + (8 + rows*cols*chans*bytesPerElement);
    fwrite( &miMATRIX, 4, 1, fp ); // Data type.
    fwrite( &bytes, 4, 1, fp); // Data size in bytes.
    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
    //====================Array flags (16 bytes, page 1-15)====================
    bytes = 8;
    fwrite( &miUINT32, 4, 1, fp );
    fwrite( &bytes, 4, 1, fp );
    flags = 0x00; // Complex, logical, and global flags all off.
 
    tmp32 = 0;
    tmp32 = (flags << 8 ) | (mxClass);
    fwrite( &tmp32, 4, 1, fp );
 
    fwrite( &zero, 4, 1, fp ); // Padding to 64-bit boundary.
    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
    //===============Dimensions subelement (24 bytes, page 1-17)===============
    bytes = 12;
    fwrite( &miINT32, 4, 1, fp );
    fwrite( &bytes, 4, 1, fp );
 
    fwrite( &rows, 4, 1, fp );
    fwrite( &cols, 4, 1, fp );
    fwrite( &chans, 4, 1, fp );
    fwrite( &zero, 4, 1, fp ); // Padding to 64-bit boundary.
    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
    //==Array name (8 + strlen(varName) + (8-(strlen(varName)%8))%8 bytes, page 1-17)==
    bytes = strlen(varName);
 
    fwrite( &miINT8, 4, 1, fp );
    fwrite( &bytes, 4, 1, fp );
    fwrite( varName, 1, bytes, fp );
 
    // Pad to nearest 64-bit boundary.
    padBytes = (8-(bytes%8))%8;
    fwrite( &zero, 1, padBytes, fp );
    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
    //====Matrix data (rows*cols*chans*bytesPerElement+8 bytes, page 1-20)=====
    bytes = rows*cols*chans*bytesPerElement;
    fwrite( &miClass, 4, 1, fp );
    fwrite( &bytes, 4, 1, fp );
 
    for( k = 0; k < chans; ++k )
    {
        if( doBgrSwap )
        {
            k2 = (k==0)? 2 : ((k==2)? 0 : 1);
        }
        else
            k2 = k;
 
        for( j = 0; j < cols; ++j )
        {
            for( i = 0; i < rows; ++i )
            {
                rowPtr = mat.data + mat.step*i;
                fwrite( rowPtr + (chans*j + k2)*bytesPerElement, bytesPerElement, 1, fp );
            }
        }
    }
 
    // Pad to 64-bit boundary.
    padBytes = (8-(bytes%8))%8;
    fwrite( &zero, 1, padBytes, fp );
    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
    fclose(fp);
    delete[] text;
    delete[] subsysOffset;
    delete[] ver;
}
 
 
 
 
 
}
 

Rev 36	Rev 42
1	`#include "cvtools.h"`	1	`#include "cvtools.h"`
2		2
3	`#ifdef _WIN32`	3	`#ifdef _WIN32`
4	`#include <cstdint>`	4	`#include <cstdint>`
5	`#endif`	5	`#endif`
6		6
7	`#include <stdio.h>`	7	`#include <stdio.h>`
8		8
9	`namespace cvtools{`	9	`namespace cvtools{`
10		10
-		11	`// Convert a 3xN matrix to a vector of Point3fs.`
-		12	`void matToPoints3f(const cv::Mat &mat, std::vector<cv::Point3f> &points){`
-		13
-		14	`unsigned int nPoints = mat.cols;`
-		15	`points.resize(nPoints);`
-		16
-		17	`for(unsigned int i=0; i<nPoints; i++)`
-		18	`points[i] = cv::Point3f(mat.at<float>(0, i), mat.at<float>(1, i), mat.at<float>(2, i));`
-		19	`}`
-		20
-		21	`// Convert a (Dim+1)xN matrix of homogenous points to a DimxN matrix of points in non-homogenous coordinates.`
-		22	`void convertMatFromHomogeneous(cv::Mat &src, cv::Mat &dst){`
-		23	`unsigned int N = src.cols;`
-		24	`unsigned int Dim = src.rows-1;`
-		25	`dst.create(Dim, N, src.type());`
-		26	`for(unsigned int i=0; i<N; i++){`
-		27	`for(unsigned int j=0; j<Dim; j++)`
-		28	`dst.at<float>(j,i) = src.at<float>(j,i)/src.at<float>(Dim,i);`
-		29	`}`
-		30
-		31	`}`
-		32
11	`// Function to solve the hand-eye (or eye-in-hand) calibration problem.`	33	`// Function to solve the hand-eye (or eye-in-hand) calibration problem.`
12	`// Finds [Omega \| tau], to minimize \|\|[R_mark \| t_mark][Omega \| tau] - [Omega \| tau][R \| t]\|\|^2`	34	`// Finds [Omega \| tau], to minimize \|\|[R_mark \| t_mark][Omega \| tau] - [Omega \| tau][R \| t]\|\|^2`
13	`// Algorithm according to Tsai, Lenz, A new technique for fully autonomous and efficient 3d robotics hand-eye calibration`	35	`// Algorithm according to Tsai, Lenz, A new technique for fully autonomous and efficient 3d robotics hand-eye calibration`
14	`// DTU, 2014, Jakob Wilm`	36	`// DTU, 2014, Jakob Wilm`
15	`void handEyeCalibrationTsai(const std::vector<cv::Matx33f> R, const std::vector<cv::Vec3f> t, const std::vector<cv::Matx33f> R_mark, const std::vector<cv::Vec3f> t_mark, cv::Matx33f &Omega, cv::Vec3f &tau){`	37	`void handEyeCalibrationTsai(const std::vector<cv::Matx33f> R, const std::vector<cv::Vec3f> t, const std::vector<cv::Matx33f> R_mark, const std::vector<cv::Vec3f> t_mark, cv::Matx33f &Omega, cv::Vec3f &tau){`
16	`// NOT DEBUGGED!`	-
-		38
17	`int N = R.size();`	39	`int N = R.size();`
18	`assert(N == R_mark.size());`	40	`assert(N == R_mark.size());`
19	`assert(N == t.size());`	41	`assert(N == t.size());`
20	`assert(N == t_mark.size());`	42	`assert(N == t_mark.size());`
21		43
22	`// construct equations for rotation`	44	`// construct equations for rotation`
23	`cv::Mat A(3*N, 3, CV_32F);`	45	`cv::Mat A(3*N, 3, CV_32F);`
24	`cv::Mat b(3*N, 1, CV_32F);`	46	`cv::Mat b(3*N, 1, CV_32F);`
25	`for(int i=0; i<N; i++){`	47	`for(int i=0; i<N; i++){`
26	`// angle axis representations`	48	`// angle axis representations`
27	`cv::Vec3f rot;`	49	`cv::Vec3f rot;`
28	`cv::Vec3f rot_mark;`	50	`cv::Vec3f rot_mark;`
29	`cv::Rodrigues(R[i], rot);`	51	`cv::Rodrigues(R[i], rot);`
30	`cv::Rodrigues(R_mark[i], rot_mark);`	52	`cv::Rodrigues(R_mark[i], rot_mark);`
31		53
32	`cv::Vec3f P = 2.0sin(cv::norm(rot)/2.0)cv::normalize(rot);`	54	`cv::Vec3f P = 2.0sin(cv::norm(rot)/2.0)cv::normalize(rot);`
33	`//std::cout << "P: " << std::endl << P << std::endl;`	55	`//std::cout << "P: " << std::endl << P << std::endl;`
34	`cv::Vec3f P_mark = 2.0sin(cv::norm(rot_mark)/2.0)cv::normalize(rot_mark);`	56	`cv::Vec3f P_mark = 2.0sin(cv::norm(rot_mark)/2.0)cv::normalize(rot_mark);`
35	`//std::cout << "P_mark: " << std::endl << P_mark << std::endl;`	57	`//std::cout << "P_mark: " << std::endl << P_mark << std::endl;`
36	`cv::Vec3f sum = P+P_mark;`	58	`cv::Vec3f sum = P+P_mark;`
37	`cv::Mat crossProduct = (cv::Mat_<float>(3,3) << 0.0, -sum(2), sum(1), sum(2), 0.0, -sum(0), -sum(1), sum(0), 0.0);`	59	`cv::Mat crossProduct = (cv::Mat_<float>(3,3) << 0.0, -sum(2), sum(1), sum(2), 0.0, -sum(0), -sum(1), sum(0), 0.0);`
38	`//std::cout << "crossProduct: " << std::endl << crossProduct << std::endl;`	60	`//std::cout << "crossProduct: " << std::endl << crossProduct << std::endl;`
39	`crossProduct.copyTo(A.rowRange(i3, i3+3));`	61	`crossProduct.copyTo(A.rowRange(i3, i3+3));`
40		62
41	`cv::Mat(P-P_mark).copyTo(b.rowRange(i3, i3+3));`	63	`cv::Mat(P-P_mark).copyTo(b.rowRange(i3, i3+3));`
42	`}`	64	`}`
43		65
44	`// solve for rotation`	66	`// solve for rotation`
45	`cv::Vec3f P_prime;`	67	`cv::Vec3f P_prime;`
46	`cv::solve(A, b, P_prime, cv::DECOMP_SVD);`	68	`cv::solve(A, b, P_prime, cv::DECOMP_SVD);`
47	`cv::Vec3f P = 2.0P_prime/(cv::sqrt(1.0 + cv::norm(P_prime)cv::norm(P_prime)));`	69	`cv::Vec3f P = 2.0P_prime/(cv::sqrt(1.0 + cv::norm(P_prime)cv::norm(P_prime)));`
48	`float nP = cv::norm(P);`	70	`float nP = cv::norm(P);`
49	`cv::Mat crossProduct = (cv::Mat_<float>(3,3) << 0.0, -P(2), P(1), P(2), 0.0, -P(0), -P(1), P(0), 0.0);`	71	`cv::Mat crossProduct = (cv::Mat_<float>(3,3) << 0.0, -P(2), P(1), P(2), 0.0, -P(0), -P(1), P(0), 0.0);`
50	`cv::Mat OmegaMat = (1.0-nPnP/2.0)cv::Mat::eye(3,3,CV_32F) + 0.5(cv::Mat(P)cv::Mat(P).t() + cv::sqrt(4.0 - nPnP)crossProduct);`	72	`cv::Mat OmegaMat = (1.0-nPnP/2.0)cv::Mat::eye(3,3,CV_32F) + 0.5(cv::Mat(P)cv::Mat(P).t() + cv::sqrt(4.0 - nPnP)crossProduct);`
51	`Omega = cv::Matx33f(OmegaMat);`	73	`Omega = cv::Matx33f(OmegaMat);`
52		74
53	`// construct equations for translation`	75	`// construct equations for translation`
54	`A.setTo(0.0);`	76	`A.setTo(0.0);`
55	`b.setTo(0.0);`	77	`b.setTo(0.0);`
56	`for(int i=0; i<N; i++){`	78	`for(int i=0; i<N; i++){`
57		79
58	`cv::Mat diff = cv::Mat(R_mark[i]) - cv::Mat::eye(3, 3, CV_32F);`	80	`cv::Mat diff = cv::Mat(R_mark[i]) - cv::Mat::eye(3, 3, CV_32F);`
59	`diff.copyTo(A.rowRange(i3, i3+3));`	81	`diff.copyTo(A.rowRange(i3, i3+3));`
60		82
61	`cv::Mat diff_mark = cv::Mat(Omega*t[i] - t_mark[i]);`	83	`cv::Mat diff_mark = cv::Mat(Omega*t[i] - t_mark[i]);`
62	`diff_mark.copyTo(b.rowRange(i3, i3+3));`	84	`diff_mark.copyTo(b.rowRange(i3, i3+3));`
63	`}`	85	`}`
64		86
65	`// solve for translation`	87	`// solve for translation`
66	`cv::solve(A, b, tau, cv::DECOMP_SVD);`	88	`cv::solve(A, b, tau, cv::DECOMP_SVD);`
67	`}`	89	`}`
68		90
69	`// Function to fit two sets of corresponding pose data.`	91	`// Function to fit two sets of corresponding pose data.`
70	`// Finds [Omega \| tau], to minimize \|\|[R_mark \| t_mark] - [Omega \| tau][R \| t]\|\|^2`	92	`// Finds [Omega \| tau], to minimize \|\|[R_mark \| t_mark] - [Omega \| tau][R \| t]\|\|^2`
71	`// Algorithm and notation according to Mili Shah, Comparing two sets of corresponding six degree of freedom data, CVIU 2011.`	93	`// Algorithm and notation according to Mili Shah, Comparing two sets of corresponding six degree of freedom data, CVIU 2011.`
72	`// DTU, 2013, Oline V. Olesen, Jakob Wilm`	94	`// DTU, 2013, Oline V. Olesen, Jakob Wilm`
73	`void fitSixDofData(const std::vector<cv::Matx33f> R, const std::vector<cv::Vec3f> t, const std::vector<cv::Matx33f> R_mark, const std::vector<cv::Vec3f> t_mark, cv::Matx33f &Omega, cv::Vec3f &tau){`	95	`void fitSixDofData(const std::vector<cv::Matx33f> R, const std::vector<cv::Vec3f> t, const std::vector<cv::Matx33f> R_mark, const std::vector<cv::Vec3f> t_mark, cv::Matx33f &Omega, cv::Vec3f &tau){`
74		96
75	`int N = R.size();`	97	`int N = R.size();`
76	`assert(N == R_mark.size());`	98	`assert(N == R_mark.size());`
77	`assert(N == t.size());`	99	`assert(N == t.size());`
78	`assert(N == t_mark.size());`	100	`assert(N == t_mark.size());`
79		101
80	`// Mean translations`	102	`// Mean translations`
81	`cv::Vec3f t_mean;`	103	`cv::Vec3f t_mean;`
82	`cv::Vec3f t_mark_mean;`	104	`cv::Vec3f t_mark_mean;`
83	`for(int i=0; i<N; i++){`	105	`for(int i=0; i<N; i++){`
84	`t_mean += 1.0/N * t[i];`	106	`t_mean += 1.0/N * t[i];`
85	`t_mark_mean += 1.0/N * t_mark[i];`	107	`t_mark_mean += 1.0/N * t_mark[i];`
86	`}`	108	`}`
87		109
88	`// Data with mean adjusted translations`	110	`// Data with mean adjusted translations`
89	`cv::Mat X_bar(3, 4*N, CV_32F);`	111	`cv::Mat X_bar(3, 4*N, CV_32F);`
90	`cv::Mat X_mark_bar(3, 4*N, CV_32F);`	112	`cv::Mat X_mark_bar(3, 4*N, CV_32F);`
91	`for(int i=0; i<N; i++){`	113	`for(int i=0; i<N; i++){`
92	`cv::Mat(R[i]).copyTo(X_bar.colRange(i4,i4+3));`	114	`cv::Mat(R[i]).copyTo(X_bar.colRange(i4,i4+3));`
93	`cv::Mat(t[i] - t_mean).copyTo(X_bar.col(i*4+3));`	115	`cv::Mat(t[i] - t_mean).copyTo(X_bar.col(i*4+3));`
94	`cv::Mat(R_mark[i]).copyTo(X_mark_bar.colRange(i4,i4+3));`	116	`cv::Mat(R_mark[i]).copyTo(X_mark_bar.colRange(i4,i4+3));`
95	`cv::Mat(t_mark[i] - t_mark_mean).copyTo(X_mark_bar.col(i*4+3));`	117	`cv::Mat(t_mark[i] - t_mark_mean).copyTo(X_mark_bar.col(i*4+3));`
96	`}`	118	`}`
97	`//std::cout << X_bar << std::endl;`	119	`//std::cout << X_bar << std::endl;`
98	`// SVD`	120	`// SVD`
99	`cv::Mat W, U, VT;`	121	`cv::Mat W, U, VT;`
100	`cv::SVDecomp(X_bar*X_mark_bar.t(), W, U, VT);`	122	`cv::SVDecomp(X_bar*X_mark_bar.t(), W, U, VT);`
101		123
102	`cv::Matx33f D = cv::Matx33f::eye();`	124	`cv::Matx33f D = cv::Matx33f::eye();`
103	`if(cv::determinant(VT*U) < 0)`	125	`if(cv::determinant(VT*U) < 0)`
104	`D(3,3) = -1;`	126	`D(3,3) = -1;`
105		127
106	`// Best rotation`	128	`// Best rotation`
107	`Omega = cv::Matx33f(cv::Mat(VT.t()))Dcv::Matx33f(cv::Mat(U.t()));`	129	`Omega = cv::Matx33f(cv::Mat(VT.t()))Dcv::Matx33f(cv::Mat(U.t()));`
108		130
109	`// Best translation`	131	`// Best translation`
110	`tau = t_mark_mean - Omega*t_mean;`	132	`tau = t_mark_mean - Omega*t_mean;`
111		133
112	`}`	134	`}`
113		135
114	`// Forward distortion of points. The inverse of the undistortion in cv::initUndistortRectifyMap().`	136	`// Forward distortion of points. The inverse of the undistortion in cv::initUndistortRectifyMap().`
115	`// Inspired by Pascal Thomet, http://code.opencv.org/issues/1387#note-11`	137	`// Inspired by Pascal Thomet, http://code.opencv.org/issues/1387#note-11`
116	`// Convention for distortion parameters: http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html`	138	`// Convention for distortion parameters: http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html`
117	`void initDistortMap(const cv::Matx33f cameraMatrix, const cv::Vec<float, 5> distCoeffs, const cv::Size size, cv::Mat &map1, cv::Mat &map2){`	139	`void initDistortMap(const cv::Matx33f cameraMatrix, const cv::Vec<float, 5> distCoeffs, const cv::Size size, cv::Mat &map1, cv::Mat &map2){`
118		140
119	`float fx = cameraMatrix(0,0);`	141	`float fx = cameraMatrix(0,0);`
120	`float fy = cameraMatrix(1,1);`	142	`float fy = cameraMatrix(1,1);`
121	`float ux = cameraMatrix(0,2);`	143	`float ux = cameraMatrix(0,2);`
122	`float uy = cameraMatrix(1,2);`	144	`float uy = cameraMatrix(1,2);`
123		145
124	`float k1 = distCoeffs[0];`	146	`float k1 = distCoeffs[0];`
125	`float k2 = distCoeffs[1];`	147	`float k2 = distCoeffs[1];`
126	`float p1 = distCoeffs[2];`	148	`float p1 = distCoeffs[2];`
127	`float p2 = distCoeffs[3];`	149	`float p2 = distCoeffs[3];`
128	`float k3 = distCoeffs[4];`	150	`float k3 = distCoeffs[4];`
129		151
130	`map1.create(size, CV_32F);`	152	`map1.create(size, CV_32F);`
131	`map2.create(size, CV_32F);`	153	`map2.create(size, CV_32F);`
132		154
133	`for(int col = 0; col < size.width; col++){`	155	`for(int col = 0; col < size.width; col++){`
134	`for(int row = 0; row < size.height; row++){`	156	`for(int row = 0; row < size.height; row++){`
135		157
136	`// move origo to principal point and convert using focal length`	158	`// move origo to principal point and convert using focal length`
137	`float x = (col-ux)/fx;`	159	`float x = (col-ux)/fx;`
138	`float y = (row-uy)/fy;`	160	`float y = (row-uy)/fy;`
139		161
140	`float xCorrected, yCorrected;`	162	`float xCorrected, yCorrected;`
141		163
142	`//Step 1 : correct distortion`	164	`//Step 1 : correct distortion`
143	`float r2 = xx + yy;`	165	`float r2 = xx + yy;`
144	`//radial`	166	`//radial`
145	`xCorrected = x * (1. + k1r2 + k2r2r2 + k3r2r2r2);`	167	`xCorrected = x * (1. + k1r2 + k2r2r2 + k3r2r2r2);`
146	`yCorrected = y * (1. + k1r2 + k2r2r2 + k3r2r2r2);`	168	`yCorrected = y * (1. + k1r2 + k2r2r2 + k3r2r2r2);`
147	`//tangential`	169	`//tangential`
148	`xCorrected = xCorrected + (2.p1xy + p2(r2+2.xx));`	170	`xCorrected = xCorrected + (2.p1xy + p2(r2+2.xx));`
149	`yCorrected = yCorrected + (p1(r2+2.yy) + 2.p2xy);`	171	`yCorrected = yCorrected + (p1(r2+2.yy) + 2.p2xy);`
150		172
151	`//convert back to pixel coordinates`	173	`//convert back to pixel coordinates`
152	`float col_displaced = xCorrected * fx + ux;`	174	`float col_displaced = xCorrected * fx + ux;`
153	`float row_displaced = yCorrected * fy + uy;`	175	`float row_displaced = yCorrected * fy + uy;`
154		176
155	`// correct the vector in the opposite direction`	177	`// correct the vector in the opposite direction`
156	`map1.at<float>(row,col) = col+(col-col_displaced);`	178	`map1.at<float>(row,col) = col+(col-col_displaced);`
157	`map2.at<float>(row,col) = row +(row-row_displaced);`	179	`map2.at<float>(row,col) = row +(row-row_displaced);`
158	`}`	180	`}`
159	`}`	181	`}`
160	`}`	182	`}`
161		183
162	`// Downsample a texture which was created in virtual column/row space for a diamond pixel array projector`	184	`// Downsample a texture which was created in virtual column/row space for a diamond pixel array projector`
163	`cv::Mat diamondDownsample(cv::Mat &pattern){`	185	`cv::Mat diamondDownsample(cv::Mat &pattern){`
164		186
165	`cv::Mat pattern_diamond(pattern.rows,pattern.cols/2,CV_8UC3);`	187	`cv::Mat pattern_diamond(pattern.rows,pattern.cols/2,CV_8UC3);`
166		188
167	`for(unsigned int col = 0; col < pattern_diamond.cols; col++){`	189	`for(unsigned int col = 0; col < pattern_diamond.cols; col++){`
168	`for(unsigned int row = 0; row < pattern_diamond.rows; row++){`	190	`for(unsigned int row = 0; row < pattern_diamond.rows; row++){`
169		191
170	`pattern_diamond.at<cv::Vec3b>(row,col)=pattern.at<cv::Vec3b>(row,col*2+row%2);`	192	`pattern_diamond.at<cv::Vec3b>(row,col)=pattern.at<cv::Vec3b>(row,col*2+row%2);`
171	`}`	193	`}`
172	`}`	194	`}`
173		195
174	`return pattern_diamond;`	196	`return pattern_diamond;`
175		197
176	`}`	198	`}`
177		199
178		200
179	`void mouseCallback(int evt, int x, int y, int flags, void* param){`	201	`void mouseCallback(int evt, int x, int y, int flags, void* param){`
180	`cv::Mat im = (cv::Mat) param;`	202	`cv::Mat im = (cv::Mat) param;`
181	`if (evt == CV_EVENT_LBUTTONDOWN) {`	203	`if (evt == CV_EVENT_LBUTTONDOWN) {`
182	`if(im->type() == CV_8UC3){`	204	`if(im->type() == CV_8UC3){`
183	`printf("%d %d: %d, %d, %d\n",`	205	`printf("%d %d: %d, %d, %d\n",`
184	`x, y,`	206	`x, y,`
185	`(int)(*im).at<cv::Vec3b>(y, x)[0],`	207	`(int)(*im).at<cv::Vec3b>(y, x)[0],`
186	`(int)(*im).at<cv::Vec3b>(y, x)[1],`	208	`(int)(*im).at<cv::Vec3b>(y, x)[1],`
187	`(int)(*im).at<cv::Vec3b>(y, x)[2]);`	209	`(int)(*im).at<cv::Vec3b>(y, x)[2]);`
188	`} else if (im->type() == CV_32F) {`	210	`} else if (im->type() == CV_32F) {`
189	`printf("%d %d: %f\n",`	211	`printf("%d %d: %f\n",`
190	`x, y,`	212	`x, y,`
191	`im->at<float>(y, x));`	213	`im->at<float>(y, x));`
192	`}`	214	`}`
193	`}`	215	`}`
194	`}`	216	`}`
195		217
196	`void imshow(const char *windowName, cv::Mat im, unsigned int x, unsigned int y){`	218	`void imshow(const char *windowName, cv::Mat im, unsigned int x, unsigned int y){`
197		219
198	`// Imshow`	220	`// Imshow`
199	`if(!cvGetWindowHandle(windowName)){`	221	`if(!cvGetWindowHandle(windowName)){`
200	`int windowFlags = CV_GUI_EXPANDED \| CV_WINDOW_KEEPRATIO;`	222	`int windowFlags = CV_GUI_EXPANDED \| CV_WINDOW_KEEPRATIO;`
201	`cv::namedWindow(windowName, windowFlags);`	223	`cv::namedWindow(windowName, windowFlags);`
202	`cv::moveWindow(windowName, x, y);`	224	`cv::moveWindow(windowName, x, y);`
203	`}`	225	`}`
204	`cv::imshow(windowName, im);`	226	`cv::imshow(windowName, im);`
205	`}`	227	`}`
206		228
207	`void imagesc(const char *windowName, cv::Mat im){`	229	`void imagesc(const char *windowName, cv::Mat im){`
208		230
209	`// Imshow with scaled image`	231	`// Imshow with scaled image`
210		232
211		233
212	`}`	234	`}`
213		235
214	`cv::Mat histimage(cv::Mat histogram){`	236	`cv::Mat histimage(cv::Mat histogram){`
215		237
216	`cv::Mat histImage(512, 640, CV_8UC3, cv::Scalar(0));`	238	`cv::Mat histImage(512, 640, CV_8UC3, cv::Scalar(0));`
217		239
218	`// Normalize the result to [ 2, histImage.rows-2 ]`	240	`// Normalize the result to [ 2, histImage.rows-2 ]`
219	`cv::normalize(histogram, histogram, 2, histImage.rows-2, cv::NORM_MINMAX, -1, cv::Mat());`	241	`cv::normalize(histogram, histogram, 2, histImage.rows-2, cv::NORM_MINMAX, -1, cv::Mat());`
220		242
221	`float bin_w = (float)histImage.cols/(float)histogram.rows;`	243	`float bin_w = (float)histImage.cols/(float)histogram.rows;`
222		244
223	`// Draw main histogram`	245	`// Draw main histogram`
224	`for(int i = 1; i < histogram.rows-10; i++){`	246	`for(int i = 1; i < histogram.rows-10; i++){`
225	`cv::line(histImage, cv::Point( bin_w*(i-1), histImage.rows - cvRound(histogram.at<float>(i-1)) ),`	247	`cv::line(histImage, cv::Point( bin_w*(i-1), histImage.rows - cvRound(histogram.at<float>(i-1)) ),`
226	`cv::Point( bin_w*(i), histImage.rows - cvRound(histogram.at<float>(i)) ),`	248	`cv::Point( bin_w*(i), histImage.rows - cvRound(histogram.at<float>(i)) ),`
227	`cv::Scalar(255, 255, 255), 2, 4);`	249	`cv::Scalar(255, 255, 255), 2, 4);`
228	`}`	250	`}`
229		251
230	`// Draw red max`	252	`// Draw red max`
231	`for(int i = histogram.rows-10; i < histogram.rows; i++){`	253	`for(int i = histogram.rows-10; i < histogram.rows; i++){`
232	`cv::line(histImage, cv::Point( bin_w*(i-1), histImage.rows - cvRound(histogram.at<float>(i-1)) ),`	254	`cv::line(histImage, cv::Point( bin_w*(i-1), histImage.rows - cvRound(histogram.at<float>(i-1)) ),`
233	`cv::Point( bin_w*(i), histImage.rows - cvRound(histogram.at<float>(i)) ),`	255	`cv::Point( bin_w*(i), histImage.rows - cvRound(histogram.at<float>(i)) ),`
234	`cv::Scalar(0, 0, 255), 2, 4);`	256	`cv::Scalar(0, 0, 255), 2, 4);`
235	`}`	257	`}`
236		258
237	`return histImage;`	259	`return histImage;`
238	`}`	260	`}`
239		261
240	`void hist(const char *windowName, cv::Mat histogram, unsigned int x, unsigned int y){`	262	`void hist(const char *windowName, cv::Mat histogram, unsigned int x, unsigned int y){`
241		263
242	`// Display`	264	`// Display`
243	`imshow(windowName, histimage(histogram), x, y);`	265	`imshow(windowName, histimage(histogram), x, y);`
244	`cv::Point(1,2);`	266	`cv::Point(1,2);`
245	`}`	267	`}`
246		268
247		269
248	`void writeMat(cv::Mat const& mat, const char* filename, const char* varName, bool bgr2rgb){`	270	`void writeMat(cv::Mat const& mat, const char* filename, const char* varName, bool bgr2rgb){`
249	`/*!`	271	`/*!`
250	`* \author Philip G. Lee <rocketman768@gmail.com>`	272	`* \author Philip G. Lee <rocketman768@gmail.com>`
251	`* Write \b mat into \b filename`	273	`* Write \b mat into \b filename`
252	`* in uncompressed .mat format (Level 5 MATLAB) for Matlab.`	274	`* in uncompressed .mat format (Level 5 MATLAB) for Matlab.`
253	`* The variable name in matlab will be \b varName. If`	275	`* The variable name in matlab will be \b varName. If`
254	`* \b bgr2rgb is true and there are 3 channels, swaps 1st and 3rd`	276	`* \b bgr2rgb is true and there are 3 channels, swaps 1st and 3rd`
255	`* channels in the output. This is needed because OpenCV matrices`	277	`* channels in the output. This is needed because OpenCV matrices`
256	`* are bgr, while Matlab is rgb. This has been tested to work with`	278	`* are bgr, while Matlab is rgb. This has been tested to work with`
257	`* 3-channel single-precision floating point matrices, and I hope`	279	`* 3-channel single-precision floating point matrices, and I hope`
258	`* it works on other types/channels, but not exactly sure.`	280	`* it works on other types/channels, but not exactly sure.`
259	`* Documentation at <http://www.mathworks.com/help/pdf_doc/matlab/matfile_format.pdf>`	281	`* Documentation at <http://www.mathworks.com/help/pdf_doc/matlab/matfile_format.pdf>`
260	`*/`	282	`*/`
261	`int textLen = 116;`	283	`int textLen = 116;`
262	`char* text;`	284	`char* text;`
263	`int subsysOffsetLen = 8;`	285	`int subsysOffsetLen = 8;`
264	`char* subsysOffset;`	286	`char* subsysOffset;`
265	`int verLen = 2;`	287	`int verLen = 2;`
266	`char* ver;`	288	`char* ver;`
267	`char flags;`	289	`char flags;`
268	`int bytes;`	290	`int bytes;`
269	`int padBytes;`	291	`int padBytes;`
270	`int bytesPerElement;`	292	`int bytesPerElement;`
271	`int i,j,k,k2;`	293	`int i,j,k,k2;`
272	`bool doBgrSwap;`	294	`bool doBgrSwap;`
273	`char mxClass;`	295	`char mxClass;`
274	`int32_t miClass;`	296	`int32_t miClass;`
275	`uchar const* rowPtr;`	297	`uchar const* rowPtr;`
276	`uint32_t tmp32;`	298	`uint32_t tmp32;`
277	`float tmp;`	299	`float tmp;`
278	`FILE* fp;`	300	`FILE* fp;`
279		301
280	`// Matlab constants.`	302	`// Matlab constants.`
281	`const uint16_t MI = 0x4d49; // Contains "MI" in ascii.`	303	`const uint16_t MI = 0x4d49; // Contains "MI" in ascii.`
282	`const int32_t miINT8 = 1;`	304	`const int32_t miINT8 = 1;`
283	`const int32_t miUINT8 = 2;`	305	`const int32_t miUINT8 = 2;`
284	`const int32_t miINT16 = 3;`	306	`const int32_t miINT16 = 3;`
285	`const int32_t miUINT16 = 4;`	307	`const int32_t miUINT16 = 4;`
286	`const int32_t miINT32 = 5;`	308	`const int32_t miINT32 = 5;`
287	`const int32_t miUINT32 = 6;`	309	`const int32_t miUINT32 = 6;`
288	`const int32_t miSINGLE = 7;`	310	`const int32_t miSINGLE = 7;`
289	`const int32_t miDOUBLE = 9;`	311	`const int32_t miDOUBLE = 9;`
290	`const int32_t miMATRIX = 14;`	312	`const int32_t miMATRIX = 14;`
291	`const char mxDOUBLE_CLASS = 6;`	313	`const char mxDOUBLE_CLASS = 6;`
292	`const char mxSINGLE_CLASS = 7;`	314	`const char mxSINGLE_CLASS = 7;`
293	`const char mxINT8_CLASS = 8;`	315	`const char mxINT8_CLASS = 8;`
294	`const char mxUINT8_CLASS = 9;`	316	`const char mxUINT8_CLASS = 9;`
295	`const char mxINT16_CLASS = 10;`	317	`const char mxINT16_CLASS = 10;`
296	`const char mxUINT16_CLASS = 11;`	318	`const char mxUINT16_CLASS = 11;`
297	`const char mxINT32_CLASS = 12;`	319	`const char mxINT32_CLASS = 12;`
298	`const char mxUINT32_CLASS = 13;`	320	`const char mxUINT32_CLASS = 13;`
299	`const uint64_t zero = 0; // Used for padding.`	321	`const uint64_t zero = 0; // Used for padding.`
300		322
301	`fp = fopen( filename, "wb" );`	323	`fp = fopen( filename, "wb" );`
302		324
303	`if( fp == 0 )`	325	`if( fp == 0 )`
304	`return;`	326	`return;`
305		327
306	`const int rows = mat.rows;`	328	`const int rows = mat.rows;`
307	`const int cols = mat.cols;`	329	`const int cols = mat.cols;`
308	`const int chans = mat.channels();`	330	`const int chans = mat.channels();`
309		331
310	`doBgrSwap = (chans==3) && bgr2rgb;`	332	`doBgrSwap = (chans==3) && bgr2rgb;`
311		333
312	`// I hope this mapping is right :-/`	334	`// I hope this mapping is right :-/`
313	`switch( mat.depth() ){`	335	`switch( mat.depth() ){`
314	`case CV_8U:`	336	`case CV_8U:`
315	`mxClass = mxUINT8_CLASS;`	337	`mxClass = mxUINT8_CLASS;`
316	`miClass = miUINT8;`	338	`miClass = miUINT8;`
317	`bytesPerElement = 1;`	339	`bytesPerElement = 1;`
318	`break;`	340	`break;`
319	`case CV_8S:`	341	`case CV_8S:`
320	`mxClass = mxINT8_CLASS;`	342	`mxClass = mxINT8_CLASS;`
321	`miClass = miINT8;`	343	`miClass = miINT8;`
322	`bytesPerElement = 1;`	344	`bytesPerElement = 1;`
323	`break;`	345	`break;`
324	`case CV_16U:`	346	`case CV_16U:`
325	`mxClass = mxUINT16_CLASS;`	347	`mxClass = mxUINT16_CLASS;`
326	`miClass = miUINT16;`	348	`miClass = miUINT16;`
327	`bytesPerElement = 2;`	349	`bytesPerElement = 2;`
328	`break;`	350	`break;`
329	`case CV_16S:`	351	`case CV_16S:`
330	`mxClass = mxINT16_CLASS;`	352	`mxClass = mxINT16_CLASS;`
331	`miClass = miINT16;`	353	`miClass = miINT16;`
332	`bytesPerElement = 2;`	354	`bytesPerElement = 2;`
333	`break;`	355	`break;`
334	`case CV_32S:`	356	`case CV_32S:`
335	`mxClass = mxINT32_CLASS;`	357	`mxClass = mxINT32_CLASS;`
336	`miClass = miINT32;`	358	`miClass = miINT32;`
337	`bytesPerElement = 4;`	359	`bytesPerElement = 4;`
338	`break;`	360	`break;`
339	`case CV_32F:`	361	`case CV_32F:`
340	`mxClass = mxSINGLE_CLASS;`	362	`mxClass = mxSINGLE_CLASS;`
341	`miClass = miSINGLE;`	363	`miClass = miSINGLE;`
342	`bytesPerElement = 4;`	364	`bytesPerElement = 4;`
343	`break;`	365	`break;`
344	`case CV_64F:`	366	`case CV_64F:`
345	`mxClass = mxDOUBLE_CLASS;`	367	`mxClass = mxDOUBLE_CLASS;`
346	`miClass = miDOUBLE;`	368	`miClass = miDOUBLE;`
347	`bytesPerElement = 8;`	369	`bytesPerElement = 8;`
348	`break;`	370	`break;`
349	`default:`	371	`default:`
350	`return;`	372	`return;`
351	`}`	373	`}`
352		374
353	`//==================Mat-file header (128 bytes, page 1-5)==================`	375	`//==================Mat-file header (128 bytes, page 1-5)==================`
354	`text = new char[textLen]; // Human-readable text.`	376	`text = new char[textLen]; // Human-readable text.`
355	`memset( text, ' ', textLen );`	377	`memset( text, ' ', textLen );`
356	`text[textLen-1] = '\0';`	378	`text[textLen-1] = '\0';`
357	`const char* t = "MATLAB 5.0 MAT-file, Platform: PCWIN";`	379	`const char* t = "MATLAB 5.0 MAT-file, Platform: PCWIN";`
358	`memcpy( text, t, strlen(t) );`	380	`memcpy( text, t, strlen(t) );`
359		381
360	`subsysOffset = new char[subsysOffsetLen]; // Zeros for us.`	382	`subsysOffset = new char[subsysOffsetLen]; // Zeros for us.`
361	`memset( subsysOffset, 0x00, subsysOffsetLen );`	383	`memset( subsysOffset, 0x00, subsysOffsetLen );`
362	`ver = new char[verLen];`	384	`ver = new char[verLen];`
363	`ver[0] = 0x00;`	385	`ver[0] = 0x00;`
364	`ver[1] = 0x01;`	386	`ver[1] = 0x01;`
365		387
366	`fwrite( text, 1, textLen, fp );`	388	`fwrite( text, 1, textLen, fp );`
367	`fwrite( subsysOffset, 1, subsysOffsetLen, fp );`	389	`fwrite( subsysOffset, 1, subsysOffsetLen, fp );`
368	`fwrite( ver, 1, verLen, fp );`	390	`fwrite( ver, 1, verLen, fp );`
369	`// Endian indicator. MI will show up as "MI" on big-endian`	391	`// Endian indicator. MI will show up as "MI" on big-endian`
370	`// systems and "IM" on little-endian systems.`	392	`// systems and "IM" on little-endian systems.`
371	`fwrite( &MI, 2, 1, fp );`	393	`fwrite( &MI, 2, 1, fp );`
372	`//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++`	394	`//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++`
373		395
374	`//===================Data element tag (8 bytes, page 1-8)==================`	396	`//===================Data element tag (8 bytes, page 1-8)==================`
375	`bytes = 16 + 24 + (8 + strlen(varName) + (8-(strlen(varName)%8))%8)`	397	`bytes = 16 + 24 + (8 + strlen(varName) + (8-(strlen(varName)%8))%8)`
376	`+ (8 + rowscolschans*bytesPerElement);`	398	`+ (8 + rowscolschans*bytesPerElement);`
377	`fwrite( &miMATRIX, 4, 1, fp ); // Data type.`	399	`fwrite( &miMATRIX, 4, 1, fp ); // Data type.`
378	`fwrite( &bytes, 4, 1, fp); // Data size in bytes.`	400	`fwrite( &bytes, 4, 1, fp); // Data size in bytes.`
379	`//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++`	401	`//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++`
380		402
381	`//====================Array flags (16 bytes, page 1-15)====================`	403	`//====================Array flags (16 bytes, page 1-15)====================`
382	`bytes = 8;`	404	`bytes = 8;`
383	`fwrite( &miUINT32, 4, 1, fp );`	405	`fwrite( &miUINT32, 4, 1, fp );`
384	`fwrite( &bytes, 4, 1, fp );`	406	`fwrite( &bytes, 4, 1, fp );`
385	`flags = 0x00; // Complex, logical, and global flags all off.`	407	`flags = 0x00; // Complex, logical, and global flags all off.`
386		408
387	`tmp32 = 0;`	409	`tmp32 = 0;`
388	`tmp32 = (flags << 8 ) \| (mxClass);`	410	`tmp32 = (flags << 8 ) \| (mxClass);`
389	`fwrite( &tmp32, 4, 1, fp );`	411	`fwrite( &tmp32, 4, 1, fp );`
390		412
391	`fwrite( &zero, 4, 1, fp ); // Padding to 64-bit boundary.`	413	`fwrite( &zero, 4, 1, fp ); // Padding to 64-bit boundary.`
392	`//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++`	414	`//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++`
393		415
394	`//===============Dimensions subelement (24 bytes, page 1-17)===============`	416	`//===============Dimensions subelement (24 bytes, page 1-17)===============`
395	`bytes = 12;`	417	`bytes = 12;`
396	`fwrite( &miINT32, 4, 1, fp );`	418	`fwrite( &miINT32, 4, 1, fp );`
397	`fwrite( &bytes, 4, 1, fp );`	419	`fwrite( &bytes, 4, 1, fp );`
398		420
399	`fwrite( &rows, 4, 1, fp );`	421	`fwrite( &rows, 4, 1, fp );`
400	`fwrite( &cols, 4, 1, fp );`	422	`fwrite( &cols, 4, 1, fp );`
401	`fwrite( &chans, 4, 1, fp );`	423	`fwrite( &chans, 4, 1, fp );`
402	`fwrite( &zero, 4, 1, fp ); // Padding to 64-bit boundary.`	424	`fwrite( &zero, 4, 1, fp ); // Padding to 64-bit boundary.`
403	`//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++`	425	`//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++`
404		426
405	`//==Array name (8 + strlen(varName) + (8-(strlen(varName)%8))%8 bytes, page 1-17)==`	427	`//==Array name (8 + strlen(varName) + (8-(strlen(varName)%8))%8 bytes, page 1-17)==`
406	`bytes = strlen(varName);`	428	`bytes = strlen(varName);`
407		429
408	`fwrite( &miINT8, 4, 1, fp );`	430	`fwrite( &miINT8, 4, 1, fp );`
409	`fwrite( &bytes, 4, 1, fp );`	431	`fwrite( &bytes, 4, 1, fp );`
410	`fwrite( varName, 1, bytes, fp );`	432	`fwrite( varName, 1, bytes, fp );`
411		433
412	`// Pad to nearest 64-bit boundary.`	434	`// Pad to nearest 64-bit boundary.`
413	`padBytes = (8-(bytes%8))%8;`	435	`padBytes = (8-(bytes%8))%8;`
414	`fwrite( &zero, 1, padBytes, fp );`	436	`fwrite( &zero, 1, padBytes, fp );`
415	`//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++`	437	`//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++`
416		438
417	`//====Matrix data (rowscolschans*bytesPerElement+8 bytes, page 1-20)=====`	439	`//====Matrix data (rowscolschans*bytesPerElement+8 bytes, page 1-20)=====`
418	`bytes = rowscolschans*bytesPerElement;`	440	`bytes = rowscolschans*bytesPerElement;`
419	`fwrite( &miClass, 4, 1, fp );`	441	`fwrite( &miClass, 4, 1, fp );`
420	`fwrite( &bytes, 4, 1, fp );`	442	`fwrite( &bytes, 4, 1, fp );`
421		443
422	`for( k = 0; k < chans; ++k )`	444	`for( k = 0; k < chans; ++k )`
423	`{`	445	`{`
424	`if( doBgrSwap )`	446	`if( doBgrSwap )`
425	`{`	447	`{`
426	`k2 = (k==0)? 2 : ((k==2)? 0 : 1);`	448	`k2 = (k==0)? 2 : ((k==2)? 0 : 1);`
427	`}`	449	`}`
428	`else`	450	`else`
429	`k2 = k;`	451	`k2 = k;`
430		452
431	`for( j = 0; j < cols; ++j )`	453	`for( j = 0; j < cols; ++j )`
432	`{`	454	`{`
433	`for( i = 0; i < rows; ++i )`	455	`for( i = 0; i < rows; ++i )`
434	`{`	456	`{`
435	`rowPtr = mat.data + mat.step*i;`	457	`rowPtr = mat.data + mat.step*i;`
436	`fwrite( rowPtr + (chansj + k2)bytesPerElement, bytesPerElement, 1, fp );`	458	`fwrite( rowPtr + (chansj + k2)bytesPerElement, bytesPerElement, 1, fp );`
437	`}`	459	`}`
438	`}`	460	`}`
439	`}`	461	`}`
440		462
441	`// Pad to 64-bit boundary.`	463	`// Pad to 64-bit boundary.`
442	`padBytes = (8-(bytes%8))%8;`	464	`padBytes = (8-(bytes%8))%8;`
443	`fwrite( &zero, 1, padBytes, fp );`	465	`fwrite( &zero, 1, padBytes, fp );`
444	`//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++`	466	`//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++`
445		467
446	`fclose(fp);`	468	`fclose(fp);`
447	`delete[] text;`	469	`delete[] text;`
448	`delete[] subsysOffset;`	470	`delete[] subsysOffset;`
449	`delete[] ver;`	471	`delete[] ver;`
450	`}`	472	`}`
451		473
452		474
453		475
454		476
455		477
456	`}`	478	`}`
457		479

Subversion Repositories seema-scanner

(root)/src/cvtools.cpp – Rev 36 → 42