zx vor 2 Jahren
Ursprung
Commit
2b4e5f5019

+ 3 - 6
CMakeLists.txt

@@ -25,17 +25,14 @@ link_directories("/usr/local/lib")
 
 
 
-add_executable( triangle_measure main.cpp StripCenter/Steger.cpp PnP/pnp.cpp)
+add_executable( triangle_measure main.cpp StripCenter/Steger.cpp PnP/pnp.cpp Interpolator.cpp)
 target_link_libraries( triangle_measure
 		${OpenCV_LIBS}
 		)
 
-
-add_executable(sample_calib_all sample_calib_all.cpp ParametersSolver.cpp
-		)
-target_link_libraries( sample_calib_all
+add_executable( caliber camera_calib/caliber.cpp)
+target_link_libraries( caliber
 		${OpenCV_LIBS}
-		${CERES_LIBRARIES}
 		)
 
 

+ 251 - 0
Interpolator.cpp

@@ -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;
+
+}

+ 32 - 0
Interpolator.h

@@ -0,0 +1,32 @@
+//
+// Created by zx on 22-12-18.
+//
+
+#ifndef TRIANGLE_MEASURE__CALIBER_H_
+#define TRIANGLE_MEASURE__CALIBER_H_
+#include "Steger.h"
+
+typedef struct {
+  double x;
+  double y;
+  double z;
+}CalibPoint;
+class Interpolator
+{
+ public:
+    Interpolator();
+    void AddImage(cv::Mat image,double distance);
+    cv::Mat Calib3();
+    cv::Mat Calib4();
+
+    static std::vector<cv::Point3d> predict3(cv::Mat image,cv::Mat cameraMatrix, cv::Mat distCoeffs,cv::Mat P);
+    static std::vector<cv::Point3d> predict4(cv::Mat image,cv::Mat cameraMatrix, cv::Mat distCoeffs,cv::Mat P);
+    static cv::Mat undistored_image(cv::Mat image,cv::Mat cameraMatrix, cv::Mat distCoeffs);
+
+ protected:
+    std::vector<CalibPoint> samples_;
+
+};
+
+
+#endif //TRIANGLE_MEASURE__CALIBER_H_

+ 13 - 0
Readme.md

@@ -0,0 +1,13 @@
+
+
+在steger目录下,右键打开控制台
+输入以下命令:
+mkdir build
+cd build
+cmake ..
+make -j2
+
+若无红字报错,输入以下命令测试:
+./triangle_measure
+
+控制台会显示时间,单位ms

+ 0 - 14
PnP/TimeRecord.cpp

@@ -7,20 +7,6 @@
 std::map<std::string, TimeRecord::Record> TimeRecord::records_=std::map<std::string, TimeRecord::Record>();
 
 
-template <class F>
-void TimeRecord::Evaluate(F&& func, const std::string& func_name)
-{
-    auto t1 = std::chrono::high_resolution_clock::now();
-    std::forward<F>(func)();
-    auto t2 = std::chrono::high_resolution_clock::now();
-    auto time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count() * 1000;
-
-    if (records_.find(func_name) != records_.end()) {
-        records_[func_name].time_usage_in_ms_.emplace_back(time_used);
-    } else {
-        records_.insert({func_name, Record(func_name, time_used)});
-    }
-}
 void TimeRecord::PrintAll()
 {
     std::cout <<">> ===== Printing run time ====="<<std::endl;

+ 13 - 1
PnP/TimeRecord.h

@@ -26,7 +26,19 @@ class TimeRecord
     };
 
     template <class F>
-    static void Evaluate(F&& func, const std::string& func_name);
+    inline static void Evaluate(F&& func, const std::string& func_name)
+    {
+      auto t1 = std::chrono::high_resolution_clock::now();
+      std::forward<F>(func)();
+      auto t2 = std::chrono::high_resolution_clock::now();
+      auto time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count() * 1000;
+
+      if (records_.find(func_name) != records_.end()) {
+        records_[func_name].time_usage_in_ms_.emplace_back(time_used);
+      } else {
+        records_.insert({func_name, Record(func_name, time_used)});
+      }
+    }
     static void PrintAll();
     static void DumpIntoFile(const std::string& file_name);
     static double GetMeanTime(const std::string& func_name);

+ 59 - 9
camera_calib/caliber.cpp

@@ -8,8 +8,47 @@
 using namespace cv;
 using namespace std;
 
+cv::Mat undistored_image(cv::Mat image,Mat cameraMatrix, 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;
+
+}
+
 Mat image, img_gray;
