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


#include "cvtools.h"
 
#ifdef _WIN32
#include <cstdint>
#endif
 
#include <stdio.h>
 
#include <png.h>
#include <zlib.h>
 
namespace cvtools{
 
// Convert an BGR 3-channel image back into a Bayered image. This saves memory when reading images from disk.
void cvtColorBGRToBayerBG(const cv::Mat &imBGR, cv::Mat &imBayerBG){
 
    imBayerBG.create(imBGR.size(), CV_8UC1);
 
    for(int r=0; r<imBGR.rows; r++){
        for(int c=0; c<imBGR.cols; c++){
 
            bool evenRow = r % 2;
            bool evenCol = c % 2;
 
            if(evenRow & evenCol)
                imBayerBG.at<uchar>(r,c) = imBGR.at<cv::Vec3b>(r,c)[0];
            else if(!evenRow & !evenCol)
                imBayerBG.at<uchar>(r,c) = imBGR.at<cv::Vec3b>(r,c)[2];
            else
                imBayerBG.at<uchar>(r,c) = imBGR.at<cv::Vec3b>(r,c)[1];
 
        }
 
    }
}
 
// Removes matches not satisfying the epipolar constraint.
// F is the fundamental matrix.
// Works like cv::correctMatches(), except it removes matches with an error distance greater than maxD.
void removeIncorrectMatches(const cv::Mat F, const std::vector<cv::Point2f> &q0, const std::vector<cv::Point2f> &q1, const float maxD,
                                std::vector<cv::Point2f> q0Correct, std::vector<cv::Point2f> q1Correct){
 
    int n0 = (int)q0.size(), n1 = (int)q1.size();
    q0Correct.reserve(n0);
    q1Correct.reserve(n1);
 
    // Point to line distance
//    for( int i = 0; i < n1; i++ ){
//        cv::Vec3f p0 = cv::Vec3f(q0[i].x, q0[i].y, 1.0);
//        // Epipolar line defined by p0
//        cv::Vec3f l = F*p0;
//        l /= sqrt(l(0)*l(0) + l(1)*l(1));
//        for( int j = 0; j < n2; j++ ){
//            cv::Vec3f p1 = cv::Vec3f(q1[i].x, q1[i].y, 1.0);
//            // Signed distance to line
//            float d = l.dot(p1);
//            if(d < maxD){
//                q0Correct.push_back(q0[i]);
//                q1Correct.push_back(q1[i]);
//            }
//        }
//    }
 
    // Symmetric epipolar distance
    std::vector<cv::Point3f> l0, l1;
    cv::computeCorrespondEpilines(q0, 1, F, l0);
    cv::computeCorrespondEpilines(q1, 2, F, l1);
 
    for(int i = 0; i < n0; i++){
        cv::Vec3f p0 = cv::Vec3f(q0[i].x, q0[i].y, 1.0);
        for(int j = 0; j < n1; j++){
            cv::Vec3f p1 = cv::Vec3f(q1[j].x, q1[j].y, 1.0);
            float d01 = l0[i].dot(p1);
            float d10 = l1[j].dot(p0);
            float d = d01*d01 + d10*d10;
            if(d < maxD){
                q0Correct.push_back(q0[i]);
                q1Correct.push_back(q1[i]);
            }
        }
    }
 
//    // Sampson Error (H&Z, p. 287) (expensive...)
//    std::vector<cv::Point3f> p0, p1;
//    cv::convertPointsToHomogeneous(q0, p0);
//    cv::convertPointsToHomogeneous(q1, p1);
//    cv::Mat Fp0Mat = cv::Mat(F)*cv::Mat(p0).reshape(1).t();
//    cv::Mat FTp1Mat = cv::Mat(F.t())*cv::Mat(p1).reshape(1).t();
//    for( int i = 0; i < n1; i++ ){
//        cv::Vec3f Fp0 = Fp0Mat.col(i);
//        for( int j = 0; j < n2; j++ ){
//            cv::Vec3f FTp1 = FTp1Mat.col(j);
//            cv::Matx<float,1,1> p1TFp0 = cv::Matx31f(p1[j]).t()*F*cv::Matx31f(p0[i]);
//            float d = p1TFp0(0)*p1TFp0(0) / (Fp0(0)*Fp0(0) + Fp0(1)*Fp0(1) + FTp1(0)*FTp1(0) + FTp1(1)*FTp1(1));
//            if(d < maxD){
//                q0Correct.push_back(q0[i]);
//                q1Correct.push_back(q1[i]);
//            }
//        }
//    }
 
    return;
}
 
// Performs bitwise right-shift on every value of matrix I. This is done e.g. to remove the least significant bits in a 16 to 8-bit image conversion.
void rshift(cv::Mat& I, unsigned int shift){
 
    int nRows = I.rows;
    int nCols = I.cols;
 
    if (I.isContinuous()){
        nCols *= nRows;
        nRows = 1;
    }
 
    int i,j;
    unsigned short* p;
    for( i = 0; i < nRows; ++i){
        p = I.ptr<unsigned short>(i);
        for ( j = 0; j < nCols; ++j){
            p[j] = p[j]>>shift;
        }
    }
}
 
 
// Lightly modified OpenCV function which accepts a line width argument
void drawChessboardCorners(cv::InputOutputArray _image, cv::Size patternSize, cv::InputArray _corners, bool patternWasFound, int line_width){
    cv::Mat corners = _corners.getMat();
    if( corners.empty() )
        return;
    cv::Mat image = _image.getMat(); CvMat c_image = _image.getMat();
    int nelems = corners.checkVector(2, CV_32F, true);
    CV_Assert(nelems >= 0);
    cvtools::cvDrawChessboardCorners( &c_image, patternSize, (CvPoint2D32f*)corners.data,
                             nelems, patternWasFound, line_width);
}
 
void cvDrawChessboardCorners(CvArr* _image, CvSize pattern_size, CvPoint2D32f* corners, int count, int found, int line_width){
    const int shift = 0;
    const int radius = 12;
    const int r = radius*(1 << shift);
    int i;
    CvMat stub, *image;
    double scale = 1;
    int type, cn, line_type;
 
    image = cvGetMat( _image, &stub );
 
    type = CV_MAT_TYPE(image->type);
    cn = CV_MAT_CN(type);
    if( cn != 1 && cn != 3 && cn != 4 )
        CV_Error( CV_StsUnsupportedFormat, "Number of channels must be 1, 3 or 4" );
 
    switch( CV_MAT_DEPTH(image->type) )
    {
    case CV_8U:
        scale = 1;
        break;
    case CV_16U:
        scale = 256;
        break;
    case CV_32F:
        scale = 1./255;
        break;
    default:
        CV_Error( CV_StsUnsupportedFormat,
            "Only 8-bit, 16-bit or floating-point 32-bit images are supported" );
    }
 
    line_type = type == CV_8UC1 || type == CV_8UC3 ? CV_AA : 8;
 
    if( !found )
    {
        CvScalar color = {{0,0,255}};
        if( cn == 1 )
            color = cvScalarAll(200);
        color.val[0] *= scale;
        color.val[1] *= scale;
        color.val[2] *= scale;
        color.val[3] *= scale;
 
        for( i = 0; i < count; i++ )
        {
            CvPoint pt;
            pt.x = cvRound(corners[i].x*(1 << shift));
            pt.y = cvRound(corners[i].y*(1 << shift));
            cvLine( image, cvPoint( pt.x - r, pt.y - r ),
                    cvPoint( pt.x + r, pt.y + r ), color, line_width, line_type, shift );
            cvLine( image, cvPoint( pt.x - r, pt.y + r),
                    cvPoint( pt.x + r, pt.y - r), color, line_width, line_type, shift );
            cvCircle( image, pt, r+(1<<shift), color, line_width, line_type, shift );
        }
    }
    else
    {
        int x, y;
        CvPoint prev_pt = {0, 0};
        const int line_max = 7;
        static const CvScalar line_colors[line_max] =
        {
            {{0,0,255}},
            {{0,128,255}},
            {{0,200,200}},
            {{0,255,0}},
            {{200,200,0}},
            {{255,0,0}},
            {{255,0,255}}
        };
 
        for( y = 0, i = 0; y < pattern_size.height; y++ )
        {
            CvScalar color = line_colors[y % line_max];
            if( cn == 1 )
                color = cvScalarAll(200);
            color.val[0] *= scale;
            color.val[1] *= scale;
            color.val[2] *= scale;
            color.val[3] *= scale;
 
            for( x = 0; x < pattern_size.width; x++, i++ )
            {
                CvPoint pt;
                pt.x = cvRound(corners[i].x*(1 << shift));
                pt.y = cvRound(corners[i].y*(1 << shift));
 
                if( i != 0 )
                    cvLine( image, prev_pt, pt, color, 1, line_type, shift );
 
                cvLine( image, cvPoint(pt.x - r, pt.y - r),
                        cvPoint(pt.x + r, pt.y + r), color, line_width, line_type, shift );
                cvLine( image, cvPoint(pt.x - r, pt.y + r),
                        cvPoint(pt.x + r, pt.y - r), color, line_width, line_type, shift );
                cvCircle( image, pt, r+(1<<shift), color, line_width, line_type, shift );
                prev_pt = pt;
            }
        }
    }
}
 
// Returns the result of mod(mat(x,y), moduli) for each matrix element
cv::Mat modulo(const cv::Mat &mat, float n){
 
    cv::Mat ret(mat.rows, mat.cols, mat.type());
 
    for(int row=0; row<ret.rows; row++){
        for(int col=0; col<ret.cols; col++){
            float val = mat.at<float>(row, col);
            // note: std::fmod calculates the remainder, not arithmetic modulo
            ret.at<float>(row, col) = val - n * std::floor(val / n);
        }
    }
 
    return ret;
}
 
// 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){
 
