c++ ,sibuxiangyi_sanpinwaicha

#include "stdafx.h"
#include "Grating.h"

Grating::Grating()
{
}


Grating::~Grating()
{
}

//https://www.cnblogs.com/wujianming-110117/p/14301087.html
//相机标定参数，包括相机内参以及相机的畸变系数
bool Grating::getCameraParamFile()
{
  //cv::Mat camera_mat, distCoeffs;
  cv::FileStorage fin;
  fin.open("cameraparam.xml", cv::FileStorage::READ);
  if (fin.isOpened())
  {
    fin["cameraMatrix"] >> camera_mat;
    fin["distCoeffs"] >> distcoeffs_mat;
    /*cout << cameraMatrix << endl;
    cout << distCoeffs << endl;*/
    fin.release();
  }
  else
  {
    return 0;
  }
  return 1;
}
//四步相移法
//同一个频率的条纹，分不同的相位，投射到物体表面上。
void Grating::fourStepPhaseShifting(cv::Mat grating_img1[])
{
  //CV_8UC1 含义https://blog.csdn.net/weixin_51326570/article/details/113932128
  //CV_<bit_depth>(S|U|F)C<number_of_channels>,,,8位深度，U，无符号，C1是单通道灰度图，C2是3通道RGB图
  //定义fai的三个零矩阵
  cv::Mat fai_img[3] =
  {
    //grating_rows=1200,grating_cols=1600
    cv::Mat::zeros(grating_rows, grating_cols, CV_8UC1),
    cv::Mat::zeros(grating_rows, grating_cols, CV_8UC1),
    cv::Mat::zeros(grating_rows, grating_cols, CV_8UC1)
  };
  //定义调制度的三个零矩阵
  cv::Mat b_img[3] =
  {
    cv::Mat::zeros(grating_rows, grating_cols, CV_8UC1),
    cv::Mat::zeros(grating_rows, grating_cols, CV_8UC1),
    cv::Mat::zeros(grating_rows, grating_cols, CV_8UC1)
  };//调制度
  cv::Mat grating_img[13];
  cv::Mat grating_img_dis[13];  //增加畸变矫正图
  getCameraParamFile();       //获取相机的参数




  for (int img_num = 1;img_num < 13;++img_num)//12张照片，
  {
    //修改前  小畸变镜头可采用这句
    //cv::cvtColor(grating_img1[img_num], grating_img[img_num], cv::COLOR_RGB2GRAY);
    //cv::medianBlur(grating_img[img_num], grating_img[img_num], 3);
    //cv::blur(grating_img[img_num], grating_img1[img_num], cv::Size(5, 5));
    //undistort(grating_img1[img_num], grating_img[img_num], camera_mat, distcoeffs_mat);
    
    //修改后  针对畸变较大的镜头要进行畸变的矫正
    //undistort函数：https://blog.csdn.net/qq_30815237/article/details/87622654
    //undistort参数：//输入原图；//输出矫正后的图像；//内参矩阵；//畸变系数
    undistort(grating_img1[img_num], grating_img_dis[img_num], camera_mat, distcoeffs_mat);
    //转变为灰度图
    cv::cvtColor(grating_img_dis[img_num], grating_img[img_num], cv::COLOR_RGB2GRAY);
  }
  //13923456892
  /*cv::namedWindow("fai_img[0]", 0);
  cv::resizeWindow("fai_img[0]", cv::Size(800, 600));
  cv::imshow("fai_img[0]", grating_img[2]);*/
  int thresh_shadow = 4;
  uchar*pb_img[3] = { NULL };
  uchar*pshadow_img;
  uchar*p[13] = { NULL };
  //遍历行，1200行
  for (int y = 0;y < grating_rows;++y)
  {
    //for循环，得到12个图片，每个图片的i行的像素值；
    for (int img_num = 1;img_num < 13;++img_num)
    {
      //cv::medianBlur(grating_img[img_num], grating_img[img_num], 3);
      //说明：https://blog.csdn.net/xxhjxx/article/details/116013899
      //.ptr<>（）返回的是一个地址    .ptr<uchar>（）是获取像素值的函数
      p[img_num] = grating_img[img_num].ptr<uchar>(y);
    }
    //图片的第y行的调制度像素值，为什么是3个b_img，因为是3个频率
    pb_img[0] = b_img[0].ptr<uchar>(y);
    pb_img[1] = b_img[1].ptr<uchar>(y);
    pb_img[2] = b_img[2].ptr<uchar>(y);
    //说明：cv::Mat shadowflag_img = cv::Mat::zeros(grating_rows, grating_cols, CV_8UC1);
    //uchar*pshadow_img;
    pshadow_img = shadowflag_img.ptr<uchar>(y);
    //grating_cols=1600
    for (int x = 0;x < grating_cols;++x)
    {
      //说明.at<double>(y, x) .at<>() 返回的是一个值，
      

      //第94行，是定位到行，p[4][x]意思是第四幅图片的x列；
      //每个频率，投射4个图片，四步相移，
      //频率1的四步相移，求出fai0//fai=atan2[(I4-I2)/(I1-I3)]
      double fai_temp1 = atan2((double)(p[4][x] - p[2][x]), (double)(p[1][x] - p[3][x]));//3-1 0-2
      if (fai_temp1 >= 0)
        fai_phase[0].at<double>(y, x) = fai_temp1;//0-2pi之间，相位，放在fai_phase[0].at<double>(行, 列)里面
      else 
        fai_phase[0].at<double>(y, x) = fai_temp1 + CV_PI * 2;
      //频率2的四步相移，求出fai1
      double fai_temp2 = atan2((double)(p[8][x] - p[6][x]), (double)(p[5][x] - p[7][x]));
      if (fai_temp2 >= 0)
        fai_phase[1].at<double>(y, x) = fai_temp2;
      else 
        fai_phase[1].at<double>(y, x) = fai_temp2 + CV_PI * 2;
      //频率3的四步相移，求出fai2
      double fai_temp3 = atan2((double)(p[12][x] - p[10][x]), (double)(p[9][x] - p[11][x]));
      if (fai_temp3 >= 0)
        fai_phase[2].at<double>(y, x) = fai_temp3;
      else 
        fai_phase[2].at<double>(y, x) = fai_temp3 + CV_PI * 2;
      

      //0-2pi之间，相位，转为灰度值
      fai_img[0].at<uchar>(y, x) = fai_phase[0].at<double>(y, x) / (2 * CV_PI) * 255;
      fai_img[1].at<uchar>(y, x) = fai_phase[1].at<double>(y, x) / (2 * CV_PI) * 255;
      fai_img[2].at<uchar>(y, x) = fai_phase[2].at<double>(y, x) / (2 * CV_PI) * 255;


      //调制度计算
      //移除不可靠区域，为什么要移开？背后的原理是什么
      //这个为什么是0.5，是因为2/N的N=4，那么就是2/4=0.5
      //B0=2/N   *sqrt((I0sin0+I1sin90+I2sin180+I3sin270)^2 + (I0cos0+I1cos90+I2cos180+I3cos270)^2)
      pb_img[0][x] = 0.5 * sqrt(pow(p[1][x] - p[3][x], 2) + pow(p[2][x] - p[4][x], 2));
      pb_img[1][x] = 0.5 * sqrt(pow(p[5][x] - p[7][x], 2) + pow(p[6][x] - p[8][x], 2));
      pb_img[2][x] = 0.5 * sqrt(pow(p[9][x] - p[11][x], 2) + pow(p[10][x] - p[12][x], 2));
      //滤波thresh_shadow=4
      if (pb_img[0][x] <= thresh_shadow || pb_img[1][x] <= thresh_shadow || pb_img[2][x] <= thresh_shadow)
        pshadow_img[x] = 0;
      else 
        pshadow_img[x] = 255;    
    }
  }
  /*cv::namedWindow("fai_img[0]", 0);
  cv::resizeWindow("fai_img[0]", cv::Size(800,600));
  cv::imshow("fai_img[0]", fai_img[0]);
  cv::namedWindow("fai_img[1]", 0);
  cv::resizeWindow("fai_img[1]", cv::Size(800,600));
  cv::imshow("fai_img[1]", fai_img[1]);
  cv::namedWindow("fai_img[2]", 0);
  cv::resizeWindow("fai_img[2]", cv::Size(800,600));
  cv::imshow("fai_img[2]", fai_img[2]);*/
  cv::imwrite("fai_img[0].bmp", fai_img[0]);
  /*cv::namedWindow("shadow_img", 0);
  cv::resizeWindow("shadow_img", cv::Size(800, 600));
  cv::imshow("shadow_img", shadowflag_img);
  cv::waitKey(50);*/
}

