Subversion Repositories seema-scanner

#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);

//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;

}

}