├── .gitignore
├── 3d_points.txt
├── README.md
├── CMakeLists.txt
├── cmake
    └── FindEigen3.cmake
├── test_dlt.cpp
├── test_epnp.cpp
├── pnp_solver.h
└── pnp_solver.cpp


/.gitignore:
--------------------------------------------------------------------------------
1 | # Default ignored files
2 | /cmake-build-debug
3 | /.idea


--------------------------------------------------------------------------------
/3d_points.txt:
--------------------------------------------------------------------------------
 1 | 10 10 5
 2 | 20 10 5
 3 | 10 20 5
 4 | 20 20 5
 5 | 10 10 10 
 6 | 20 10 10 
 7 | 10 20 10 
 8 | 20 20 10 
 9 | 15 20 15 
10 | 15 10 15
11 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # pnp算法的个人实现
 2 | 
 3 | dlt 实现具体细节参考  https://zhuanlan.zhihu.com/p/76047709
 4 | 
 5 | 
 6 | epnp 算法实现  https://zhuanlan.zhihu.com/p/76361026
 7 | 
 8 | 
 9 | 
10 | ## Build the program
11 | 
12 | 1. `mkdir build`
13 | 2. `cd build`
14 | 3. `cmake ..`
15 | 4. `make `
16 | 
17 | 
18 | 
19 | ## Run 
20 | 
21 | `bin/test_pnp` to  test the  EPnP .
22 | 
23 | `bin/test_dlt` to test the DLT .


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.14)
 2 | project(pnp_solver)
 3 | 
 4 | set(DEFAULT_BUILD_TYPE "Debug")
 5 | 
 6 | set(CMAKE_CXX_STANDARD 11)
 7 | 
 8 | list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake")
 9 | 
10 | 
11 | # third party libs
12 | # eigen
13 | find_package(Eigen3 REQUIRED)
14 | message(STATUS "EIGEN3_DIR:"  ${EIGEN3_INCLUDE_DIR})
15 | include_directories(
16 |         ${PROJECT_SOURCE_DIR}
17 |         ${EIGEN3_INCLUDE_DIR} )
18 | set(EXECUTABLE_OUTPUT_PATH  ${PROJECT_SOURCE_DIR}/bin)
19 | 
20 | add_executable(test_dlt test_dlt.cpp pnp_solver.cpp)
21 | add_executable(test_epnp  pnp_solver.cpp test_epnp.cpp)


--------------------------------------------------------------------------------
/cmake/FindEigen3.cmake:
--------------------------------------------------------------------------------
 1 | # Copyright 2018 The Cartographer Authors
 2 | #
 3 | # Licensed under the Apache License, Version 2.0 (the "License");
 4 | # you may not use this file except in compliance with the License.
 5 | # You may obtain a copy of the License at
 6 | #
 7 | #      http://www.apache.org/licenses/LICENSE-2.0
 8 | #
 9 | # Unless required by applicable law or agreed to in writing, software
10 | # distributed under the License is distributed on an "AS IS" BASIS,
11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | # See the License for the specific language governing permissions and
13 | # limitations under the License.
14 | 
15 | find_package(Eigen3 QUIET NO_MODULE)
16 | if (NOT EIGEN3_FOUND)
17 |   list(APPEND EIGEN3_POSSIBLE_DIRS
18 |     /usr/local/include/eigen3
19 |     /usr/include/eigen3
20 |   )
21 |   find_path(EIGEN3_INCLUDE_DIR
22 |     NAMES Eigen/Core
23 |     PATHS ${EIGEN3_POSSIBLE_DIRS}
24 |   )
25 |   if (EIGEN3_INCLUDE_DIR AND EXISTS ${EIGEN3_INCLUDE_DIR})
26 |     set(EIGEN3_FOUND TRUE)
27 |   else()
28 |     message(WARNING "Failed to find Eigen3. Please, define the path manually.")
29 |   endif()
30 | endif()
31 | 


