Subversion Repositories seema-scanner

Rev

Rev 167 | Rev 196 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 167 Rev 176
Line 3... Line 3...
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 
#include <numeric>
8 
 
 9 
 
9 
#include <png.h>
 10 
#include <png.h>
10 
#include <zlib.h>
 11 
#include <zlib.h>
11 
 
 12 
 
12 
namespace cvtools{
 13 
namespace cvtools{
Line 338... Line 339...
338 
}
 339 
}
339 
 
 340 
 
340 
// Closed form solution to solve for the rotation axis from sets of 3D point coordinates of flat pattern feature points
 341 
// 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
 342 
// Algorithm according to Chen et al., Rotation axis calibration of a turntable using constrained global optimization, Optik 2014
342 
// DTU, 2014, Jakob Wilm
 343 
// 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){
 344 
void rotationAxisCalibration(const std::vector< std::vector<cv::Point3f> > Qcam, const std::vector<cv::Point3f> Qobj, cv::Vec3f &axis, cv::Vec3f &point, float &error){
344 
 
 345 
 
345 
    // number of frames (points on each arch)
 346 
    // number of frames (points on each arch)
346 
    int l = Qcam.size();
 347 
    int l = Qcam.size();
347 
 
 348 
 
348 
    // number of points in each frame
 349 
    // number of points in each frame
Line 395... Line 396...
395 
    axis = cv::normalize(axis);
 396 
    axis = cv::normalize(axis);
396 
 
 397 
 
397 
    float nx = u.at<float>(idx, 0);
 398 
    float nx = u.at<float>(idx, 0);
398 
    float ny = u.at<float>(idx, 1);
 399 
    float ny = u.at<float>(idx, 1);
399 
    float nz = u.at<float>(idx, 2);
 400 
    float nz = u.at<float>(idx, 2);
400 
    //float d  = u.at<float>(idx, 3);
 401 
//    float d  = u.at<float>(idx, 3);
401 
    float dh = u.at<float>(idx, 4);
 402 
    float dh = u.at<float>(idx, 4);
402 
    float dv = u.at<float>(idx, 5);
 403 
    float dv = u.at<float>(idx, 5);
403 
 
 404 
 
404 
//    // Paper version: c is initially eliminated
 405 
//    // Paper version: c is initially eliminated
405 
//    cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));
 406 
//    cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));
Line 470... Line 471...
470 
 
 471 
 
471 
    point[0] = a;
 472 
    point[0] = a;
472 
    point[1] = b;
 473 
    point[1] = b;
473 
    point[2] = c;
 474 
    point[2] = c;
474 
 
 475 
 
- 
 
 476 
    // Error estimate (mean 3D point deviations from circles around rotation axis)
- 
 
 477 
    error = 0;
- 
 
 478 
    // loop through saddle points
- 
 
 479 
    for(unsigned int idx=0; idx<mn; idx++){
- 
 
 480 
 
- 
 
 481 
        // vector of distances from rotation axis
- 
 
 482 
        std::vector<float> dI(l);
- 
 
 483 
        // loop through angular positions
- 
 
 484 
        for(int k=0; k<l; k++){
- 
 
 485 
            cv::Vec3f p = cv::Vec3f(Qcam[k][idx]);
- 
 
 486 
            // point to line distance
- 
 
 487 
            dI[k] = cv::norm((point-p)-(point-p).dot(axis)*axis);
- 
 
 488 
        }
- 
 
 489 
        float sum = std::accumulate(dI.begin(), dI.end(), 0.0);
- 
 
 490 
        float mean = sum / dI.size();
- 
 
 491 
        float meanDev = 0;
- 
 
 492 
        for(int k=0; k<l; k++){
- 
 
 493 
            meanDev += std::abs(dI[k] - mean);
- 
 
 494 
        }
- 
 
 495 
        meanDev /= l;
- 
 
 496 
        //std::cout << meanDev << std::endl;
- 
 
 497 
        error += meanDev;
- 
 
 498 
    }
- 
 
 499 
    error /= mn;
475 
}
 500 
}
476 
 
 501 
 
477 
// Function to fit two sets of corresponding pose data.
 502 
// Function to fit two sets of corresponding pose data.
478 
// Finds [Omega | tau], to minimize ||[R_mark | t_mark] - [Omega | tau][R | t]||^2
 503 
// 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.
 504 
// Algorithm and notation according to Mili Shah, Comparing two sets of corresponding six degree of freedom data, CVIU 2011.