Subversion Repositories seema-scanner

#include <ceres/ceres.h>

struct CircleResidual {

  CircleResidual(std::vector<cv::Point3f> _pointsOnArc)

      : pointsOnArc(_pointsOnArc) {}

  template <typename T>

  bool operator()(const T* const point_x, const T* const point_y, const T* const point_z,

                  const T* const axis_x, const T* const axis_y, const T* const axis_z,

                  T* residual) const {

    cv::Vec3f axis(*axis_x, *axis_y, *axis_z);

    cv::Vec3f point(*point_x, *point_y, *point_z);

    unsigned int l = pointsOnArc.size();

    std::vector<float> dI(l);

    for(unsigned int i=0; i<l; i++){

      cv::Vec3f p = cv::Vec3f(pointsOnArc[i]);

      // point to line distance

      dI[i] = cv::norm((point-p)-(point-p).dot(axis)*axis);

    }

    float sum = std::accumulate(dI.begin(), dI.end(), 0.0);

    float mean = sum / dI.size();

    float meanDev = 0;

    for(unsigned int k=0; k<l; k++){

      meanDev += std::abs(dI[k] - mean);

    }

    meanDev /= l;

    residual[0] = T(meanDev);

    return true;

  }

 private:

  // Observations for one checkerboard corner.

  const std::vector<cv::Point3f> pointsOnArc;

};

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

static void rotationAxisCalibration(const std::vector< std::vector<cv::Point3f> > Qcam, const std::vector<cv::Point3f> Qobj, cv::Vec3f &axis, cv::Vec3f &point, float &error){

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

    // Non-linear optimization using Ceres

    double point_x = point[0];

    double point_y = point[1];

    double point_z = point[2];

    double axis_x = axis[0];

    double axis_y = axis[1];

    double axis_z = axis[2];

    ceres::Problem problem;

    // loop through saddle points

    for(unsigned int idx=0; idx<mn; idx++){

        std::vector<cv::Point3f> pointsOnArch(l);

        for(int k=0; k<l; k++){

            pointsOnArch[k] = Qcam[k][idx];

        }

        ceres::CostFunction* cost_function =

           new ceres::NumericDiffCostFunction<CircleResidual, ceres::CENTRAL, 1, 1, 1, 1, 1, 1, 1>(

               new CircleResidual(pointsOnArch));

        problem.AddResidualBlock(cost_function, NULL, &point_x, &point_y, &point_z, &axis_x, &axis_y, &axis_z);

    }

    // Run the solver!

    ceres::Solver::Options options;

    options.linear_solver_type = ceres::DENSE_QR;

    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;

    ceres::Solve(options, &problem, &summary);

    std::cout << summary.BriefReport() << "\n";

    point = cv::Point3f(point_x, point_y, point_z);

    axis = cv::Point3f(axis_x, axis_y, axis_z);

    // Error estimate (mean 3D point deviations from circles around rotation axis)

    error = 0;

    // loop through saddle points

    for(unsigned int idx=0; idx<mn; idx++){

        // vector of distances from rotation axis

        std::vector<float> dI(l);

        // loop through angular positions

        for(int k=0; k<l; k++){

            cv::Vec3f p = cv::Vec3f(Qcam[k][idx]);

            // point to line distance

            dI[k] = cv::norm((point-p)-(point-p).dot(axis)*axis);

        }

        float sum = std::accumulate(dI.begin(), dI.end(), 0.0);

        float mean = sum / dI.size();

        float meanDev = 0;

        for(int k=0; k<l; k++){

            meanDev += std::abs(dI[k] - mean);

        }

        meanDev /= l;

        //std::cout << meanDev << std::endl;

        error += meanDev;

    }

    error /= mn;

}