--------------------------------------------------------------------------------
/test_dlt.cpp:
--------------------------------------------------------------------------------
 1 | #include <iostream>
 2 | #include <vector>
 3 | #include <Eigen/Core>
 4 | #include <Eigen/Dense>
 5 | #include <Eigen/Geometry>
 6 | #include "pnp_solver.h"
 7 | #include <fstream>
 8 | #include <random>
 9 | bool GenerateTestData(const Eigen::Matrix3d & R,const Eigen::Vector3d & t, const Eigen::Matrix3d &K,std::vector<Eigen::Vector3d> &pts_3d, std::vector<Eigen::Vector2d>  &pts_2d ){
10 | 
11 |     double w_sigma= 1.0;                 // 噪声Sigma值
12 |     std::default_random_engine generator;
13 |     std::normal_distribution<double> noise(0.,w_sigma);
14 | 
15 | 
16 |     std::ifstream infile("3d_points.txt");
17 | 
18 |     double x,y,z;
19 |     while(infile>>x>>y>>z){
20 |     pts_3d.emplace_back(x,y,z);
21 |     }
22 | 
23 |     for(const auto &pt:pts_3d){
24 | 
25 |         double n=noise(generator);
26 |         Eigen::Vector3d tem_p= K*R *(pt-t);
27 |         double u=tem_p(0)/tem_p(2)+n;
28 |         double v=tem_p(1)/tem_p(2)+n;
29 |         pts_2d.emplace_back(u,v);
30 | 
31 | 
32 |     }
33 |     return true;
34 | 
35 | 
36 | }
37 | 
38 | 
39 | 
40 | int main(int argc,char** argv) {
41 | 
42 |     Eigen::AngleAxisd rotation_angle_axis(M_PI/4,Eigen::Vector3d(0,0,1));
43 |     Eigen::Matrix3d R_cw=rotation_angle_axis.toRotationMatrix();
44 | 
45 |     Eigen::Vector3d t_w(1,0,0);
46 |     Eigen::Matrix3d K;
47 |     K<<100,0,160,
48 |         0,100,120 ,
49 |         0,0,1;
50 | 
51 |     std::vector<Eigen::Vector3d> pts_3d;
52 |     std::vector<Eigen::Vector2d> pts_2d;
53 |     GenerateTestData(R_cw,t_w,K,pts_3d,pts_2d);
54 | 
55 | 
56 |     PnPSolver pnp_solver;
57 | 
58 |     Eigen::Matrix3d R_estimate,K_estimate;
59 |     Eigen::Vector3d t_estimate;
60 | 
61 |      pnp_solver.SolvePnPByDLT(pts_3d,pts_2d,R_estimate,t_estimate,K_estimate);
62 | 
63 | 
64 | 
65 |      std::cout<<"R"<<std::endl;
66 |      std::cout<<R_cw<<std::endl;
67 | 
68 |      std::cout<<"R_estimated "<< std::endl;
69 |      std::cout<<R_estimate<<std::endl;
70 |      std::cout<<"R_derter:"<<R_estimate.determinant()<<std::endl;
71 | 
72 |      std::cout<<"K"<<std::endl;
73 |      std::cout<<K<<std::endl;
74 | 
75 |      std::cout<<"K_estimated"<<std::endl;
76 |      std::cout<<K_estimate<<std::endl;
77 | 
78 | 
79 |     std::cout<<"t_w"<<std::endl;
80 |     std::cout<<t_w<<std::endl;
81 | 
82 |     std::cout<<"t_estimated"<<std::endl;
83 |     std::cout<<t_estimate<<std::endl;
84 | 
85 | 
86 |     return 0;
87 | }


--------------------------------------------------------------------------------
/test_epnp.cpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by xiaomi on 8/6/19.
 3 | //
 4 | 
 5 | #include <iostream>
 6 | #include <vector>
 7 | #include <Eigen/Core>
 8 | #include <Eigen/Dense>
 9 | #include <Eigen/Geometry>
10 | #include "pnp_solver.h"
11 | #include <fstream>
12 | #include <random>
13 | bool GenerateTestData(const Eigen::Matrix3d & R_cw,const Eigen::Vector3d & t_cw, const Eigen::Matrix3d &K,std::vector<Eigen::Vector3d> &pts_3d, std::vector<Eigen::Vector2d>  &pts_2d ){
14 | 
15 |     double w_sigma= 1.0;                 // 噪声Sigma值
16 |     std::default_random_engine generator;
17 |     std::normal_distribution<double> noise(0.,w_sigma);
18 | 
19 | 
20 |     std::ifstream infile("3d_points.txt");
21 | 
22 |     double x,y,z;
23 |     while(infile>>x>>y>>z){
24 |         pts_3d.emplace_back(x,y,z);
25 |     }
26 | 
27 |     for(const auto &pt:pts_3d){
28 | 
29 |         double n=noise(generator);
30 |         Eigen::Vector3d tem_p= K*(R_cw *pt+t_cw);
31 |         double u=tem_p(0)/tem_p(2)+n;
32 |         double v=tem_p(1)/tem_p(2)+n;
33 |         pts_2d.emplace_back(u,v);
34 | 
35 | 
36 |     }
37 |     return true;
38 | 
39 | 
40 | }
41 | 
42 | 
43 | 
44 | int main(int argc,char** argv) {
45 | 
46 |     Eigen::AngleAxisd rotation_angle_axis(M_PI/4,Eigen::Vector3d(0,0,1));
47 |     Eigen::Matrix3d R_cw=rotation_angle_axis.toRotationMatrix();
48 | 
49 |     Eigen::Vector3d t_cw(1, 0, 0);
50 |     Eigen::Matrix3d K;
51 |     K<<100,0,160,
52 |             0,100,120 ,
53 |             0,0,1;
54 | 
55 |     std::vector<Eigen::Vector3d> pts_3d;
56 |     std::vector<Eigen::Vector2d> pts_2d;
57 | 
58 |     GenerateTestData(R_cw, t_cw, K, pts_3d, pts_2d);
59 | 
60 | 
61 |     PnPSolver pnp_solver;
62 | 
63 |     Eigen::Matrix3d R_estimate,K_estimate;
64 |     Eigen::Vector3d t_estimate;
65 | 
66 |     pnp_solver.SolvePnPByEPNP(pts_3d,pts_2d,K, R_estimate,t_estimate);
67 | 
68 | 
69 | 
70 |     std::cout<<"R"<<std::endl;
71 |     std::cout<<R_cw<<std::endl;
72 | 
73 |     std::cout<<"R_estimated "<< std::endl;
74 |     std::cout<<R_estimate<<std::endl;
75 | 
76 | //    std::cout<<"K"<<std::endl;
77 | //    std::cout<<K<<std::endl;
78 | //
79 | //    std::cout<<"K_estimated"<<std::endl;
80 | //    std::cout<<K_estimate<<std::endl;
81 | 
82 | 
83 |     std::cout<<"t_cw"<<std::endl;
84 |     std::cout << t_cw << std::endl;
85 | 
86 |     std::cout<<"t_estimated"<<std::endl;
87 |     std::cout<<t_estimate<<std::endl;
88 | 
89 | 
90 |     return 0;
91 | }


