add calib all

CMakeLists.txt

@@ -6,7 +6,7 @@ add_compile_options(-std=c++11)
 
 find_package(Eigen3 REQUIRED)
 FIND_PACKAGE(OpenCV REQUIRED)
-
+FIND_PACKAGE(Ceres REQUIRED)
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wl,--no-as-needed")
 set(CMAKE_BUILD_TYPE "RELEASE")
@@ -18,16 +18,24 @@ include_directories(
 		PnP
 		StripCenter
 		{EIGEN3_INCLUDE_DIR}
+		${CERES_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
 )
 link_directories("/usr/local/lib")
 
 
 
-
 add_executable( triangle_measure main.cpp StripCenter/Steger.cpp PnP/pnp.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
+		${OpenCV_LIBS}
+		${CERES_LIBRARIES}
+		)
+
+

ParametersSolver.cpp

@@ -0,0 +1,147 @@
+//
+// Created by zx on 22-11-9.
+//
+
+#include "ParametersSolver.h"
+#include "ceres/ceres.h"
+#include <chrono>
+class CostFunctor{
+ public:
+    std::vector<cv::Point2d> ixy_;
+    std::vector<cv::Point3d> wpt_;
+ public:
+    CostFunctor(std::vector<cv::Point2d> ixy,std::vector<cv::Point3d> wpt)
+    {
+        ixy_=ixy;
+        wpt_=wpt;
+    }
+    template <typename T>
+    bool operator()(const T* const value, T* residual) const
+    {
+        if (ixy_.size()!=wpt_.size())
+            printf("Error : ixy_.size()!=wpt_.size() \n");
+        //参数拜访顺序：R P Y tx ty tz fx fy cx cy k1 k2 k3 r 共14个参数
+        T R=value[0];
+        T P=value[1];
+        T Y=value[2];
+        T tx=value[3];
+        T ty=value[4];
+        T tz=value[5];
+
+        T fx=value[6];
+        T fy=value[7];
+
+        T cx=value[8];
+        T cy=value[9];
+
+        T k1=value[10];
+        T k2=value[11];
+        T p1=value[12];
+        T p2=value[13];
+        T k3=value[14];
+
+
+        Eigen::AngleAxis<T> rollAngle(Eigen::AngleAxis<T>(R,Eigen::Matrix<T,3,1>::UnitX()));
+        Eigen::AngleAxis<T> pitchAngle(Eigen::AngleAxis<T>(P,Eigen::Matrix<T,3,1>::UnitY()));
+        Eigen::AngleAxis<T> yawAngle(Eigen::AngleAxis<T>(Y,Eigen::Matrix<T,3,1>::UnitZ()));
+
+        Eigen::Matrix<T,3,3> rotation_matrix;
+        rotation_matrix=yawAngle*pitchAngle*rollAngle;
+
+
+        //每个样本，产生2n维残差项
+
+        for (int i=0;i<ixy_.size();++i)
+        {
+            Eigen::Matrix<T,3,1> wp;
+            wp[0]=T(wpt_[i].x)-tx;
+            wp[1]=T(wpt_[i].y)-ty;
+            wp[2]=T(wpt_[i].z)-tz;
+
+            Eigen::Matrix<T,3,1> Rwp=rotation_matrix.inverse()*wp;
+            T X=Rwp[0];
+            T Y=Rwp[1];
+            T Z=Rwp[2];
+
+            T ix=T(ixy_[i].x);
+            T iy=T(ixy_[i].y);
+
+            T rr=X*X+Y*Y;
+            T nx=X*(T(1.0)+k1*rr+k2*rr*rr+k3*rr*rr*rr) + T(2.0)*p1*X*Y + p2*(rr+T(2.0)*X*X);
+            T ny=Y*(T(1.0)+k1*rr+k2*rr*rr+k3*rr*rr*rr) + p1*(rr+T(2.0)*Y*Y) + T(2.0)*p2*X*Y;
+
+            residual[2*i]=nx*fx/Z+cx-ix;
+            residual[2*i+1]=ny*fy/Z+cy-iy;
+        }
+
+
+    }
+};
+
+ParametersSolver::ParametersSolver()
+{
+
+}
+ParametersSolver::~ParametersSolver()
+{
+
+}
+
+void ParametersSolver::Solve(std::vector<cv::Point2d> ixy,std::vector<cv::Point3d> wpt)
+{
+    if(ixy.size()!=wpt.size())
+    {
+        printf("Error : ixy.size()!=wpt.size()\n");
+        return ;
+    }
+    // 寻优参数x的初始值
+    auto t0 = std::chrono::steady_clock::now();
+
+    double R=0;
+    double P=0;
+    double Y=0;
+    double tx=0;
+    double ty=0;
+    double tz=0;
+
+    double fx=0;
+    double fy=0;
+
+    double cx=0;
+    double cy=0;
+
+    double k1=0;
+    double k2=0;
+    double p1=0;
+    double p2=0;
+    double k3=0;
+
+    const int NUM=15;
+    double x[NUM]={R,P,Y,tx,ty,tz,fx,fy,cx,cy,k1,k2,p1,p2,k3};
+
+    const int samples=ixy.size();
+
+    // 第二部分：构建寻优问题
+    ceres::Problem problem;
+    //使用自动求导，将之前的代价函数结构体传入，残差维度2n，参数x的维度NUM。
+    ceres::CostFunction* cost_function =new ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, NUM>(
+            new CostFunctor(ixy,wpt),2*samples);
+    problem.AddResidualBlock(cost_function, NULL, x); //向问题中添加误差项
+
+    //第三部分： 配置并运行求解器
+    ceres::Solver::Options options;
+    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
+    options.minimizer_progress_to_stdout = true;//输出到cout
+    ceres::Solver::Summary summary;//优化信息
+    ceres::Solve(options, &problem, &summary);//求解!!!
+
+    auto t1 = std::chrono::steady_clock::now();
+
+    std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
+
+    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0);
+    double time=double(duration.count()) * std::chrono::microseconds::period::num /
+            std::chrono::microseconds::period::den;
+    std::cout << "time : "<<time<<std::endl;
+
+}

