triangle/Interpolator.cpp

//
// Created by zx on 22-12-18.
//

#include "Interpolator.h"


Interpolator::Interpolator(){}
void Interpolator::AddImage(cv::Mat src,double distance)
{
  CSteger steger;
  cv::Mat image = src(cv::Rect(0, 0, src.cols, src.rows)).clone();
  for (int i = 0; i < image.rows; i++)
  {
    for (int j = 0; j < image.cols; j++)
    {
      if (image.at<uchar>(i, j) < 150)
        image.at<uchar>(i, j) = 0;
    }
  }
  cv::GaussianBlur(image, image, cv::Size(3, 3), 3, 3);
  std::vector<cv::Point2d> points = steger.StripCenter(image);
  for(int i=0;i<points.size();++i)
  {
    CalibPoint pt;
    pt.x=points[i].x;
    pt.y=points[i].y;
    pt.z=distance;
    samples_.push_back(pt);
  }

}
cv::Mat Interpolator::Calib3()
{
  cv::Mat A=cv::Mat::zeros(samples_.size(),10,CV_64FC1);
  cv::Mat B=cv::Mat::zeros(samples_.size(),1,CV_64FC1);
  for(int i=0;i<samples_.size();++i)
  {
    CalibPoint pt=samples_[i];
    double x=pt.x/1000.0;
    double y=pt.y/1000.0;
    double z=pt.z;

    A.at<double>(i,0)=pow(x,3);
    A.at<double>(i,1)=pow(y,3);
    A.at<double>(i,2)=x*x*y;
    A.at<double>(i,3)=x*y*y;
    A.at<double>(i,4)=x*x;
    A.at<double>(i,5)=y*y;
    A.at<double>(i,6)=x*y;
    A.at<double>(i,7)=x;
    A.at<double>(i,8)=y;
    A.at<double>(i,9)=1;

    B.at<double>(i,0)=z;

  }

  cv::Mat P=(A.t()*A).inv()*A.t()*B;
  return P;

}

cv::Mat Interpolator::Calib4()
{
  cv::Mat A=cv::Mat::zeros(samples_.size(),15,CV_64FC1);
  cv::Mat B=cv::Mat::zeros(samples_.size(),1,CV_64FC1);
  for(int i=0;i<samples_.size();++i)
  {
    CalibPoint pt=samples_[i];
    double x=pt.x/1000.0;
    double y=pt.y/1000.0;
    double z=pt.z;

    A.at<double>(i,0)=pow(x,4);
    A.at<double>(i,1)=pow(y,4);
    A.at<double>(i,2)=x*x*x*y;
    A.at<double>(i,3)=x*x*y*y;
    A.at<double>(i,4)=x*y*y*y;

    A.at<double>(i,5)=pow(x,3);
    A.at<double>(i,6)=pow(y,3);
    A.at<double>(i,7)=x*x*y;
    A.at<double>(i,8)=x*y*y;
    A.at<double>(i,9)=x*x;
    A.at<double>(i,10)=y*y;
    A.at<double>(i,11)=x*y;
    A.at<double>(i,12)=x;
    A.at<double>(i,13)=y;
    A.at<double>(i,14)=1;

    B.at<double>(i,0)=z;

  }

  cv::Mat P=(A.t()*A).inv()*A.t()*B;
  return P;

}

cv::Mat Interpolator::undistored_image(cv::Mat image,cv::Mat cameraMatrix, cv::Mat distCoeffs)
{
  int rows = image.rows, cols = image.cols;
  cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸变以后的图

  double cx=cameraMatrix.at<double>(0,2);
  double cy=cameraMatrix.at<double>(1,2);
  double fx=cameraMatrix.at<double>(0,0);
  double fy=cameraMatrix.at<double>(1,1);

  double k1=distCoeffs.at<double>(0,0);
  double k2=distCoeffs.at<double>(0,1);
  double p1=distCoeffs.at<double>(0,2);
  double p2=distCoeffs.at<double>(0,3);
  double k3=distCoeffs.at<double>(0,4);

  // 计算去畸变后图像的内容
  for (int v = 0; v < rows; v++) {
    for (int u = 0; u < cols; u++) {
      // 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
      double x = (u - cx) / fx, y = (v - cy) / fy;
      double r = sqrt(x * x + y * y);
      double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r+k3 *r*r* r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
      double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r+k3 *r*r* r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
      double u_distorted = fx * x_distorted + cx;
      double v_distorted = fy * y_distorted + cy;

      // 赋值 (最近邻插值)
      if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
        image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
      } else {
        image_undistort.at<uchar>(v, u) = 0;
      }
    }
  }
  return image_undistort;

}


