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



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

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

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

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

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

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

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

        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 ]

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

Rev 147	Rev 167
Line 280...	Line 280...
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);`
Line 319...	Line 319...
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]);`
Line 344...	Line 344...
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;`
Line 382...	Line 382...
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	`}`
Line 395...	Line 395...
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));`
Line 437...	Line 437...
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;`
Line 478...	Line 478...
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));`
501	`cv::Mat(t[i] - t_mean).copyTo(X_bar.col(i*4+3));`	501	`cv::Mat(t[i] - t_mean).copyTo(X_bar.col(i*4+3));`
502	`cv::Mat(R_mark[i]).copyTo(X_mark_bar.colRange(i4,i4+3));`	502	`cv::Mat(R_mark[i]).copyTo(X_mark_bar.colRange(i4,i4+3));`
503	`cv::Mat(t_mark[i] - t_mark_mean).copyTo(X_mark_bar.col(i*4+3));`	503	`cv::Mat(t_mark[i] - t_mark_mean).copyTo(X_mark_bar.col(i*4+3));`
504	`}`	504	`}`
Line 570...	Line 570...
570	`// Downsample a texture which was created in virtual column/row space for a diamond pixel array projector`	570	`// Downsample a texture which was created in virtual column/row space for a diamond pixel array projector`
571	`cv::Mat diamondDownsample(cv::Mat &pattern){`	571	`cv::Mat diamondDownsample(cv::Mat &pattern){`
572		572
573	`cv::Mat pattern_diamond(pattern.rows,pattern.cols/2,CV_8UC3);`	573	`cv::Mat pattern_diamond(pattern.rows,pattern.cols/2,CV_8UC3);`
574		574
575	`for(unsigned int col = 0; col < pattern_diamond.cols; col++){`	575	`for(int col = 0; col < pattern_diamond.cols; col++){`
576	`for(unsigned int row = 0; row < pattern_diamond.rows; row++){`	576	`for(int row = 0; row < pattern_diamond.rows; row++){`
577		577
578	`pattern_diamond.at<cv::Vec3b>(row,col)=pattern.at<cv::Vec3b>(row,col*2+row%2);`	578	`pattern_diamond.at<cv::Vec3b>(row,col)=pattern.at<cv::Vec3b>(row,col*2+row%2);`
579	`}`	579	`}`
580	`}`	580	`}`
581		581
582	`return pattern_diamond;`	582	`return pattern_diamond;`
583		583
584	`}`	584	`}`
585		585
586		586
587	`void mouseCallback(int evt, int x, int y, int flags, void* param){`	587	`void mouseCallback(int evt, int x, int y, void* param){`
588	`cv::Mat im = (cv::Mat) param;`	588	`cv::Mat im = (cv::Mat) param;`
589	`if (evt == CV_EVENT_LBUTTONDOWN) {`	589	`if (evt == CV_EVENT_LBUTTONDOWN) {`
590	`if(im->type() == CV_8UC3){`	590	`if(im->type() == CV_8UC3){`
591	`printf("%d %d: %d, %d, %d\n",`	591	`printf("%d %d: %d, %d, %d\n",`
592	`x, y,`	592	`x, y,`
Line 610...	Line 610...
610	`cv::moveWindow(windowName, x, y);`	610	`cv::moveWindow(windowName, x, y);`
611	`}`	611	`}`
612	`cv::imshow(windowName, im);`	612	`cv::imshow(windowName, im);`
613	`}`	613	`}`
614		614
615	`void imagesc(const char *windowName, cv::Mat im){`	-
616		-
617	`// Imshow with scaled image`	-
618		-
619		-
620	`}`	-
621		-
622	`cv::Mat histimage(cv::Mat histogram){`	615	`cv::Mat histimage(cv::Mat histogram){`
623		616
624	`cv::Mat histImage(512, 640, CV_8UC3, cv::Scalar(0));`	617	`cv::Mat histImage(512, 640, CV_8UC3, cv::Scalar(0));`
625		618
626	`// Normalize the result to [ 2, histImage.rows-2 ]`	619	`// Normalize the result to [ 2, histImage.rows-2 ]`
Line 680...	Line 673...
680	`bool doBgrSwap;`	673	`bool doBgrSwap;`
681	`char mxClass;`	674	`char mxClass;`
682	`int32_t miClass;`	675	`int32_t miClass;`
683	`uchar const* rowPtr;`	676	`uchar const* rowPtr;`
684	`uint32_t tmp32;`	677	`uint32_t tmp32;`
685	`float tmp;`	678	`//float tmp;`
686	`FILE* fp;`	679	`FILE* fp;`
687		680
688	`// Matlab constants.`	681	`// Matlab constants.`
689	`const uint16_t MI = 0x4d49; // Contains "MI" in ascii.`	682	`const uint16_t MI = 0x4d49; // Contains "MI" in ascii.`
690	`const int32_t miINT8 = 1;`	683	`const int32_t miINT8 = 1;`
Line 701...	Line 694...
701	`const char mxINT8_CLASS = 8;`	694	`const char mxINT8_CLASS = 8;`
702	`const char mxUINT8_CLASS = 9;`	695	`const char mxUINT8_CLASS = 9;`
703	`const char mxINT16_CLASS = 10;`	696	`const char mxINT16_CLASS = 10;`
704	`const char mxUINT16_CLASS = 11;`	697	`const char mxUINT16_CLASS = 11;`
705	`const char mxINT32_CLASS = 12;`	698	`const char mxINT32_CLASS = 12;`
706	`const char mxUINT32_CLASS = 13;`	699	`//const char mxUINT32_CLASS = 13;`
707	`const uint64_t zero = 0; // Used for padding.`	700	`const uint64_t zero = 0; // Used for padding.`
708		701
709	`fp = fopen( filename, "wb" );`	702	`fp = fopen( filename, "wb" );`
710		703
711	`if( fp == 0 )`	704	`if( fp == 0 )`

Subversion Repositories seema-scanner

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