    size_t 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(unsigned 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(unsigned 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);
 
    cv::Mat err_tau = b - (A*cv::Mat(tau));
    std::cout << err_tau << std::endl;
}
 
// Closed form solution to solve for the rotation axis from sets of 3D point coordinates of flat pattern feature points
// Algorithm according to Chen et al., Rotation axis calibration of a turntable using constrained global optimization, Optik 2014
// DTU, 2014, Jakob Wilm
void rotationAxisCalibration(const std::vector< std::vector<cv::Point3f> > Qcam, const std::vector<cv::Point3f> Qobj, cv::Vec3f &axis, cv::Vec3f &point){
 
    // number of frames (points on each arch)
    int l = Qcam.size();
 
    // number of points in each frame
    size_t mn = Qobj.size();
 
    assert(mn == Qcam[0].size());
 
    // construct matrix for axis determination
    cv::Mat M(6, 6, CV_32F, cv::Scalar(0));
 
    for(int k=0; k<l; k++){
        for(unsigned int idx=0; idx<mn; idx++){
 
//            float i = Qobj[idx].x+4;
//            float j = Qobj[idx].y+4;
            float i = Qobj[idx].x;
            float j = Qobj[idx].y;
 
            float x = Qcam[k][idx].x;
            float y = Qcam[k][idx].y;
            float z = Qcam[k][idx].z;
 
            M += (cv::Mat_<float>(6,6) << x*x, x*y, x*z, x, i*x, j*x,
                                            0, y*y, y*z, y, i*y, j*y,
                                            0,   0, z*z, z, i*z, j*z,
                                            0,   0,   0, 1,   i,   j,
                                            0,   0,   0, 0, i*i, i*j,
                                            0,   0,   0, 0,   0, j*j);
 
        }
    }
 
    cv::completeSymm(M, false);
 
    // solve for axis
    std::vector<float> lambda;
    cv::Mat u;
    cv::eigen(M, lambda, u);
 
    float minLambda = std::abs(lambda[0]);
    int idx = 0;
    for(unsigned int i=1; i<lambda.size(); i++){
        if(abs(lambda[i]) < minLambda){
            minLambda = lambda[i];
            idx = i;
        }
    }
 
    axis = u.row(idx).colRange(0, 3);
    axis = cv::normalize(axis);
 
    float nx = u.at<float>(idx, 0);
    float ny = u.at<float>(idx, 1);
    float nz = u.at<float>(idx, 2);
    //float d  = u.at<float>(idx, 3);
    float dh = u.at<float>(idx, 4);
    float dv = u.at<float>(idx, 5);
 
//    // Paper version: c is initially eliminated
//    cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));
//    cv::Mat bb(l*mn, 1, CV_32F);
 
//    for(int k=0; k<l; k++){
//        for(int idx=0; idx<mn; idx++){
 
//            float i = Qobj[idx].x;
//            float j = Qobj[idx].y;
 
//            float x = Qcam[k][idx].x;
//            float y = Qcam[k][idx].y;
//            float z = Qcam[k][idx].z;
 
//            float f = x*x + y*y + z*z + (2*x*nx + 2*y*ny + 2*z*nz)*(i*dh + j*dv);
 
//            int row = k*mn+idx;
//            A.at<float>(row, 0) = 2*x - (2*z*nx)/nz;
//            A.at<float>(row, 1) = 2*y - (2*z*ny)/nz;
//            A.at<float>(row, idx+2) = 1.0;
 
//            bb.at<float>(row, 0) = f + (2*z*d)/nz;
//        }
//    }
 
//    // solve for point
//    cv::Mat abe;
//    cv::solve(A, bb, abe, cv::DECOMP_SVD);
 
//    float a = abe.at<float>(0, 0);
//    float b = abe.at<float>(1, 0);
//    float c = -(nx*a+ny*b+d)/nz;
 
    // Our version: solve simultanously for a,b,c
    cv::Mat A(l*mn, mn+3, CV_32F, cv::Scalar(0.0));
    cv::Mat bb(l*mn, 1, CV_32F);
 
    for(int k=0; k<l; k++){
        for(unsigned int idx=0; idx<mn; idx++){
 
            float i = Qobj[idx].x;
            float j = Qobj[idx].y;
 
            float x = Qcam[k][idx].x;
            float y = Qcam[k][idx].y;
            float z = Qcam[k][idx].z;
 
            float f = x*x + y*y + z*z + (2*x*nx + 2*y*ny + 2*z*nz)*(i*dh + j*dv);
 
            int row = k*mn+idx;
            A.at<float>(row, 0) = 2*x;
            A.at<float>(row, 1) = 2*y;
            A.at<float>(row, 2) = 2*z;
            A.at<float>(row, idx+3) = 1.0;
 
            bb.at<float>(row, 0) = f;
        }
    }
 
    // solve for point
    cv::Mat abe;
    cv::solve(A, bb, abe, cv::DECOMP_SVD);
 
    float a = abe.at<float>(0, 0);
    float b = abe.at<float>(1, 0);
    float c = abe.at<float>(2, 0);
 
    point[0] = a;
    point[1] = b;
    point[2] = c;
 
}
 
// 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){
 
