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


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

Subversion Repositories seema-scanner

(root)/src/cvtools.cpp – Rev 33 → 34