Subversion Repositories seema-scanner

#ifndef ALGORITHMTOOLS_H
#define ALGORITHMTOOLS_H

#include <opencv2/opencv.hpp>

// Convert an unsigned binary number to reflected binary Gray code.
// Source: http://en.wikipedia.org/wiki/Gray_code
inline unsigned int binaryToGray(unsigned int num) {
    return (num >> 1) ^ num;
}

// Convert a reflected binary Gray code number to a binary number.
// Source: http://en.wikipedia.org/wiki/Gray_code
inline unsigned int grayToBinary(unsigned int num){
    unsigned int mask;
    for(mask = num >> 1; mask != 0; mask = mask >> 1)
        num = num ^ mask;
    return num;
}

// Return the Nth bit of an unsigned integer number
inline bool getBit(int decimal, int N){

    return decimal & 1 << (N-1);
}

// Return the position of the least significant bit that is set
inline int leastSignificantBitSet(int x){
  if(x == 0)
      return 0;

  int val = 1;
  while(x>>=1)
      val++;

  return val;
}

// Compute the power of a number (where the exponent is an integer)
inline unsigned int powi(int num, unsigned int exponent){

    if(exponent == 0)
        return 1;

    float res = num;
    for(unsigned int i=0; i<exponent-1; i++)
        res *= num;

    return res;
}

// Compute the power of a 2 (where the exponent is an integer)
inline unsigned int twopowi(unsigned int exponent){
    return 1 << exponent;
}

inline cv::Vec3b getColorSubpix(const cv::Mat& img, cv::Point2f pt){
    assert(!img.empty());
    assert(img.channels() == 3);

    int x = (int)pt.x;
    int y = (int)pt.y;

    int x0 = cv::borderInterpolate(x,   img.cols, cv::BORDER_REFLECT_101);
    int x1 = cv::borderInterpolate(x+1, img.cols, cv::BORDER_REFLECT_101);
    int y0 = cv::borderInterpolate(y,   img.rows, cv::BORDER_REFLECT_101);
    int y1 = cv::borderInterpolate(y+1, img.rows, cv::BORDER_REFLECT_101);

    float a = pt.x - (float)x;
    float c = pt.y - (float)y;

    uchar b = (uchar)cvRound((img.at<cv::Vec3b>(y0, x0)[0] * (1.f - a) + img.at<cv::Vec3b>(y0, x1)[0] * a) * (1.f - c)
                           + (img.at<cv::Vec3b>(y1, x0)[0] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[0] * a) * c);
    uchar g = (uchar)cvRound((img.at<cv::Vec3b>(y0, x0)[1] * (1.f - a) + img.at<cv::Vec3b>(y0, x1)[1] * a) * (1.f - c)
                           + (img.at<cv::Vec3b>(y1, x0)[1] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[1] * a) * c);
    uchar r = (uchar)cvRound((img.at<cv::Vec3b>(y0, x0)[2] * (1.f - a) + img.at<cv::Vec3b>(y0, x1)[2] * a) * (1.f - c)
                           + (img.at<cv::Vec3b>(y1, x0)[2] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[2] * a) * c);

    return cv::Vec3b(b, g, r);
}

inline cv::Mat computePhaseVector(unsigned int length, float phase, float pitch){

    cv::Mat phaseVector(length, 1, CV_8UC3);
    //phaseVector.setTo(0);

    const float pi = M_PI;

    // Loop through vector
    for(int i=0; i<phaseVector.rows; i++){
        // Amplitude of channels
        float amp = 0.5*(1+cos(2*pi*i/pitch - phase));
        phaseVector.at<cv::Vec3b>(i, 0) = cv::Vec3b(255.0*amp,255.0*amp,255.0*amp);
    }

    return phaseVector;
}

inline cv::Mat getPhase(const cv::Mat I1, const cv::Mat I2, const cv::Mat I3){

    cv::Mat_<float> I1_(I1);
    cv::Mat_<float> I2_(I2);
    cv::Mat_<float> I3_(I3);

    cv::Mat phase;

    // One call approach
    cv::phase(2.0*I1_-I3_-I2_, sqrt(3.0)*(I2_-I3_), phase);
    return phase;

}

// Phase unwrapping by means of a phase cue
inline cv::Mat unwrapWithCue(const cv::Mat &up, const cv::Mat &upCue, float nPhases){

    const float pi = M_PI;

    // Determine fringe order
    cv::Mat P = (upCue*nPhases-up)/(2.0*pi);

    // Round to integers
    P.convertTo(P, CV_8U);
    P.convertTo(P, CV_32F);

    // Add to phase
    cv::Mat upUnwrapped = up + P*2*pi;

    // Scale to range [0; 2pi]
    upUnwrapped *= 1.0/nPhases;

    return upUnwrapped;
}

// Absolute phase and magnitude from N frames
inline std::vector<cv::Mat> getDFTComponents(const std::vector<cv::Mat> &frames){

    unsigned int N = frames.size();

//    std::vector<cv::Mat> framesReverse = frames;
//    std::reverse(framesReverse.begin(), framesReverse.end());

    // DFT approach
    cv::Mat I;
    cv::merge(frames, I);
    unsigned int w = I.cols;
    unsigned int h = I.rows;
    I = I.reshape(1, h*w);
    I.convertTo(I, CV_32F);
    cv::Mat fI;
    cv::dft(I, fI, cv::DFT_ROWS + cv::DFT_COMPLEX_OUTPUT);
    fI = fI.reshape(N*2, h);

    std::vector<cv::Mat> fIcomp;
    cv::split(fI, fIcomp);

    return fIcomp;

}

#endif // ALGORITHMTOOLS_H