    size_t 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(unsigned 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(unsigned 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(int col = 0; col < pattern_diamond.cols; col++){
        for(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, 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);
}
 
 
 
 
 
 
 
 
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 147	Rev 167
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	`#include <png.h>`	9	`#include <png.h>`
10	`#include <zlib.h>`	10	`#include <zlib.h>`
11		11
12	`namespace cvtools{`	12	`namespace cvtools{`
13		13
14	`// Convert an BGR 3-channel image back into a Bayered image. This saves memory when reading images from disk.`	14	`// Convert an BGR 3-channel image back into a Bayered image. This saves memory when reading images from disk.`
15	`void cvtColorBGRToBayerBG(const cv::Mat &imBGR, cv::Mat &imBayerBG){`	15	`void cvtColorBGRToBayerBG(const cv::Mat &imBGR, cv::Mat &imBayerBG){`
16		16
17	`imBayerBG.create(imBGR.size(), CV_8UC1);`	17	`imBayerBG.create(imBGR.size(), CV_8UC1);`
18		18
19	`for(int r=0; r<imBGR.rows; r++){`	19	`for(int r=0; r<imBGR.rows; r++){`
20	`for(int c=0; c<imBGR.cols; c++){`	20	`for(int c=0; c<imBGR.cols; c++){`
21		21
22	`bool evenRow = r % 2;`	22	`bool evenRow = r % 2;`
23	`bool evenCol = c % 2;`	23	`bool evenCol = c % 2;`
24		24
25	`if(evenRow & evenCol)`	25	`if(evenRow & evenCol)`
26	`imBayerBG.at<uchar>(r,c) = imBGR.at<cv::Vec3b>(r,c)[0];`	26	`imBayerBG.at<uchar>(r,c) = imBGR.at<cv::Vec3b>(r,c)[0];`
27	`else if(!evenRow & !evenCol)`	27	`else if(!evenRow & !evenCol)`
28	`imBayerBG.at<uchar>(r,c) = imBGR.at<cv::Vec3b>(r,c)[2];`	28	`imBayerBG.at<uchar>(r,c) = imBGR.at<cv::Vec3b>(r,c)[2];`
29	`else`	29	`else`
30	`imBayerBG.at<uchar>(r,c) = imBGR.at<cv::Vec3b>(r,c)[1];`	30	`imBayerBG.at<uchar>(r,c) = imBGR.at<cv::Vec3b>(r,c)[1];`
31		31
32	`}`	32	`}`
33		33
34	`}`	34	`}`
35	`}`	35	`}`
36		36
37	`// Removes matches not satisfying the epipolar constraint.`	37	`// Removes matches not satisfying the epipolar constraint.`
38	`// F is the fundamental matrix.`	38	`// F is the fundamental matrix.`
39	`// Works like cv::correctMatches(), except it removes matches with an error distance greater than maxD.`	39	`// Works like cv::correctMatches(), except it removes matches with an error distance greater than maxD.`
40	`void removeIncorrectMatches(const cv::Mat F, const std::vector<cv::Point2f> &q0, const std::vector<cv::Point2f> &q1, const float maxD,`	40	`void removeIncorrectMatches(const cv::Mat F, const std::vector<cv::Point2f> &q0, const std::vector<cv::Point2f> &q1, const float maxD,`
41	`std::vector<cv::Point2f> q0Correct, std::vector<cv::Point2f> q1Correct){`	41	`std::vector<cv::Point2f> q0Correct, std::vector<cv::Point2f> q1Correct){`
42		42
43	`int n0 = (int)q0.size(), n1 = (int)q1.size();`	43	`int n0 = (int)q0.size(), n1 = (int)q1.size();`
44	`q0Correct.reserve(n0);`	44	`q0Correct.reserve(n0);`
45	`q1Correct.reserve(n1);`	45	`q1Correct.reserve(n1);`
46		46
47	`// Point to line distance`	47	`// Point to line distance`
48	`// for( int i = 0; i < n1; i++ ){`	48	`// for( int i = 0; i < n1; i++ ){`
49	`// cv::Vec3f p0 = cv::Vec3f(q0[i].x, q0[i].y, 1.0);`	49	`// cv::Vec3f p0 = cv::Vec3f(q0[i].x, q0[i].y, 1.0);`
50	`// // Epipolar line defined by p0`	50	`// // Epipolar line defined by p0`
51	`// cv::Vec3f l = F*p0;`	51	`// cv::Vec3f l = F*p0;`
52	`// l /= sqrt(l(0)l(0) + l(1)l(1));`	52	`// l /= sqrt(l(0)l(0) + l(1)l(1));`
53	`// for( int j = 0; j < n2; j++ ){`	53	`// for( int j = 0; j < n2; j++ ){`
54	`// cv::Vec3f p1 = cv::Vec3f(q1[i].x, q1[i].y, 1.0);`	54	`// cv::Vec3f p1 = cv::Vec3f(q1[i].x, q1[i].y, 1.0);`
55	`// // Signed distance to line`	55	`// // Signed distance to line`
56	`// float d = l.dot(p1);`	56	`// float d = l.dot(p1);`
57	`// if(d < maxD){`	57	`// if(d < maxD){`
58	`// q0Correct.push_back(q0[i]);`	58	`// q0Correct.push_back(q0[i]);`
59	`// q1Correct.push_back(q1[i]);`	59	`// q1Correct.push_back(q1[i]);`
60	`// }`	60	`// }`
61	`// }`	61	`// }`
62	`// }`	62	`// }`
63		63
64	`// Symmetric epipolar distance`	64	`// Symmetric epipolar distance`
65	`std::vector<cv::Point3f> l0, l1;`	65	`std::vector<cv::Point3f> l0, l1;`
66	`cv::computeCorrespondEpilines(q0, 1, F, l0);`	66	`cv::computeCorrespondEpilines(q0, 1, F, l0);`
67	`cv::computeCorrespondEpilines(q1, 2, F, l1);`	67	`cv::computeCorrespondEpilines(q1, 2, F, l1);`
68		68
69	`for(int i = 0; i < n0; i++){`	69	`for(int i = 0; i < n0; i++){`
70	`cv::Vec3f p0 = cv::Vec3f(q0[i].x, q0[i].y, 1.0);`	70	`cv::Vec3f p0 = cv::Vec3f(q0[i].x, q0[i].y, 1.0);`
71	`for(int j = 0; j < n1; j++){`	71	`for(int j = 0; j < n1; j++){`
72	`cv::Vec3f p1 = cv::Vec3f(q1[j].x, q1[j].y, 1.0);`	72	`cv::Vec3f p1 = cv::Vec3f(q1[j].x, q1[j].y, 1.0);`
73	`float d01 = l0[i].dot(p1);`	73	`float d01 = l0[i].dot(p1);`
74	`float d10 = l1[j].dot(p0);`	74	`float d10 = l1[j].dot(p0);`
75	`float d = d01d01 + d10d10;`	75	`float d = d01d01 + d10d10;`
76	`if(d < maxD){`	76	`if(d < maxD){`
77	`q0Correct.push_back(q0[i]);`	77	`q0Correct.push_back(q0[i]);`
78	`q1Correct.push_back(q1[i]);`	78	`q1Correct.push_back(q1[i]);`
79	`}`	79	`}`
80	`}`	80	`}`
81	`}`	81	`}`
82		82
83	`// // Sampson Error (H&Z, p. 287) (expensive...)`	83	`// // Sampson Error (H&Z, p. 287) (expensive...)`
84	`// std::vector<cv::Point3f> p0, p1;`	84	`// std::vector<cv::Point3f> p0, p1;`
85	`// cv::convertPointsToHomogeneous(q0, p0);`	85	`// cv::convertPointsToHomogeneous(q0, p0);`
86	`// cv::convertPointsToHomogeneous(q1, p1);`	86	`// cv::convertPointsToHomogeneous(q1, p1);`
87	`// cv::Mat Fp0Mat = cv::Mat(F)*cv::Mat(p0).reshape(1).t();`	87	`// cv::Mat Fp0Mat = cv::Mat(F)*cv::Mat(p0).reshape(1).t();`
88	`// cv::Mat FTp1Mat = cv::Mat(F.t())*cv::Mat(p1).reshape(1).t();`	88	`// cv::Mat FTp1Mat = cv::Mat(F.t())*cv::Mat(p1).reshape(1).t();`
89	`// for( int i = 0; i < n1; i++ ){`	89	`// for( int i = 0; i < n1; i++ ){`
90	`// cv::Vec3f Fp0 = Fp0Mat.col(i);`	90	`// cv::Vec3f Fp0 = Fp0Mat.col(i);`
91	`// for( int j = 0; j < n2; j++ ){`	91	`// for( int j = 0; j < n2; j++ ){`
92	`// cv::Vec3f FTp1 = FTp1Mat.col(j);`	92	`// cv::Vec3f FTp1 = FTp1Mat.col(j);`
93	`// cv::Matx<float,1,1> p1TFp0 = cv::Matx31f(p1[j]).t()Fcv::Matx31f(p0[i]);`	93	`// cv::Matx<float,1,1> p1TFp0 = cv::Matx31f(p1[j]).t()Fcv::Matx31f(p0[i]);`
94	`// float d = p1TFp0(0)p1TFp0(0) / (Fp0(0)Fp0(0) + Fp0(1)Fp0(1) + FTp1(0)FTp1(0) + FTp1(1)*FTp1(1));`	94	`// float d = p1TFp0(0)p1TFp0(0) / (Fp0(0)Fp0(0) + Fp0(1)Fp0(1) + FTp1(0)FTp1(0) + FTp1(1)*FTp1(1));`
95	`// if(d < maxD){`	95	`// if(d < maxD){`
96	`// q0Correct.push_back(q0[i]);`	96	`// q0Correct.push_back(q0[i]);`
97	`// q1Correct.push_back(q1[i]);`	97	`// q1Correct.push_back(q1[i]);`
98	`// }`	98	`// }`
99	`// }`	99	`// }`
100	`// }`	100	`// }`
101		101
102	`return;`	102	`return;`
103	`}`	103	`}`
104		104
105	`// Performs bitwise right-shift on every value of matrix I. This is done e.g. to remove the least significant bits in a 16 to 8-bit image conversion.`	105	`// Performs bitwise right-shift on every value of matrix I. This is done e.g. to remove the least significant bits in a 16 to 8-bit image conversion.`
106	`void rshift(cv::Mat& I, unsigned int shift){`	106	`void rshift(cv::Mat& I, unsigned int shift){`
107		107
108	`int nRows = I.rows;`	108	`int nRows = I.rows;`
109	`int nCols = I.cols;`	109	`int nCols = I.cols;`
110		110
111	`if (I.isContinuous()){`	111	`if (I.isContinuous()){`
112	`nCols *= nRows;`	112	`nCols *= nRows;`
113	`nRows = 1;`	113	`nRows = 1;`
114	`}`	114	`}`
115		115
116	`int i,j;`	116	`int i,j;`
117	`unsigned short* p;`	117	`unsigned short* p;`
118	`for( i = 0; i < nRows; ++i){`	118	`for( i = 0; i < nRows; ++i){`
119	`p = I.ptr<unsigned short>(i);`	119	`p = I.ptr<unsigned short>(i);`
120	`for ( j = 0; j < nCols; ++j){`	120	`for ( j = 0; j < nCols; ++j){`
121	`p[j] = p[j]>>shift;`	121	`p[j] = p[j]>>shift;`
122	`}`	122	`}`
123	`}`	123	`}`
124	`}`	124	`}`
125		125
126		126
127	`// Lightly modified OpenCV function which accepts a line width argument`	127	`// Lightly modified OpenCV function which accepts a line width argument`
128	`void drawChessboardCorners(cv::InputOutputArray _image, cv::Size patternSize, cv::InputArray _corners, bool patternWasFound, int line_width){`	128	`void drawChessboardCorners(cv::InputOutputArray _image, cv::Size patternSize, cv::InputArray _corners, bool patternWasFound, int line_width){`
129	`cv::Mat corners = _corners.getMat();`	129	`cv::Mat corners = _corners.getMat();`
130	`if( corners.empty() )`	130	`if( corners.empty() )`
131	`return;`	131	`return;`
132	`cv::Mat image = _image.getMat(); CvMat c_image = _image.getMat();`	132	`cv::Mat image = _image.getMat(); CvMat c_image = _image.getMat();`
133	`int nelems = corners.checkVector(2, CV_32F, true);`	133	`int nelems = corners.checkVector(2, CV_32F, true);`
134	`CV_Assert(nelems >= 0);`	134	`CV_Assert(nelems >= 0);`
135	`cvtools::cvDrawChessboardCorners( &c_image, patternSize, (CvPoint2D32f*)corners.data,`	135	`cvtools::cvDrawChessboardCorners( &c_image, patternSize, (CvPoint2D32f*)corners.data,`
136	`nelems, patternWasFound, line_width);`	136	`nelems, patternWasFound, line_width);`
137	`}`	137	`}`
138		138
139	`void cvDrawChessboardCorners(CvArr* _image, CvSize pattern_size, CvPoint2D32f* corners, int count, int found, int line_width){`	139	`void cvDrawChessboardCorners(CvArr* _image, CvSize pattern_size, CvPoint2D32f* corners, int count, int found, int line_width){`
140	`const int shift = 0;`	140	`const int shift = 0;`
141	`const int radius = 12;`	141	`const int radius = 12;`
142	`const int r = radius*(1 << shift);`	142	`const int r = radius*(1 << shift);`
143	`int i;`	143	`int i;`
144	`CvMat stub, *image;`	144	`CvMat stub, *image;`
145	`double scale = 1;`	145	`double scale = 1;`
146	`int type, cn, line_type;`	146	`int type, cn, line_type;`
147		147
148	`image = cvGetMat( _image, &stub );`	148	`image = cvGetMat( _image, &stub );`
149		149
150	`type = CV_MAT_TYPE(image->type);`	150	`type = CV_MAT_TYPE(image->type);`
151	`cn = CV_MAT_CN(type);`	151	`cn = CV_MAT_CN(type);`
152	`if( cn != 1 && cn != 3 && cn != 4 )`	152	`if( cn != 1 && cn != 3 && cn != 4 )`
153	`CV_Error( CV_StsUnsupportedFormat, "Number of channels must be 1, 3 or 4" );`	153	`CV_Error( CV_StsUnsupportedFormat, "Number of channels must be 1, 3 or 4" );`
154		154
155	`switch( CV_MAT_DEPTH(image->type) )`	155	`switch( CV_MAT_DEPTH(image->type) )`
156	`{`	156	`{`
157	`case CV_8U:`	157	`case CV_8U:`
158	`scale = 1;`	158	`scale = 1;`
159	`break;`	159	`break;`
160	`case CV_16U:`	160	`case CV_16U:`
161	`scale = 256;`	161	`scale = 256;`
162	`break;`	162	`break;`
163	`case CV_32F:`	163	`case CV_32F:`
164	`scale = 1./255;`	164	`scale = 1./255;`
165	`break;`	165	`break;`
166	`default:`	166	`default:`
167	`CV_Error( CV_StsUnsupportedFormat,`	167	`CV_Error( CV_StsUnsupportedFormat,`
168	`"Only 8-bit, 16-bit or floating-point 32-bit images are supported" );`	168	`"Only 8-bit, 16-bit or floating-point 32-bit images are supported" );`
169	`}`	169	`}`
170		170
171	`line_type = type == CV_8UC1 \|\| type == CV_8UC3 ? CV_AA : 8;`	171	`line_type = type == CV_8UC1 \|\| type == CV_8UC3 ? CV_AA : 8;`
172		172
173	`if( !found )`	173	`if( !found )`
174	`{`	174	`{`
175	`CvScalar color = {{0,0,255}};`	175	`CvScalar color = {{0,0,255}};`
176	`if( cn == 1 )`	176	`if( cn == 1 )`
177	`color = cvScalarAll(200);`	177	`color = cvScalarAll(200);`
178	`color.val[0] *= scale;`	178	`color.val[0] *= scale;`
179	`color.val[1] *= scale;`	179	`color.val[1] *= scale;`
180	`color.val[2] *= scale;`	180	`color.val[2] *= scale;`
181	`color.val[3] *= scale;`	181	`color.val[3] *= scale;`
182		182
183	`for( i = 0; i < count; i++ )`	183	`for( i = 0; i < count; i++ )`
184	`{`	184	`{`
185	`CvPoint pt;`	185	`CvPoint pt;`
186	`pt.x = cvRound(corners[i].x*(1 << shift));`	186	`pt.x = cvRound(corners[i].x*(1 << shift));`
187	`pt.y = cvRound(corners[i].y*(1 << shift));`	187	`pt.y = cvRound(corners[i].y*(1 << shift));`
188	`cvLine( image, cvPoint( pt.x - r, pt.y - r ),`	188	`cvLine( image, cvPoint( pt.x - r, pt.y - r ),`
189	`cvPoint( pt.x + r, pt.y + r ), color, line_width, line_type, shift );`	189	`cvPoint( pt.x + r, pt.y + r ), color, line_width, line_type, shift );`
190	`cvLine( image, cvPoint( pt.x - r, pt.y + r),`	190	`cvLine( image, cvPoint( pt.x - r, pt.y + r),`
191	`cvPoint( pt.x + r, pt.y - r), color, line_width, line_type, shift );`	191	`cvPoint( pt.x + r, pt.y - r), color, line_width, line_type, shift );`
192	`cvCircle( image, pt, r+(1<<shift), color, line_width, line_type, shift );`	192	`cvCircle( image, pt, r+(1<<shift), color, line_width, line_type, shift );`
193	`}`	193	`}`
194	`}`	194	`}`
195	`else`	195	`else`
196	`{`	196	`{`
197	`int x, y;`	197	`int x, y;`
198	`CvPoint prev_pt = {0, 0};`	198	`CvPoint prev_pt = {0, 0};`
199	`const int line_max = 7;`	199	`const int line_max = 7;`
200	`static const CvScalar line_colors[line_max] =`	200	`static const CvScalar line_colors[line_max] =`
201	`{`	201	`{`
202	`{{0,0,255}},`	202	`{{0,0,255}},`
203	`{{0,128,255}},`	203	`{{0,128,255}},`
204	`{{0,200,200}},`	204	`{{0,200,200}},`
205	`{{0,255,0}},`	205	`{{0,255,0}},`
206	`{{200,200,0}},`	206	`{{200,200,0}},`
207	`{{255,0,0}},`	207	`{{255,0,0}},`
208	`{{255,0,255}}`	208	`{{255,0,255}}`
209	`};`	209	`};`
210		210
211	`for( y = 0, i = 0; y < pattern_size.height; y++ )`	211	`for( y = 0, i = 0; y < pattern_size.height; y++ )`
212	`{`	212	`{`
213	`CvScalar color = line_colors[y % line_max];`	213	`CvScalar color = line_colors[y % line_max];`
214	`if( cn == 1 )`	214	`if( cn == 1 )`
215	`color = cvScalarAll(200);`	215	`color = cvScalarAll(200);`
216	`color.val[0] *= scale;`	216	`color.val[0] *= scale;`
217	`color.val[1] *= scale;`	217	`color.val[1] *= scale;`
218	`color.val[2] *= scale;`	218	`color.val[2] *= scale;`
219	`color.val[3] *= scale;`	219	`color.val[3] *= scale;`
220		220
221	`for( x = 0; x < pattern_size.width; x++, i++ )`	221	`for( x = 0; x < pattern_size.width; x++, i++ )`
222	`{`	222	`{`
223	`CvPoint pt;`	223	`CvPoint pt;`
224	`pt.x = cvRound(corners[i].x*(1 << shift));`	224	`pt.x = cvRound(corners[i].x*(1 << shift));`
225	`pt.y = cvRound(corners[i].y*(1 << shift));`	225	`pt.y = cvRound(corners[i].y*(1 << shift));`
226		226
227	`if( i != 0 )`	227	`if( i != 0 )`
228	`cvLine( image, prev_pt, pt, color, 1, line_type, shift );`	228	`cvLine( image, prev_pt, pt, color, 1, line_type, shift );`
229		229
230	`cvLine( image, cvPoint(pt.x - r, pt.y - r),`	230	`cvLine( image, cvPoint(pt.x - r, pt.y - r),`
231	`cvPoint(pt.x + r, pt.y + r), color, line_width, line_type, shift );`	231	`cvPoint(pt.x + r, pt.y + r), color, line_width, line_type, shift );`
232	`cvLine( image, cvPoint(pt.x - r, pt.y + r),`	232	`cvLine( image, cvPoint(pt.x - r, pt.y + r),`
233	`cvPoint(pt.x + r, pt.y - r), color, line_width, line_type, shift );`	233	`cvPoint(pt.x + r, pt.y - r), color, line_width, line_type, shift );`
234	`cvCircle( image, pt, r+(1<<shift), color, line_width, line_type, shift );`	234	`cvCircle( image, pt, r+(1<<shift), color, line_width, line_type, shift );`
235	`prev_pt = pt;`	235	`prev_pt = pt;`
236	`}`	236	`}`
237	`}`	237	`}`
238	`}`	238	`}`
239	`}`	239	`}`
240		240
241	`// Returns the result of mod(mat(x,y), moduli) for each matrix element`	241	`// Returns the result of mod(mat(x,y), moduli) for each matrix element`
242	`cv::Mat modulo(const cv::Mat &mat, float n){`	242	`cv::Mat modulo(const cv::Mat &mat, float n){`
243		243
244	`cv::Mat ret(mat.rows, mat.cols, mat.type());`	244	`cv::Mat ret(mat.rows, mat.cols, mat.type());`
245		245
246	`for(int row=0; row<ret.rows; row++){`	246	`for(int row=0; row<ret.rows; row++){`
247	`for(int col=0; col<ret.cols; col++){`	247	`for(int col=0; col<ret.cols; col++){`
248	`float val = mat.at<float>(row, col);`	248	`float val = mat.at<float>(row, col);`
249	`// note: std::fmod calculates the remainder, not arithmetic modulo`	249	`// note: std::fmod calculates the remainder, not arithmetic modulo`
250	`ret.at<float>(row, col) = val - n * std::floor(val / n);`	250	`ret.at<float>(row, col) = val - n * std::floor(val / n);`
251	`}`	251	`}`
252	`}`	252	`}`
253		253
254	`return ret;`	254	`return ret;`
255	`}`	255	`}`
256		256
257	`// Convert a 3xN matrix to a vector of Point3fs.`	257	`// Convert a 3xN matrix to a vector of Point3fs.`
258	`void matToPoints3f(const cv::Mat &mat, std::vector<cv::Point3f> &points){`	258	`void matToPoints3f(const cv::Mat &mat, std::vector<cv::Point3f> &points){`
259		259
260	`unsigned int nPoints = mat.cols;`	260	`unsigned int nPoints = mat.cols;`
261	`points.resize(nPoints);`	261	`points.resize(nPoints);`
262		262
263	`for(unsigned int i=0; i<nPoints; i++)`	263	`for(unsigned int i=0; i<nPoints; i++)`
264	`points[i] = cv::Point3f(mat.at<float>(0, i), mat.at<float>(1, i), mat.at<float>(2, i));`	264	`points[i] = cv::Point3f(mat.at<float>(0, i), mat.at<float>(1, i), mat.at<float>(2, i));`
265	`}`	265	`}`
266		266
267	`// Convert a (Dim+1)xN matrix of homogenous points to a DimxN matrix of points in non-homogenous coordinates.`	267	`// Convert a (Dim+1)xN matrix of homogenous points to a DimxN matrix of points in non-homogenous coordinates.`
268	`void convertMatFromHomogeneous(cv::Mat &src, cv::Mat &dst){`	268	`void convertMatFromHomogeneous(cv::Mat &src, cv::Mat &dst){`
269	`unsigned int N = src.cols;`	269	`unsigned int N = src.cols;`
270	`unsigned int Dim = src.rows-1;`	270	`unsigned int Dim = src.rows-1;`
271	`dst.create(Dim, N, src.type());`	271	`dst.create(Dim, N, src.type());`
272	`for(unsigned int i=0; i<N; i++){`	272	`for(unsigned int i=0; i<N; i++){`
273	`for(unsigned int j=0; j<Dim; j++)`	273	`for(unsigned int j=0; j<Dim; j++)`
274	`dst.at<float>(j,i) = src.at<float>(j,i)/src.at<float>(Dim,i);`	274	`dst.at<float>(j,i) = src.at<float>(j,i)/src.at<float>(Dim,i);`
275	`}`	275	`}`
276		276
277	`}`	277	`}`
278		278
279	`// Function to solve the hand-eye (or eye-in-hand) calibration problem.`	279	`// Function to solve the hand-eye (or eye-in-hand) calibration problem.`
280	`// Finds [Omega \| tau], to minimize \|\|[R_mark \| t_mark][Omega \| tau] - [Omega \| tau][R \| t]\|\|^2`	280	`// Finds [Omega \| tau], to minimize \|\|[R_mark \| t_mark][Omega \| tau] - [Omega \| tau][R \| t]\|\|^2`
281	`// Algorithm according to Tsai, Lenz, A new technique for fully autonomous and efficient 3d robotics hand-eye calibration`	281	`// Algorithm according to Tsai, Lenz, A new technique for fully autonomous and efficient 3d robotics hand-eye calibration`
282	`// DTU, 2014, Jakob Wilm`	282	`// DTU, 2014, Jakob Wilm`
283	`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){`	283	`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){`
284		284
285	`int N = R.size();`	285	`size_t N = R.size();`
286	`assert(N == R_mark.size());`	286	`assert(N == R_mark.size());`
287	`assert(N == t.size());`	287	`assert(N == t.size());`
288	`assert(N == t_mark.size());`	288	`assert(N == t_mark.size());`
289		289
290	`// construct equations for rotation`	290	`// construct equations for rotation`
291	`cv::Mat A(3*N, 3, CV_32F);`	291	`cv::Mat A(3*N, 3, CV_32F);`
292	`cv::Mat b(3*N, 1, CV_32F);`	292	`cv::Mat b(3*N, 1, CV_32F);`
293	`for(int i=0; i<N; i++){`	293	`for(unsigned int i=0; i<N; i++){`
294	`// angle axis representations`	294	`// angle axis representations`
295	`cv::Vec3f rot;`	295	`cv::Vec3f rot;`
296	`cv::Vec3f rot_mark;`	296	`cv::Vec3f rot_mark;`
297	`cv::Rodrigues(R[i], rot);`	297	`cv::Rodrigues(R[i], rot);`
298	`cv::Rodrigues(R_mark[i], rot_mark);`	298	`cv::Rodrigues(R_mark[i], rot_mark);`
299		299
300	`cv::Vec3f P = 2.0sin(cv::norm(rot)/2.0)cv::normalize(rot);`	300	`cv::Vec3f P = 2.0sin(cv::norm(rot)/2.0)cv::normalize(rot);`
301	`//std::cout << "P: " << std::endl << P << std::endl;`	301	`//std::cout << "P: " << std::endl << P << std::endl;`
302	`cv::Vec3f P_mark = 2.0sin(cv::norm(rot_mark)/2.0)cv::normalize(rot_mark);`	302	`cv::Vec3f P_mark = 2.0sin(cv::norm(rot_mark)/2.0)cv::normalize(rot_mark);`
303	`//std::cout << "P_mark: " << std::endl << P_mark << std::endl;`	303	`//std::cout << "P_mark: " << std::endl << P_mark << std::endl;`
304	`cv::Vec3f sum = P+P_mark;`	304	`cv::Vec3f sum = P+P_mark;`
305	`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);`	305	`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);`
306	`//std::cout << "crossProduct: " << std::endl << crossProduct << std::endl;`	306	`//std::cout << "crossProduct: " << std::endl << crossProduct << std::endl;`
307	`crossProduct.copyTo(A.rowRange(i3, i3+3));`	307	`crossProduct.copyTo(A.rowRange(i3, i3+3));`
308		308
309	`cv::Mat(P-P_mark).copyTo(b.rowRange(i3, i3+3));`	309	`cv::Mat(P-P_mark).copyTo(b.rowRange(i3, i3+3));`
310	`}`	310	`}`
311		311
312	`// solve for rotation`	312	`// solve for rotation`
313	`cv::Vec3f P_prime;`	313	`cv::Vec3f P_prime;`
314	`cv::solve(A, b, P_prime, cv::DECOMP_SVD);`	314	`cv::solve(A, b, P_prime, cv::DECOMP_SVD);`
315	`cv::Vec3f P = 2.0P_prime/(cv::sqrt(1.0 + cv::norm(P_prime)cv::norm(P_prime)));`	315	`cv::Vec3f P = 2.0P_prime/(cv::sqrt(1.0 + cv::norm(P_prime)cv::norm(P_prime)));`
316	`float nP = cv::norm(P);`	316	`float nP = cv::norm(P);`
317	`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);`	317	`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);`
318	`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);`	318	`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);`
319	`Omega = cv::Matx33f(OmegaMat);`	319	`Omega = cv::Matx33f(OmegaMat);`
320		320
321	`// construct equations for translation`	321	`// construct equations for translation`
322	`A.setTo(0.0);`	322	`A.setTo(0.0);`
323	`b.setTo(0.0);`	323	`b.setTo(0.0);`
324	`for(int i=0; i<N; i++){`	324	`for(unsigned int i=0; i<N; i++){`
325		325
326	`cv::Mat diff = cv::Mat(R_mark[i]) - cv::Mat::eye(3, 3, CV_32F);`	326	`cv::Mat diff = cv::Mat(R_mark[i]) - cv::Mat::eye(3, 3, CV_32F);`
327	`diff.copyTo(A.rowRange(i3, i3+3));`	327	`diff.copyTo(A.rowRange(i3, i3+3));`
328		328
329	`cv::Mat diff_mark = cv::Mat(Omega*t[i] - t_mark[i]);`	329	`cv::Mat diff_mark = cv::Mat(Omega*t[i] - t_mark[i]);`
330	`diff_mark.copyTo(b.rowRange(i3, i3+3));`	330	`diff_mark.copyTo(b.rowRange(i3, i3+3));`
331	`}`	331	`}`
332		332
333	`// solve for translation`	333	`// solve for translation`
334	`cv::solve(A, b, tau, cv::DECOMP_SVD);`	334	`cv::solve(A, b, tau, cv::DECOMP_SVD);`
335		335
336	`cv::Mat err_tau = b - (A*cv::Mat(tau));`	336	`cv::Mat err_tau = b - (A*cv::Mat(tau));`
337	`std::cout << err_tau << std::endl;`	337	`std::cout << err_tau << std::endl;`
338	`}`	338	`}`
339		339
340	`// Closed form solution to solve for the rotation axis from sets of 3D point coordinates of flat pattern feature points`	340	`// Closed form solution to solve for the rotation axis from sets of 3D point coordinates of flat pattern feature points`
341	`// Algorithm according to Chen et al., Rotation axis calibration of a turntable using constrained global optimization, Optik 2014`	341	`// Algorithm according to Chen et al., Rotation axis calibration of a turntable using constrained global optimization, Optik 2014`
342	`// DTU, 2014, Jakob Wilm`	342	`// DTU, 2014, Jakob Wilm`
343	`void rotationAxisCalibration(const std::vector< std::vector<cv::Point3f> > Qcam, const std::vector<cv::Point3f> Qobj, cv::Vec3f &axis, cv::Vec3f &point){`	343	`void rotationAxisCalibration(const std::vector< std::vector<cv::Point3f> > Qcam, const std::vector<cv::Point3f> Qobj, cv::Vec3f &axis, cv::Vec3f &point){`
344		344
345	`// number of frames (points on each arch)`	345	`// number of frames (points on each arch)`
346	`int l = Qcam.size();`	346	`int l = Qcam.size();`
347		347
348	`// number of points in each frame`	348	`// number of points in each frame`
349	`int mn = Qobj.size();`	349	`size_t mn = Qobj.size();`
350		350
351	`assert(mn == Qcam[0].size());`	351	`assert(mn == Qcam[0].size());`
352		352
353	`// construct matrix for axis determination`	353	`// construct matrix for axis determination`
354	`cv::Mat M(6, 6, CV_32F, cv::Scalar(0));`	354	`cv::Mat M(6, 6, CV_32F, cv::Scalar(0));`
355		355
356	`for(int k=0; k<l; k++){`	356	`for(int k=0; k<l; k++){`
357	`for(int idx=0; idx<mn; idx++){`	357	`for(unsigned int idx=0; idx<mn; idx++){`
358		358
359	`// float i = Qobj[idx].x+4;`	359	`// float i = Qobj[idx].x+4;`
360	`// float j = Qobj[idx].y+4;`	360	`// float j = Qobj[idx].y+4;`
361	`float i = Qobj[idx].x;`	361	`float i = Qobj[idx].x;`
362	`float j = Qobj[idx].y;`	362	`float j = Qobj[idx].y;`
363		363
364	`float x = Qcam[k][idx].x;`	364	`float x = Qcam[k][idx].x;`
365	`float y = Qcam[k][idx].y;`	365	`float y = Qcam[k][idx].y;`
366	`float z = Qcam[k][idx].z;`	366	`float z = Qcam[k][idx].z;`
367		367
368	`M += (cv::Mat_<float>(6,6) << xx, xy, xz, x, ix, j*x,`	368	`M += (cv::Mat_<float>(6,6) << xx, xy, xz, x, ix, j*x,`
369	`0, yy, yz, y, iy, jy,`	369	`0, yy, yz, y, iy, jy,`
370	`0, 0, zz, z, iz, j*z,`	370	`0, 0, zz, z, iz, j*z,`
371	`0, 0, 0, 1, i, j,`	371	`0, 0, 0, 1, i, j,`
372	`0, 0, 0, 0, ii, ij,`	372	`0, 0, 0, 0, ii, ij,`
373	`0, 0, 0, 0, 0, j*j);`	373	`0, 0, 0, 0, 0, j*j);`
374		374
375	`}`	375	`}`
376	`}`	376	`}`
377		377
378	`cv::completeSymm(M, false);`	378	`cv::completeSymm(M, false);`
379		379
380	`// solve for axis`	380	`// solve for axis`
381	`std::vector<float> lambda;`	381	`std::vector<float> lambda;`
382	`cv::Mat u;`	382	`cv::Mat u;`
383	`cv::eigen(M, lambda, u);`	383	`cv::eigen(M, lambda, u);`
384		384
385	`float minLambda = std::abs(lambda[0]);`	385	`float minLambda = std::abs(lambda[0]);`
386	`int idx = 0;`	386	`int idx = 0;`
387	`for(int i=1; i<lambda.size(); i++){`	387	`for(unsigned int i=1; i<lambda.size(); i++){`
388	`if(abs(lambda[i]) < minLambda){`	388	`if(abs(lambda[i]) < minLambda){`
389	`minLambda = lambda[i];`	389	`minLambda = lambda[i];`
390	`idx = i;`	390	`idx = i;`
391	`}`	391	`}`
392	`}`	392	`}`
393		393
394	`axis = u.row(idx).colRange(0, 3);`	394	`axis = u.row(idx).colRange(0, 3);`
395	`axis = cv::normalize(axis);`	395	`axis = cv::normalize(axis);`
396		396
397	`float nx = u.at<float>(idx, 0);`	397	`float nx = u.at<float>(idx, 0);`
398	`float ny = u.at<float>(idx, 1);`	398	`float ny = u.at<float>(idx, 1);`
399	`float nz = u.at<float>(idx, 2);`	399	`float nz = u.at<float>(idx, 2);`
400	`float d = u.at<float>(idx, 3);`	400	`//float d = u.at<float>(idx, 3);`
401	`float dh = u.at<float>(idx, 4);`	401	`float dh = u.at<float>(idx, 4);`
402	`float dv = u.at<float>(idx, 5);`	402	`float dv = u.at<float>(idx, 5);`
403		403
404	`// // Paper version: c is initially eliminated`	404	`// // Paper version: c is initially eliminated`
405	`// cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));`	405	`// cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));`
406	`// cv::Mat bb(l*mn, 1, CV_32F);`	406	`// cv::Mat bb(l*mn, 1, CV_32F);`
407		407
408	`// for(int k=0; k<l; k++){`	408	`// for(int k=0; k<l; k++){`
409	`// for(int idx=0; idx<mn; idx++){`	409	`// for(int idx=0; idx<mn; idx++){`
410		410
411	`// float i = Qobj[idx].x;`	411	`// float i = Qobj[idx].x;`
412	`// float j = Qobj[idx].y;`	412	`// float j = Qobj[idx].y;`
413		413
414	`// float x = Qcam[k][idx].x;`	414	`// float x = Qcam[k][idx].x;`
415	`// float y = Qcam[k][idx].y;`	415	`// float y = Qcam[k][idx].y;`
416	`// float z = Qcam[k][idx].z;`	416	`// float z = Qcam[k][idx].z;`
417		417
418	`// float f = xx + yy + zz + (2xnx + 2yny + 2znz)(idh + jdv);`	418	`// float f = xx + yy + zz + (2xnx + 2yny + 2znz)(idh + jdv);`
419		419
420	`// int row = k*mn+idx;`	420	`// int row = k*mn+idx;`
421	`// A.at<float>(row, 0) = 2x - (2z*nx)/nz;`	421	`// A.at<float>(row, 0) = 2x - (2z*nx)/nz;`
422	`// A.at<float>(row, 1) = 2y - (2z*ny)/nz;`	422	`// A.at<float>(row, 1) = 2y - (2z*ny)/nz;`
423	`// A.at<float>(row, idx+2) = 1.0;`	423	`// A.at<float>(row, idx+2) = 1.0;`
424		424
425	`// bb.at<float>(row, 0) = f + (2zd)/nz;`	425	`// bb.at<float>(row, 0) = f + (2zd)/nz;`
426	`// }`	426	`// }`
427	`// }`	427	`// }`
428		428
429	`// // solve for point`	429	`// // solve for point`
430	`// cv::Mat abe;`	430	`// cv::Mat abe;`
431	`// cv::solve(A, bb, abe, cv::DECOMP_SVD);`	431	`// cv::solve(A, bb, abe, cv::DECOMP_SVD);`
432		432
433	`// float a = abe.at<float>(0, 0);`	433	`// float a = abe.at<float>(0, 0);`
434	`// float b = abe.at<float>(1, 0);`	434	`// float b = abe.at<float>(1, 0);`
435	`// float c = -(nxa+nyb+d)/nz;`	435	`// float c = -(nxa+nyb+d)/nz;`
436		436
437	`// Our version: solve simultanously for a,b,c`	437	`// Our version: solve simultanously for a,b,c`
438	`cv::Mat A(l*mn, mn+3, CV_32F, cv::Scalar(0.0));`	438	`cv::Mat A(l*mn, mn+3, CV_32F, cv::Scalar(0.0));`
439	`cv::Mat bb(l*mn, 1, CV_32F);`	439	`cv::Mat bb(l*mn, 1, CV_32F);`
440		440
441	`for(int k=0; k<l; k++){`	441	`for(int k=0; k<l; k++){`
442	`for(int idx=0; idx<mn; idx++){`	442	`for(unsigned int idx=0; idx<mn; idx++){`
443		443
444	`float i = Qobj[idx].x;`	444	`float i = Qobj[idx].x;`
445	`float j = Qobj[idx].y;`	445	`float j = Qobj[idx].y;`
446		446
447	`float x = Qcam[k][idx].x;`	447	`float x = Qcam[k][idx].x;`
448	`float y = Qcam[k][idx].y;`	448	`float y = Qcam[k][idx].y;`
449	`float z = Qcam[k][idx].z;`	449	`float z = Qcam[k][idx].z;`
450		450
451	`float f = xx + yy + zz + (2xnx + 2yny + 2znz)(idh + jdv);`	451	`float f = xx + yy + zz + (2xnx + 2yny + 2znz)(idh + jdv);`
452		452
453	`int row = k*mn+idx;`	453	`int row = k*mn+idx;`
454	`A.at<float>(row, 0) = 2*x;`	454	`A.at<float>(row, 0) = 2*x;`
455	`A.at<float>(row, 1) = 2*y;`	455	`A.at<float>(row, 1) = 2*y;`
456	`A.at<float>(row, 2) = 2*z;`	456	`A.at<float>(row, 2) = 2*z;`
457	`A.at<float>(row, idx+3) = 1.0;`	457	`A.at<float>(row, idx+3) = 1.0;`
458		458
459	`bb.at<float>(row, 0) = f;`	459	`bb.at<float>(row, 0) = f;`
460	`}`	460	`}`
461	`}`	461	`}`
462		462
463	`// solve for point`	463	`// solve for point`
464	`cv::Mat abe;`	464	`cv::Mat abe;`
465	`cv::solve(A, bb, abe, cv::DECOMP_SVD);`	465	`cv::solve(A, bb, abe, cv::DECOMP_SVD);`
466		466
467	`float a = abe.at<float>(0, 0);`	467	`float a = abe.at<float>(0, 0);`
468	`float b = abe.at<float>(1, 0);`	468	`float b = abe.at<float>(1, 0);`
469	`float c = abe.at<float>(2, 0);`	469	`float c = abe.at<float>(2, 0);`
470		470
471	`point[0] = a;`	471	`point[0] = a;`
472	`point[1] = b;`	472	`point[1] = b;`
473	`point[2] = c;`	473	`point[2] = c;`
474		474
475	`}`	475	`}`
476		476
477	`// Function to fit two sets of corresponding pose data.`	477	`// Function to fit two sets of corresponding pose data.`
478	`// Finds [Omega \| tau], to minimize \|\|[R_mark \| t_mark] - [Omega \| tau][R \| t]\|\|^2`	478	`// Finds [Omega \| tau], to minimize \|\|[R_mark \| t_mark] - [Omega \| tau][R \| t]\|\|^2`
479	`// Algorithm and notation according to Mili Shah, Comparing two sets of corresponding six degree of freedom data, CVIU 2011.`	479	`// Algorithm and notation according to Mili Shah, Comparing two sets of corresponding six degree of freedom data, CVIU 2011.`
480	`// DTU, 2013, Oline V. Olesen, Jakob Wilm`	480	`// DTU, 2013, Oline V. Olesen, Jakob Wilm`
481	`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){`	481	`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){`
482		482
483	`int N = R.size();`	483	`size_t N = R.size();`
484	`assert(N == R_mark.size());`	484	`assert(N == R_mark.size());`
485	`assert(N == t.size());`	485	`assert(N == t.size());`
486	`assert(N == t_mark.size());`	486	`assert(N == t_mark.size());`
487		487
488	`// Mean translations`	488	`// Mean translations`
489	`cv::Vec3f t_mean;`	489	`cv::Vec3f t_mean;`
490	`cv::Vec3f t_mark_mean;`	490	`cv::Vec3f t_mark_mean;`
491	`for(int i=0; i<N; i++){`	491	`for(unsigned int i=0; i<N; i++){`
492	`t_mean += 1.0/N * t[i];`	492	`t_mean += 1.0/N * t[i];`
493	`t_mark_mean += 1.0/N * t_mark[i];`	493	`t_mark_mean += 1.0/N * t_mark[i];`
494	`}`	494	`}`
495		495
496	`// Data with mean adjusted translations`	496	`// Data with mean adjusted translations`
497	`cv::Mat X_bar(3, 4*N, CV_32F);`	497	`cv::Mat X_bar(3, 4*N, CV_32F);`
498	`cv::Mat X_mark_bar(3, 4*N, CV_32F);`	498	`cv::Mat X_mark_bar(3, 4*N, CV_32F);`
499	`for(int i=0; i<N; i++){`	499	`for(unsigned int i=0; i<N; i++){`
500	`cv::Mat(R[i]).copyTo(X_bar.colRange(i4,i4+3));`	500	`cv::Mat(R[i]).copyTo(X_bar.colRange(i4,i4+3));`

Subversion Repositories seema-scanner

(root)/src/cvtools.cpp – Rev 147 → 167