std::vector<cv::Point3d> Interpolator::predict3(cv::Mat src,cv::Mat cameraMatrix, cv::Mat distCoeffs,cv::Mat P)
{
  //矫正畸变
  cv::Mat image_undistort=undistored_image(src,cameraMatrix,distCoeffs);

  CSteger steger;
  cv::Mat image = image_undistort(cv::Rect(0, 0, image_undistort.cols, image_undistort.rows)).clone();
  for (int i = 0; i < image.rows; i++)
  {
    for (int j = 0; j < image.cols; j++)
    {
      if (image.at<uchar>(i, j) < 150)
        image.at<uchar>(i, j) = 0;
    }
  }
  cv::GaussianBlur(image, image, cv::Size(3, 3), 3, 3);
  std::vector<cv::Point2d> points = steger.StripCenter(image);

  double a0=P.at<double>(0,0);
  double a1=P.at<double>(1,0);
  double a2=P.at<double>(2,0);
  double a3=P.at<double>(3,0);
  double a4=P.at<double>(4,0);
  double a5=P.at<double>(5,0);
  double a6=P.at<double>(6,0);
  double a7=P.at<double>(7,0);
  double a8=P.at<double>(8,0);
  double a9=P.at<double>(9,0);

  double cx=cameraMatrix.at<double>(0,2);
  double cy=cameraMatrix.at<double>(1,2);
  double fx=cameraMatrix.at<double>(0,0);
  double fy=cameraMatrix.at<double>(1,1);

  std::vector<cv::Point3d> pts3d;

  for(int i=0;i<points.size();++i)
  {
    double x=points[i].x/1000.0;
    double y=points[i].y/1000.0;
    double z=a0*pow(x,3)+a1*pow(y,3)+a2*x*x*y+a3*x*y*y+a4*x*x+a5*y*y+a6*x*y
            +a7*x+a8*y+a9;

    //计算x,y
    double rx=(points[i].x-cx)/fx;
    double ry=(points[i].y-cy)/fy;
    pts3d.push_back(cv::Point3d(rx,ry,z));

  }
  return pts3d;

}

std::vector<cv::Point3d> Interpolator::predict4(cv::Mat src,cv::Mat cameraMatrix, cv::Mat distCoeffs,cv::Mat P)
{
  //矫正畸变
  cv::Mat image_undistort=undistored_image(src,cameraMatrix,distCoeffs);

  CSteger steger;
  cv::Mat image = image_undistort(cv::Rect(0, 0, image_undistort.cols, image_undistort.rows)).clone();
  for (int i = 0; i < image.rows; i++)
  {
    for (int j = 0; j < image.cols; j++)
    {
      if (image.at<uchar>(i, j) < 150)
        image.at<uchar>(i, j) = 0;
    }
  }
  cv::GaussianBlur(image, image, cv::Size(3, 3), 3, 3);
  std::vector<cv::Point2d> points = steger.StripCenter(image);

  double a0=P.at<double>(0,0);
  double a1=P.at<double>(1,0);
  double a2=P.at<double>(2,0);
  double a3=P.at<double>(3,0);
  double a4=P.at<double>(4,0);
  double a5=P.at<double>(5,0);
  double a6=P.at<double>(6,0);
  double a7=P.at<double>(7,0);
  double a8=P.at<double>(8,0);
  double a9=P.at<double>(9,0);
  double a10=P.at<double>(10,0);
  double a11=P.at<double>(11,0);
  double a12=P.at<double>(12,0);
  double a13=P.at<double>(13,0);
  double a14=P.at<double>(14,0);


  double cx=cameraMatrix.at<double>(0,2);
  double cy=cameraMatrix.at<double>(1,2);
  double fx=cameraMatrix.at<double>(0,0);
  double fy=cameraMatrix.at<double>(1,1);

  std::vector<cv::Point3d> pts3d;

  for(int i=0;i<points.size();++i)
  {
    double x=points[i].x/1000.0;
    double y=points[i].y/1000.0;
    double z=a0*pow(x,4)+a1*pow(y,4)+a2*x*x*x*y+a3*x*x*y*y+a4*x*y*y*y
            +a5*pow(x,3)+a6*pow(y,3)+a7*x*x*y+a8*x*y*y+a9*x*x+a10*y*y+a11*x*y
            +a12*x+a13*y+a14;

    //计算x,y
    double rx=(points[i].x-cx)/fx;
    pts3d.push_back(cv::Point3d(rx,0,z));

  }
  return pts3d;

}