//单词heterodyne :外差法；外差作用；外差振荡器；外差（电子战用语）;
void Grating::threeFrequencyHeterodyneImproved()
{
  //cv::Mat FAI_phase_img = cv::Mat::zeros(grating_rows, grating_cols, CV_8UC1);
  cv::Mat FAI_phase12_23[2] = { cv::Mat::zeros(grating_rows, grating_cols, CV_64FC1),cv::Mat::zeros(grating_rows, grating_cols, CV_64FC1) };
  cv::Mat FAI_phase_blur = cv::Mat::zeros(grating_rows, grating_cols, CV_32F);
  double deta_n12 = 0.0;
  double deta_n23 = 0.0;
  double deta_n123 = 0.0;
  double maxfai = 0.0;
  //ofstream outfile1("FAI_phase.txt");
  for (int y = 0;y < grating_rows;++y)
  {
    for (int x = 0;x < grating_cols;++x)
    {
      double n1_n2 = (fai_phase[0].at<double>(y, x) - fai_phase[1].at<double>(y, x)) / (2 * CV_PI);
      if (n1_n2 >= 0)
      {
        deta_n12 = n1_n2;
      }
      else
      {
        deta_n12 = n1_n2 + 1;
      }

      double n2_n3 = (fai_phase[1].at<double>(y, x) - fai_phase[2].at<double>(y, x)) / (2 * CV_PI);
      if (n2_n3 >= 0)
      {
        deta_n23 = n2_n3;
      }
      else
      {
        deta_n23 = n2_n3 + 1;
      }

      double n12_n23 = deta_n12 - deta_n23;
      if (n12_n23 >= 0)
      {
        deta_n123 = n12_n23;
      }
      else
      {
        deta_n123 = n12_n23 + 1;
      }

      FAI_phase12_23[0].at<double>(y, x) = 2 * CV_PI*round(p23*(0.0 + deta_n123) / (p23 - p12) - deta_n12) + 2 * CV_PI*deta_n12;
      FAI_phase12_23[1].at<double>(y, x) = 2 * CV_PI*round(p12*(0.0 + deta_n123) / (p23 - p12) - deta_n23) + 2 * CV_PI*deta_n23;
      //if (shadowflag_img.at<uchar>(y,x) ==255 )
      FAI_phase.at<double>(y, x) = 1.0 / p1*(2 * CV_PI / 3 * p1*round((p12*FAI_phase12_23[0].at<double>(y, x) +
                    p23*FAI_phase12_23[1].at<double>(y, x)) / (4 * CV_PI*p1) - fai_phase[0].at<double>(y, x) / (2 * CV_PI)) +
                    1 / 3.0*p1*fai_phase[0].at<double>(y, x)) +
                    1.0 / p1*(2 * CV_PI / 3 * p2*round((p12*FAI_phase12_23[0].at<double>(y, x) +
                    p23*FAI_phase12_23[1].at<double>(y, x)) / (4 * CV_PI*p2) - fai_phase[1].at<double>(y, x) / (2 * CV_PI)) +
                    1 / 3.0*p2*fai_phase[1].at<double>(y, x)) +
                    1.0 / p1*(2 * CV_PI / 3 * p3*round((p12*FAI_phase12_23[0].at<double>(y, x) +
                    p23*FAI_phase12_23[1].at<double>(y, x)) / (4 * CV_PI*p3) - fai_phase[2].at<double>(y, x) / (2 * CV_PI)) +
                    1 / 3.0*p3*fai_phase[2].at<double>(y, x));
      maxfai = std::max(maxfai, (FAI_phase.at<double>(y, x)));
      //outfile1 << FAI_phase.at<double>(y, x) << " ";
    }
    //outfile1 << endl;
  }
  for (int y = 0;y < FAI_phase.rows;++y)
  {
    uchar*p_img3 = FAI_phase_img.ptr<uchar>(y);
    for (int x = 0;x < FAI_phase.cols;++x)
    {
      p_img3[x] = (FAI_phase.at<double>(y, x)) / maxfai * 255;
      
      //if (y == 0)cout << FAI_phase.at<double>(y, x) << endl;
    }
  }
  //cv::namedWindow("FAI_phase_img", 0);
  //cv::resizeWindow("FAI_phase_img", cv::Size(grating_cols / 2, grating_rows / 2));
  //cv::imshow("FAI_phase_img", FAI_phase_img);
  cv::imwrite("FAI_phase_img.bmp", FAI_phase_img);
  //cv::waitKey(100);
}