--------------------------------------------------------------------------------
/pnp_solver.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by xiaomi on 8/1/19.
 3 | //
 4 | 
 5 | #ifndef PNP_SOLVER_PNP_SOLVER_H
 6 | #define PNP_SOLVER_PNP_SOLVER_H
 7 | #include <vector>
 8 | #include <Eigen/Core>
 9 | #include <Eigen/Dense>
10 | class PnPSolver{
11 | public:
12 |     bool SolvePnPByDLT( const std::vector< Eigen::Vector3d >& pts3d, const std::vector< Eigen::Vector2d >& pts2d, Eigen::Matrix3d& R, Eigen::Vector3d& t, Eigen::Matrix3d& K);
13 |     bool SolvePnPByEPNP(const std::vector<Eigen::Vector3d> &pts3d,const std::vector<Eigen::Vector2d> &pts2d,const Eigen::Matrix3d & K,Eigen::Matrix3d &R,Eigen::Vector3d & t);
14 | 
15 | 
16 | 
17 | private:
18 | 
19 |     //---These functions and variables are used by DLT
20 |     bool ConstructM(const std::vector<Eigen::Vector3d >& pts3d, const std::vector< Eigen::Vector2d >& pts2d);
21 |     bool ComputeP();
22 |     bool DecomposeP(Eigen::Matrix3d& R, Eigen::Vector3d& t, Eigen::Matrix3d& K);
23 |     Eigen::Matrix<double,3,4> P_;
24 |     Eigen::MatrixXd M_;
25 |     //-----------------
26 | 
27 | 
28 | 
29 |     //---The following  Functions or variables are used by EPNP
30 |     void ChooseControlPointsWorld(const std::vector<Eigen::Vector3d >& pts3d,std::vector<Eigen::Vector3d> &control_points_world);
31 |     void ComputeHBCoordinates(const std::vector<Eigen::Vector3d> & pts3d, const std::vector<Eigen::Vector3d> &control_points_world , std::vector<Eigen::Vector4d> &hb_coordinates);
32 |     void ComputeM(const std::vector<Eigen::Vector2d>& pts2d, const Eigen::Matrix3d&  K, const std::vector<Eigen::Vector4d>& hbcs, Eigen::MatrixXd &M  );
33 |     void ComputeL6x10(const Eigen::Matrix<double ,12,4> &  V,Eigen::Matrix<double,6,10> & L6x10);
34 |     void ComputeRho(const std::vector<Eigen::Vector3d> &control_points_world ,Eigen::VectorXd &rho );
35 |     void ComputeBetaApproximate1(const Eigen::Matrix<double ,6,10> & L6x10,const Eigen::VectorXd& rho,Eigen::Vector4d& beta);
36 |     void DoGaussNewtonOptimization(const Eigen::Matrix<double ,6,10>&  L6x10,const Eigen::VectorXd& rho,Eigen::Vector4d& beta);
37 |     void ComputeJacobianMatrix(const Eigen::Matrix<double ,6,10>&  L6x10,const Eigen::Vector4d& beta ,Eigen::Matrix<double ,6,4> & jacobian);
38 |     void ComputeResidual(const Eigen::Matrix<double ,6,10>&  L6x10,const Eigen::Vector4d& beta,const Eigen::VectorXd& rho,Eigen::VectorXd& residual );
39 | 
40 |     void ComputeControlPointsCamera(const Eigen::Vector4d& beta,const Eigen::Matrix<double ,12,4> &V,std::vector<Eigen::Vector3d >& control_points_camera);
41 |     void ComputePts3DCameraCoordinates(const std::vector<Eigen::Vector4d>& hb_coordinates, const std::vector<Eigen::Vector3d >& control_points_camera, std::vector<Eigen::Vector3d>& pts_3d_camera );
42 |     void ComputeRt(const std::vector<Eigen::Vector3d>& pts3d,const std::vector<Eigen::Vector3d > & pts_3d_camera,Eigen::Matrix3d& R,Eigen::Vector3d& t);
43 | 
44 | 
45 | 
46 | 
47 | 
48 | };
49 | 
50 | 
51 | 
52 | #endif //PNP_SOLVER_PNP_SOLVER_H
53 | 