-int BOARDSIZE[2]{ 6,9 };//棋盘格每行每列角点个数
+int BOARDSIZE[2]{ 8,11 };//棋盘格每行每列角点个数
 int main()
 {
     vector<vector<Point3f>> objpoints_img;//保存棋盘格上角点的三维坐标
@@ -18,7 +57,7 @@ int main()
     vector<Point2f> img_corner_points;//保存每张图检测到的角点
     vector<String> images_path;//创建容器存放读取图像路径
 
-    string image_path = "/home/titan/Calibration/image/pictures/*.jpg";//待处理图路径
+    string image_path = "/home/zx/doc/private_hub/triangle/images/标定数据/上方/all/*.bmp";//待处理图路径
     glob(image_path, images_path);//读取指定文件夹下图像
 
     //转世界坐标系
@@ -32,8 +71,9 @@ int main()
 
     for (int i = 0; i < images_path.size(); i++)
     {
-        image = imread(images_path[i]);
-        cvtColor(image, img_gray, COLOR_BGR2GRAY);
+        image = imread(images_path[i],0);
+        img_gray=image;
+        //cvtColor(image, img_gray, COLOR_BGR2GRAY);
         //检测角点
         bool found_success = findChessboardCorners(img_gray, Size(BOARDSIZE[0], BOARDSIZE[1]),
                                                    img_corner_points,
@@ -59,8 +99,8 @@ int main()
         //char *output = "image";
         char text[] = "image";
         char *output = text;
-        imshow(output, image);
-        waitKey(200);
+        //imshow(output, image);
+       // waitKey(0);
 
     }
 
@@ -87,11 +127,16 @@ int main()
     cout << "Translation vector:" << endl;
     cout << T << endl;
 
+  FileStorage fwrite("../cfg/cameraMatrix.yaml", FileStorage::WRITE);
+  //存入矩阵Mat类型的数据
+   fwrite.write("cameraMatrix", cameraMatrix);
+  fwrite.write("distCoeffs", distCoeffs);
+
     ///*
     //畸变图像校准
     //*/
     Mat src, dst;
-    src = imread("/home/titan/Calibration/image/pictures/02.jpg");  //读取校正前图像
+    src = imread("/home/zx/doc/private_hub/triangle/images/标定数据/上方/all/down_1.bmp",0);  //读取校正前图像
     undistort(src, dst, cameraMatrix, distCoeffs);
 
     char texts[] = "image_dst";
@@ -99,9 +144,14 @@ int main()
     //char *dst_output = "image_dst";
     imshow(dst_output, dst);
     waitKey(100);
-    imwrite("/home/titan/Calibration/image/pictures/002.jpg", dst);  //校正后图像
+    imwrite("/home/zx/doc/private_hub/triangle/images/undistored.bmp", dst);  //校正后图像
+
+    cv::Mat undist=undistored_image(src,cameraMatrix,distCoeffs);
+    imwrite("/home/zx/doc/private_hub/triangle/images/undistored_1.bmp", undist);  //校正后图像
+
+    cv::Mat diff=undist-dst;
+    imwrite("/home/zx/doc/private_hub/triangle/images/diff.bmp", diff);  //
 
     destroyAllWindows();//销毁显示窗口
-    system("pause");
     return 0;
 }

+ 14 - 0
cfg/P.yaml

@@ -0,0 +1,14 @@
+%YAML:1.0
+---
+P: !!opencv-matrix
+   rows: 15
+   cols: 1
+   dt: d
+   data: [ -6.1818650701199462e-05, -7.0167254415458956e+01,
+       4.7572922472647328e-03, -1.6725643290210002e-01,
+       2.2278754014236029e+00, -4.4814700111472927e-03,
+       2.8759879117657692e+02, 2.9843363085900165e-01,
+       -6.2866938531158212e+00, -1.3875611875528016e-01,
+       -4.3878364713609335e+02, 5.9508171930035587e+00,
+       -1.8891206579330060e+00, 2.9710090321241296e+02,
+       -7.4844815149057467e+01 ]

+ 15 - 0
cfg/cameraMatrix.yaml

@@ -0,0 +1,15 @@
+%YAML:1.0
+---
+cameraMatrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1.7771710027577881e+03, 0., 1.2490548089854140e+03, 0.,
+       1.7785985821846932e+03, 1.0257699548418632e+03, 0., 0., 1. ]
+distCoeffs: !!opencv-matrix
+   rows: 1
+   cols: 5
+   dt: d
+   data: [ -7.5134513315641208e-02, 1.3082907767897761e-01,
+       -8.0272317431114622e-04, 1.2368579605171203e-03,
+       -2.0189303265863941e-02 ]

+ 90 - 2
main.cpp

@@ -5,6 +5,7 @@
 #include "Steger.h"
 #include "pnp.h"
 #include "Steger.h"