ParametersSolver.h

@@ -0,0 +1,20 @@
+//
+// Created by zx on 22-11-9.
+//
+
+#ifndef TRIANGLE_MEASURE__PARAMETERSSOLVER_H_
+#define TRIANGLE_MEASURE__PARAMETERSSOLVER_H_
+
+#include <opencv2/opencv.hpp>
+
+class ParametersSolver
+{
+ public:
+    ParametersSolver();
+    ~ParametersSolver();
+    void Solve(std::vector<cv::Point2d> ixy,std::vector<cv::Point3d> wpt);
+
+};
+
+
+#endif //TRIANGLE_MEASURE__PARAMETERSSOLVER_H_

sample_calib_all.cpp

@@ -0,0 +1,74 @@
+//
+// Created by zx on 22-11-9.
+//
+
+#include <iostream>
+
+#include "ParametersSolver.h"
+#include "ceres/ceres.h"
+
+std::vector<cv::Point2d> generate_2dby3d(std::vector<cv::Point3d> wpts,double* value)
+{
+
+    double R=value[0];
+    double P=value[1];
+    double Y=value[2];
+    double tx=value[3];
+    double ty=value[4];
+    double tz=value[5];
+
+    double fx=value[6];
+    double fy=value[7];
+
+    double cx=value[8];
+    double cy=value[9];
+
+    double k1=value[10];
+    double k2=value[11];
+    double p1=value[12];
+    double p2=value[13];
+    double k3=value[14];
+
+    std::vector<cv::Point2d> rets;
+
+    Eigen::AngleAxis<double> rollAngle(Eigen::AngleAxisd(R,Eigen::Matrix<double,3,1>::UnitX()));
+    Eigen::AngleAxis<double> pitchAngle(Eigen::AngleAxisd(P,Eigen::Matrix<double,3,1>::UnitY()));
+    Eigen::AngleAxis<double> yawAngle(Eigen::AngleAxisd(Y,Eigen::Matrix<double,3,1>::UnitZ()));
+
+    Eigen::Matrix<double,3,3> rotation_matrix;
+    rotation_matrix=yawAngle*pitchAngle*rollAngle;
+
+    for(int i=0;i<wpts.size();++i)
+    {
+        Eigen::Matrix<double,3,1> wp;
+        wp[0]=wpts[i].x-tx;
+        wp[1]=wpts[i].y-ty;
+        wp[2]=wpts[i].z-tz;
+
+        Eigen::Matrix<double,3,1> Rwp=rotation_matrix.inverse()*wp;
+        double X=Rwp[0];
+        double Y=Rwp[1];
+        double Z=Rwp[2];
+
+        double rr=X*X+Y*Y;
+        double nx=X*(1.0+k1*rr+k2*rr*rr+k3*rr*rr*rr) + 2.0*p1*X*Y + p2*(rr+2.0*X*X);
+        double ny=Y*(1.0+k1*rr+k2*rr*rr+k3*rr*rr*rr) + p1*(rr+2.0*Y*Y) + 2.0*p2*X*Y;
+
+        cv::Point2d pt2d(nx*fx/Z+cx,ny*fy/Z+cy);
+        rets.push_back(pt2d);
+
+    }
+    return rets;
+}
+
+int main()
+{
+    std::vector<cv::Point2d> pt2ds;
+    std::vector<cv::Point3d> pt3ds;
+
+
+
+    ParametersSolver solver;
+    solver.Solve(pt2ds,pt3ds);
+    return 0;
+}