--------------------------------------------------------------------------------
/pnp_solver.cpp:
--------------------------------------------------------------------------------
  1 | //
  2 | // Created by xiaomi on 8/1/19.
  3 | //
  4 | 
  5 | #include "pnp_solver.h"
  6 | #include <Eigen/Core>
  7 | #include <Eigen/Dense>
  8 | #include <Eigen/QR>
  9 | #include <Eigen/Geometry>
 10 | #include <iostream>
 11 | #include <Eigen/Eigenvalues>
 12 | bool PnPSolver::SolvePnPByDLT(const std::vector<Eigen::Vector3d> &pts3d, const std::vector<Eigen::Vector2d> &pts2d,
 13 |                               Eigen::Matrix3d &R, Eigen::Vector3d &t, Eigen::Matrix3d &K) {
 14 |     if(!ConstructM(pts3d,pts2d))
 15 |         return false;
 16 |     ComputeP();
 17 |     DecomposeP(R,t,K);
 18 |     return  true;
 19 | 
 20 | }
 21 | bool PnPSolver::ConstructM(const std::vector<Eigen::Vector3d> &pts3d, const std::vector<Eigen::Vector2d> &pts2d) {
 22 |     int num_of_3d_points=pts3d.size();
 23 |     int num_of_2d_points=pts2d.size();
 24 |     if((num_of_3d_points < 6) || (num_of_3d_points!= num_of_2d_points)){
 25 |         return false;
 26 |     }
 27 | 
 28 |     M_.resize(2*num_of_3d_points,12);
 29 |     for (int i=0;i<num_of_3d_points;i++){
 30 |         Eigen::Matrix<double,2,12> matrix_12;
 31 |         double x=pts3d[i](0) , y=pts3d[i](1) , z=pts3d[i](2);
 32 |         double u=pts2d[i](0) , v=pts2d[i](1);
 33 |        matrix_12<<-x,-y,-z,-1,0,0,0,0,u*x,u*y,u*z,u,
 34 |                 0,0,0,0,-x,-y,-z,-1,v*x,v*y,v*z,v;
 35 | 
 36 |         M_.block(2*i,0,2,12)=matrix_12;
 37 | 
 38 |     }
 39 | 
 40 |     return  true;
 41 | }
 42 | bool PnPSolver::ComputeP() {
 43 | 
 44 |     Eigen::JacobiSVD<Eigen::MatrixXd> svd_m(M_,Eigen::ComputeFullV);
 45 |     Eigen::MatrixXd V=svd_m.matrixV();
 46 |     Eigen::MatrixXd  Sigma=svd_m.singularValues();
 47 |     Eigen::Matrix<double ,12,1> p;
 48 |     p<<V(0,11),V(1,11),V(2,11),V(3,11),
 49 |             V(4,11),V(5,11),V(6,11),V(7,11),
 50 |             V(8,11),V(9,11),V(10,11),V(11,11);
 51 |     p.normalize();
 52 | 
 53 |     P_<<p(0),p(1),p(2),p(3),
 54 |     p(4), p(5),p(6),p(7),
 55 |     p(8),p(9),p(10),p(11);
 56 |     return  true;
 57 | 
 58 | }
 59 | bool PnPSolver::DecomposeP(Eigen::Matrix3d &R, Eigen::Vector3d &t, Eigen::Matrix3d &K) {
 60 | 
 61 |     Eigen::Matrix3d H=P_.block(0,0,3,3);
 62 |     Eigen::Matrix3d H_inv=H.inverse();
 63 |     Eigen::Vector3d h=P_.col(3);
 64 |     Eigen::Vector3d tem_t=-H_inv*h;
 65 |     t=tem_t;
 66 | 
 67 |     Eigen::HouseholderQR<Eigen::Matrix3d> qr(H_inv);
 68 |     Eigen::Matrix3d temR=qr.householderQ();
 69 |     Eigen::Matrix3d temK=qr.matrixQR().triangularView<Eigen::Upper>();
 70 | 
 71 | //    std::cout<<"H_inv:"<<H_inv<<std::endl;
 72 | //    std::cout<<"Q*R"<<temR*temK<<std::endl;
 73 | 
 74 | 
 75 |     R=temR.transpose();
 76 |     K=temK.inverse();
 77 |     K=K*(1/K(2,2));
 78 | 
 79 |     if(K(0,0)<0){
 80 | 
 81 |     Eigen::AngleAxisd rotation(M_PI,Eigen::Vector3d::UnitZ());
 82 |     Eigen::Matrix3d R_z_pi=rotation.toRotationMatrix() ;
 83 | //    std::cout<<"R_z_pi"<<std::endl;
 84 | //    std::cout<<R_z_pi<<std::endl;
 85 | 
 86 |     R=R_z_pi*R;
 87 |     K=K*R_z_pi;
 88 | 
 89 |     }
 90 | 
 91 |     return  true;
 92 | 
 93 | }
 94 | bool PnPSolver::SolvePnPByEPNP(const std::vector<Eigen::Vector3d> &pts3d, const std::vector<Eigen::Vector2d> &pts2d,
 95 |                                const Eigen::Matrix3d &K, Eigen::Matrix3d &R, Eigen::Vector3d &t) {
 96 | 
 97 |     std::vector<Eigen::Vector3d> control_points_world;
 98 | 
 99 |     ChooseControlPointsWorld(pts3d, control_points_world);
100 | //    std::cout<<"control points:"<<std::endl;
101 | //    std::cout<<"----"<<std::endl;
102 | //    for(const auto & pt:control_points_world ){
103 | //        std::cout<<pt<<std::endl;
104 | //    }
105 | //    std::cout<<"----"<<std::endl;
106 | 
107 | 
108 | 
109 |     std::vector<Eigen::Vector4d> hb_coordinates;
110 | 
111 |     ComputeHBCoordinates(pts3d, control_points_world, hb_coordinates);
112 | 
113 |     //test hb coordinates and control points
114 | //    std::cout<<"pw combine hb control points---------"<<std::endl;
115 | //    for(const auto & hb: hb_coordinates){
116 | //        Eigen::Vector3d pt_w=hb(0)*control_points_world[0] +hb(1)*control_points_world[1]+hb(2)*control_points_world[2] +hb(3)*control_points_world[3];
117 | //        std::cout<<pt_w<<std::endl<<std::endl;
118 | //    }
119 |     Eigen::MatrixXd M;
120 |     ComputeM(pts2d, K, hb_coordinates, M);
121 | //    std::cout << "M:" << std::endl;
122 | //    std::cout<<M<<std::endl;
123 |     Eigen::Matrix<double, 12, 12> MtM = M.transpose() * M;
124 | //    std::cout << MtM << std::endl;
125 | 
126 |     Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 12, 12> > saes(MtM);
127 |     Eigen::MatrixXd eigen_vectors = saes.eigenvectors();
128 |     Eigen::VectorXd eigen_values = saes.eigenvalues();
129 | 
130 |     std::cout << "eigen_vector:" << std::endl;
131 |     std::cout << eigen_vectors << std::endl;
132 | 
133 |     Eigen::Matrix<double, 12, 4> V = eigen_vectors.block(0, 0, 12, 4);
134 | 
135 |     Eigen::Matrix<double, 6, 10> L6x10;
136 |     ComputeL6x10(V, L6x10);
137 |     Eigen::VectorXd rho;
138 |     ComputeRho(control_points_world, rho);
139 | 
140 |     Eigen::Vector4d beta;
141 | 
142 |     ComputeBetaApproximate1(L6x10, rho, beta);
143 |     DoGaussNewtonOptimization(L6x10, rho, beta);
144 | 
145 |     std::vector<Eigen::Vector3d> control_points_camera;
146 |     ComputeControlPointsCamera(beta, V, control_points_camera);
147 | 
148 |     std::vector<Eigen::Vector3d> pts_3d_camera;
149 |     ComputePts3DCameraCoordinates(hb_coordinates, control_points_camera, pts_3d_camera);
150 | 
151 |     ComputeRt(pts3d, pts_3d_camera, R, t);
152 | 
153 | }
154 | void PnPSolver::ChooseControlPointsWorld(const std::vector<Eigen::Vector3d> &pts3d, std::vector<Eigen::Vector3d> &control_points_world) {
155 | 
156 |     control_points_world.resize(4);
157 |     const int num_of_pts=pts3d.size();
158 |     Eigen::Vector3d sum_of_pts(0,0,0);
159 |     for (const auto pt:pts3d)
160 |         sum_of_pts+=pt;
161 |     control_points_world[0]= sum_of_pts / num_of_pts;
162 | 
163 |     Eigen::MatrixXd A;
164 |     A.resize(num_of_pts,3);
165 | 
166 |     for (int i=0;i<num_of_pts;i++){
167 |         A.row(i)= (pts3d[i] - control_points_world[0]).transpose();
168 |     }
169 |     Eigen::Matrix3d AtA= A.transpose() * A;
170 | 
171 |     Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(AtA);
172 | 
173 |     Eigen::Matrix3d eigen_vectors=saes.eigenvectors();
174 |     Eigen::Vector3d eigen_values=saes.eigenvalues();
175 | 
176 |     control_points_world[1]=control_points_world[0] + sqrt(eigen_values(0) /num_of_pts)* eigen_vectors.col(0);
177 |     control_points_world[2]=control_points_world[0] + sqrt(eigen_values(1) /num_of_pts)* eigen_vectors.col(1);
178 |     control_points_world[3]=control_points_world[0] + sqrt(eigen_values(2) /num_of_pts)* eigen_vectors.col(2);
179 | 
180 | 
181 | 
182 | }
183 | void PnPSolver::ComputeHBCoordinates(const std::vector<Eigen::Vector3d> &pts3d,
184 |                                      const std::vector<Eigen::Vector3d> &control_points_world, std::vector<Eigen::Vector4d> &hb_coordinates) {
185 |     Eigen::Matrix4d C=Eigen::Matrix4d::Ones();
186 |     const int num_of_pts3d=pts3d.size();
187 |     hb_coordinates.resize(num_of_pts3d);
188 | 
189 | 
190 |     for(int i=0;i<4;i++){
191 |         C.block(0,i,3,1)=control_points_world[i];
192 |     }
193 |     Eigen::Matrix4d C_inv=C.inverse();
194 | 
195 |     for(int i=0;i<num_of_pts3d;i++){
196 |         Eigen::Vector4d pt_homogeneous=Eigen::Vector4d::Ones();
197 |         pt_homogeneous.head(3)=pts3d[i];
198 |         hb_coordinates[i]= C_inv * pt_homogeneous;
199 |     }
200 | 
201 | 
202 | }
203 | void PnPSolver::ComputeM(const std::vector<Eigen::Vector2d> &pts2d, const Eigen::Matrix3d &K,
204 |                          const std::vector<Eigen::Vector4d> &hbcs, Eigen::MatrixXd &M) {
205 | 
206 |     const int num_of_pts=pts2d.size();
207 |     M.resize(2*num_of_pts,12);
208 |     double f_x=K(0,0),f_y=K(1,1),c_x=K(0,2),c_y=K(1,2);
209 | 
210 |     for(int i=0;i<num_of_pts;i++){
211 |         Eigen::Vector4d hb=hbcs[i];
212 |         Eigen::Vector2d u=pts2d[i];
213 |         for(int j=0;j<4;j++){
214 |             M(2*i,j*3)=hb(j)*f_x;
215 |             M(2*i,j*3+1)=0;
216 |             M(2*i,j*3+2)=hb(j)*(c_x-u (0));
217 | 
218 |             M(2*i+1,j*3)=0;
219 |             M(2*i+1,j*3+1)=hb(j)*f_y;
220 |             M(2*i+1,j*3+2)=hb(j)*(c_y-u(1));
221 | 
222 |         }
223 | 
224 | 
225 |     }
226 | 
227 | 
228 | 
229 | 
230 | }
231 | void PnPSolver::ComputeL6x10(const Eigen::Matrix<double, 12, 4> &V, Eigen::Matrix<double, 6, 10> &L6x10) {
232 |     const int idx0[6] = {0, 0, 0, 1, 1, 2};
233 |     const int idx1[6] = {1, 2, 3, 2, 3, 3};
234 |     for(int i=0;i<6;i++){
235 |         const int idi=idx0[i]*3;
236 |         const int idj=idx1[i]*3;
237 |         // the first control point.
238 |         const Eigen::Vector3d v1i = V.block ( idi, 0, 3, 1 );
239 |         const Eigen::Vector3d v2i = V.block ( idi, 1, 3, 1 );
240 |         const Eigen::Vector3d v3i = V.block ( idi, 2, 3, 1 );
241 |         const Eigen::Vector3d v4i = V.block ( idi, 3, 3, 1 );
242 | 
243 |         // the second control point
244 |         const Eigen::Vector3d v1j = V.block ( idj, 0, 3, 1 );
245 |         const Eigen::Vector3d v2j = V.block ( idj, 1, 3, 1 );
246 |         const Eigen::Vector3d v3j = V.block ( idj, 2, 3, 1 );
247 |         const Eigen::Vector3d v4j = V.block ( idj, 3, 3, 1 );
248 | 
249 |         Eigen::Vector3d S1 = v1i - v1j;
250 |         Eigen::Vector3d S2 = v2i - v2j;
251 |         Eigen::Vector3d S3 = v3i - v3j;
252 |         Eigen::Vector3d S4 = v4i - v4j;
253 | 
254 |         Eigen::Matrix<double, 1, 3> S1_T = S1.transpose();
255 |         Eigen::Matrix<double, 1, 3> S2_T = S2.transpose();
256 |         Eigen::Matrix<double, 1, 3> S3_T = S3.transpose();
257 |         Eigen::Matrix<double, 1, 3> S4_T = S4.transpose();
258 | 
259 |         //[B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
260 |         L6x10 ( i, 0 ) = S1_T * S1;
261 |         L6x10( i, 1 ) = 2 * S1_T * S2;
262 |         L6x10( i, 2 ) = S2_T * S2;
263 |         L6x10 ( i, 3 ) = 2 * S1_T * S3;
264 |         L6x10( i, 4 ) = 2 * S2_T * S3;
265 |         L6x10( i, 5 ) = S3_T * S3;
266 |         L6x10 ( i, 6 ) = 2 * S1_T * S4;
267 |         L6x10 ( i, 7 ) = 2 * S2_T * S4;
268 |         L6x10 ( i, 8 ) = 2 * S3_T * S4;
269 |         L6x10( i, 9 ) = S4_T * S4;
270 | 
271 | 
272 |     }
273 | 
274 | 
275 | }
276 | 
277 | void PnPSolver::ComputeRho(const std::vector<Eigen::Vector3d> &control_points_world, Eigen::VectorXd &rho) {
278 |     rho.resize(6);
279 | 
280 |     const int idx0[6] = {0, 0, 0, 1, 1, 2};
281 |     const int idx1[6] = {1, 2, 3, 2, 3, 3};
282 |     for(int i=0;i<6;i++){
283 |         Eigen::Vector3d dist=control_points_world[idx0[i]] -control_points_world[idx1[i]];
284 |         rho(i)=dist.dot(dist);
285 |     }
286 | 
287 | }
288 | 
289 | 
290 | void PnPSolver::ComputeBetaApproximate1(const Eigen::Matrix<double, 6, 10> &L6x10, const Eigen::VectorXd &rho,
291 |                                         Eigen::Vector4d &beta) {
292 |     // betas10        = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
293 |     // betas_approx_1 = [B11 B12     B13         B14]
294 | 
295 |     Eigen::Matrix<double ,6,4> L6x4;
296 | 
297 |     L6x4.col(0)=L6x10.col(0);
298 |     L6x4.col(1)=L6x10.col(1);
299 |     L6x4.col(2)=L6x10.col(3);
300 |     L6x4.col(3)=L6x10.col(6);
301 | 
302 |     Eigen::VectorXd b4=L6x4.colPivHouseholderQr().solve(rho);
303 | 
304 |     if(b4(0)<0)
305 |     {
306 |         beta(0)=sqrt(-b4(0));
307 |         beta(1)=-b4(1)/beta(0);
308 |         beta(2)=-b4(2)/beta(0);
309 |         beta(3)=(-b4(3))/beta(0);
310 | 
311 |     }
312 |     else {
313 |         beta(0)=sqrt(b4(0));
314 |         beta(1)=b4(1)/beta(0);
315 |         beta(2)=b4(2)/beta(0);
316 |         beta(3)=(b4(3))/beta(0);
317 | 
318 |     }
319 | 
320 | 
321 | 
322 | 
323 | 
324 | }
325 | void PnPSolver::ComputeJacobianMatrix(const Eigen::Matrix<double, 6, 10> &L6x10, const Eigen::Vector4d &beta,
326 |                                       Eigen::Matrix<double, 6, 4> & jacobian) {
327 |     Eigen::Matrix<double,10,4 > jacobian_beta_bata;
328 |     double b1=beta(0),b2=beta(1),b3=beta(2),b4=beta(3);
329 |     jacobian_beta_bata<<2*b1,0,0,0,
330 |                         b2,b1,0,0,
331 |                         0,2*b2,0,0,
332 |                         b3,0,b1,0,
333 |                         0,b3,b2,0,
334 |                         0,0,2*b3,0,
335 |                         b4,0,0,b1,
336 |                         0,b4,0,b2,
337 |                         0,0,b4,b3,
338 |                         0,0,0,2*b4;
339 |     jacobian=L6x10*jacobian_beta_bata;
340 | 
341 | 
342 | }
343 | void PnPSolver::ComputeResidual(const Eigen::Matrix<double, 6, 10> &L6x10, const Eigen::Vector4d &beta,
344 |                                 const Eigen::VectorXd &rho, Eigen::VectorXd &residual) {
345 |     Eigen::VectorXd beta_vetor(10);
346 |     double b1=beta(0),b2=beta(1),b3=beta(2),b4=beta(3);
347 |     beta_vetor<<b1*b1,
348 |                 b1*b2,
349 |                 b2*b2,
350 |                 b1*b3,
351 |                 b2*b3,
352 |                 b3*b3,
353 |                 b1*b4,
354 |                 b2*b4,
355 |                 b3*b4,
356 |                 b4*b4;
357 |     residual= L6x10 * beta_vetor - rho;
358 | 
359 | }
360 | 
361 | 
362 | void PnPSolver::DoGaussNewtonOptimization(const Eigen::Matrix<double, 6, 10> &L6x10, const Eigen::VectorXd &rho,
363 |                                            Eigen::Vector4d& beta) {
364 |     const int iterattion_times=10;
365 |     for(int i=0;i<iterattion_times;i++){
366 | 
367 |         Eigen::Matrix<double ,6,4> jacobian;
368 |         Eigen::VectorXd residual(6);
369 |         ComputeJacobianMatrix(L6x10,beta,jacobian);
370 |         ComputeResidual(L6x10,beta,rho,residual);
371 |         Eigen::Matrix<double , 4,6> jacobian_trans=jacobian.transpose();
372 |         Eigen::Matrix4d H= jacobian_trans*jacobian;
373 |         Eigen::Vector4d delta_beta =H.colPivHouseholderQr().solve(-jacobian_trans*residual);
374 |         beta+=delta_beta;
375 | 
376 |     }
377 | 
378 | 
379 | 
380 | }
381 | void PnPSolver::ComputeControlPointsCamera(const Eigen::Vector4d &beta, const Eigen::Matrix<double, 12, 4> &V,
382 |                                            std::vector<Eigen::Vector3d>& control_points_camera) {
383 |     control_points_camera.resize(4);
384 |     Eigen::VectorXd  control_points_vector(12);
385 |     control_points_vector=beta(0)*V.col(0)+beta(1)*V.col(1)+beta(2)*V.col(2)+beta(3)*V.col(3);
386 | 
387 |     for(int i=0;i<4;i++){
388 |         control_points_camera[i]=control_points_vector.block(i*3,0,3,1); //control_points_vector.segment(i*3,3);
389 | 
390 |     }
391 | 
392 | 
393 | 
394 | 
395 | }
396 | void PnPSolver::ComputePts3DCameraCoordinates(const std::vector<Eigen::Vector4d>& hb_coordinates,
397 |                                               const std::vector<Eigen::Vector3d> &control_points_camera,
398 |                                               std::vector<Eigen::Vector3d>& pts_3d_camera) {
399 |     const int num_of_pts=hb_coordinates.size();
400 |     pts_3d_camera.resize(num_of_pts);
401 |     for(int i=0;i<num_of_pts;i++){
402 |         double a0=hb_coordinates[i](0),a1=hb_coordinates[i](1),a2=hb_coordinates[i](2),a3=hb_coordinates[i](3);
403 |         pts_3d_camera[i]=a0*control_points_camera[0]+a1*control_points_camera[1]+a2*control_points_camera[2]+a3*control_points_camera[3];
404 |     }
405 | 
406 |     for ( auto &pt:pts_3d_camera){
407 |         if(pt(2)<0.0)
408 |         {
409 |             pt=-pt;
410 | //        std::cout<<"pts_3d_camera z<0"<<std::endl;
411 | //        std::cout<<pt<<std::endl;
412 | 
413 |         }
414 |     }
415 | 
416 | 
417 | 
418 | }
419 | void PnPSolver::ComputeRt(const std::vector<Eigen::Vector3d> &pts3d, const std::vector<Eigen::Vector3d> &pts_3d_camera,
420 |                           Eigen::Matrix3d &R, Eigen::Vector3d& t) {
421 |     const int num_of_pts=pts3d.size();
422 | 
423 |     Eigen::Vector3d sum_pts_world(0,0,0);
424 | 
425 |     for(const auto & pt:pts3d){
426 |         sum_pts_world+=pt;
427 |     }
428 |     Eigen::Vector3d p_center_world=sum_pts_world/num_of_pts;
429 | 
430 | 
431 |     Eigen::MatrixXd A(num_of_pts,3);
432 |     for(int i=0;i<num_of_pts;i++){
433 |         A.row(i)=(pts3d[i]-p_center_world).transpose();
434 |     }
435 | 
436 |     Eigen::Vector3d sum_pts_camera(0,0,0);
437 | 
438 |     for(const auto & pt:pts_3d_camera){
439 |         sum_pts_camera+=pt;
440 |     }
441 |     Eigen::Vector3d p_center_camera=sum_pts_camera/num_of_pts;
442 | 
443 |     Eigen::MatrixXd B(num_of_pts,3);
444 |     for(int i=0;i<num_of_pts;i++){
445 |         B.row(i)=(pts_3d_camera[i] - p_center_camera).transpose();
446 | 
447 |     }
448 | 
449 |     Eigen::Matrix3d H=B.transpose()*A;
450 | 
451 |     Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
452 |     Eigen::MatrixXd U = svd.matrixU();
453 |     Eigen::MatrixXd V = svd.matrixV();
454 | 
455 |     R = U*V.transpose();
456 |     double detR = R.determinant();
457 | 
458 |     if (detR < 0){
459 |      R.row(2)=-R.row(2);
460 |     }
461 | 
462 |     t =p_center_camera  - R*p_center_world;
463 | 
464 | 
465 | 
466 | }


--------------------------------------------------------------------------------