+#include "Caliber.h"
 #include <chrono>
 
 void test_pnp(){
@@ -46,10 +47,97 @@ void test_pnp(){
 
 
 }
-int main()
+
+void pnpcaliber_test()
 {
+  cv::FileStorage fread("../cfg/cameraMatrix.yaml", cv::FileStorage::READ);
+  //判断是否成功打开文件
+  if (!fread.isOpened())
+    {
+      std::cout << "打开文件失败,请确认文件名称是否正确!" << std::endl;
+      return ;
+    }
+
+  //读取文件中的数据
+  cv::Mat K,distCoeffs;
+  fread["cameraMatrix"] >> K;
+  fread["distCoeffs"] >> distCoeffs;
+
+  cv::FileStorage Pread("../cfg/P.yaml", cv::FileStorage::READ);
+  //判断是否成功打开文件
+  if (!Pread.isOpened())
+  {
+    std::cout << "打开文件失败,请确认文件名称是否正确!" << std::endl;
+    return ;
+  }
+  cv::Mat P;
+  Pread["P"] >> P;
+
+  cv::Mat src7= cv::imread("../images/calib/160120.png", 0);
+  std::vector<cv::Point3d> pts3d=Interpolator::predict4(src7,K,distCoeffs,P);
+
+  FILE* pf=fopen("../images/pred3d.txt","w");
+  for(int i=0;i<pts3d.size();++i)
+  {
+    fprintf(pf,"%f %f %f\n",pts3d[i].x,pts3d[i].y,pts3d[i].z);
+    printf("%.3f %.3f %.5f\n",pts3d[i].x,pts3d[i].y,pts3d[i].z);
+  }
+  fclose(pf);
+
+}
+
+void pnpcalibe()
+{
+  cv::FileStorage fread("../cfg/cameraMatrix.yaml", cv::FileStorage::READ);
+  //判断是否成功打开文件
+  if (!fread.isOpened())
+  {
+    std::cout << "打开文件失败,请确认文件名称是否正确!" << std::endl;
+    return ;
+  }
+
+  //读取文件中的数据
+  cv::Mat K,distCoeffs;
+  fread["cameraMatrix"] >> K;
+  fread["distCoeffs"] >> distCoeffs;
+  Interpolator caliber;
+
+  cv::Mat src1 = cv::imread("../images/calib/0.png", 0);
+
+  caliber.AddImage(Interpolator::undistored_image(src1,K,distCoeffs), 0.8);
+
+  cv::Mat src2 = cv::imread("../images/calib/40940.png", 0);
+  caliber.AddImage(Interpolator::undistored_image(src2,K,distCoeffs), 0.840940);
 
-    test_pnp();
+  cv::Mat src3 = cv::imread("../images/calib/80975.png", 0);
+  caliber.AddImage(Interpolator::undistored_image(src3,K,distCoeffs), 0.880970);
+
+  cv::Mat src4 = cv::imread("../images/calib/119645.png", 0);
+  caliber.AddImage(Interpolator::undistored_image(src4,K,distCoeffs), 0.919645);
+
+  cv::Mat src5 = cv::imread("../images/calib/199580.png", 0);
+  caliber.AddImage(Interpolator::undistored_image(src5,K,distCoeffs), 0.999580);
+
+  cv::Mat src6 = cv::imread("../images/calib/240370.png", 0);
+  caliber.AddImage(Interpolator::undistored_image(src6,K,distCoeffs), 1.040370);
+
+  cv::Mat P = caliber.Calib4();
+
+
+  cv::FileStorage fwrite("../cfg/P.yaml", cv::FileStorage::WRITE);
+  fwrite.write("P", P);
+
+
+  std::cout << P.t() << std::endl;
+}
+
+#include <list>
+int main()
+{
+  pnpcaliber_test();
+  //pnpcalibe();
+  return 0;
+    //test_pnp();
     /*Mat image = cv::imread(file, 0);
     	Mat image = src(Rect(0, 800, src.cols, 500));
         for (int i = 0; i < image.rows; i++)

+ 1 - 1
ParametersSolver.cpp

@@ -74,7 +74,7 @@ class CostFunctor{
             residual[2*i+1]=ny*fy/Z+cy-iy;
         }
 
-
+    return true;
     }
 };
 

+ 10 - 0
ParametersSolver.h

@@ -16,5 +16,15 @@ class ParametersSolver
 
 };
 
+class TransformByDiffSolver
+{
+ public:
+    TransformByDiffSolver();
+    ~TransformByDiffSolver();
+
+    void Solve(std::vector<cv::Point2d> ixy,std::vector<cv::Point3d> wpt);
+
+};
+
 
 #endif //TRIANGLE_MEASURE__PARAMETERSSOLVER_H_

pose_estimation_3d2d.cpp → solver/pose_estimation_3d2d.cpp


sample_calib_all.cpp → solver/sample_calib_all.cpp