std::vector<std::vector<double>>
Grating::threeFrequencyHeterodyneImproved(std::vector<cv::Point2f> cornerpoints_temp, cv::Mat grating_img[])
{
  /*cv::Mat camera_mat, distcoeffs_mat;
  getCameraParamFile(camera_mat, distcoeffs_mat);*/
  //cv::undistort(grating_imgs_before[i][j], grating_imgs[i][j], camera_mat, distcoeffs_mat);
  //四步相移，三频外差
  fourStepPhaseShifting(grating_img);
  threeFrequencyHeterodyneImproved();
  //求cornerpoints对应的theta样本
  std::vector<double> samples_FAI_temp;
  for (int cornerpoints_num = 0;cornerpoints_num < cornerpoints_temp.size();++cornerpoints_num)
  {
    double temp = FAI_phase.at<double>(cornerpoints_temp[cornerpoints_num].y, cornerpoints_temp[cornerpoints_num].x);
    samples_FAI_temp.push_back(temp);
    /*cv::circle(FAI_phase_img, cv::Point(cornerpoints_temp[cornerpoints_num].x, cornerpoints_temp[cornerpoints_num].y), 10,
    cv::Scalar(255, 255, 255), 5);*/
  }
  samples_FAI.push_back(samples_FAI_temp);
  /*cv::namedWindow("FAI_phase_img", 0);
  cv::resizeWindow("FAI_phase_img", cv::Size(grating_cols / 2, grating_rows / 2));
  cv::imshow("FAI_phase_img", FAI_phase_img);
  cv::imwrite("FAI_phase_img.bmp", FAI_phase_img);*/
  //cv::waitKey(10);
  return samples_FAI;
}