|
@@ -0,0 +1,251 @@
|
|
|
+//
|
|
|
+// 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;
|
|
|
+
|
|
|
+}
|