├── .gitignore
├── CMakeLists.txt
├── Library.cpp
├── Licenses.txt
├── Parameters.yml
├── README.md
├── SimpleMain.cpp
├── TestMain.cpp
├── pose
    ├── AOOnlyPoseAdapter.hpp
    ├── AOPoseAdapter.hpp
    ├── AbsoluteOrientation.hpp
    ├── AbsoluteOrientationNormal.hpp
    ├── MinimalSolvers.hpp
    ├── NormalAOPoseAdapter.hpp
    ├── P3P.hpp
    ├── PnPPoseAdapter.hpp
    ├── PoseAdapterBase.hpp
    ├── Simulator.hpp
    └── Utility.hpp
└── sophus
    ├── average.hpp
    ├── common.hpp
    ├── geometry.hpp
    ├── interpolate.hpp
    ├── interpolate_details.hpp
    ├── num_diff.hpp
    ├── rotation_matrix.hpp
    ├── rxso2.hpp
    ├── rxso3.hpp
    ├── se2.hpp
    ├── se3.hpp
    ├── sim2.hpp
    ├── sim3.hpp
    ├── sim_details.hpp
    ├── so2.hpp
    ├── so3.hpp
    ├── sophus.hpp
    ├── test_macros.hpp
    ├── tests.hpp
    ├── types.hpp
    └── velocities.hpp


/.gitignore:
--------------------------------------------------------------------------------
1 | /build_2015
2 | 
3 | build/
4 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required (VERSION 2.8)
 2 | 
 3 | if(COMMAND cmake_policy)
 4 |   cmake_policy(SET CMP0043 NEW)
 5 |   #cmake_policy(SET CMP0038 NEW)
 6 |   #cmake_policy(SET CMP0020 NEW)
 7 | endif()
 8 | 
 9 | project (RGBDPoseEstimation)
10 | 
11 | find_package( OpenCV REQUIRED )
12 | 
13 | if(UNIX)
14 |     SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11")
15 | endif()
16 | 
17 | find_package(Eigen3 REQUIRED PATHS "c:/all_libs/eigen/include/eigen3")
18 | include_directories ( ${EIGEN3_INCLUDE_DIR} )
19 | include_directories ( "c:/all_libs/eigen/include/eigen3" )
20 | include_directories ( ${PROJECT_SOURCE_DIR}/sophus )
21 | include_directories ( ${PROJECT_SOURCE_DIR}/pose )
22 | if( UNIX )
23 | 
24 | elseif( WIN32 )
25 | message(STATUS "platform: Windows")
26 | include_directories ( $ENV{OPENCV_INC} )
27 | endif()
28 | 
29 | file(GLOB POSE_HEADERS "pose/[a-zA-Z]*.h" "pose/[a-zA-Z]*.hpp" )
30 | message(STATUS "header names: ${POSE_HEADERS}")
31 | 
32 | add_library( rgbd_pose_estimation STATIC ${POSE_HEADERS} )
33 | set_target_properties(rgbd_pose_estimation PROPERTIES LINKER_LANGUAGE CXX)
34 | 
35 | 
36 | file(GLOB TESTER_SOURCE "TestMain.cpp" )
37 | message(STATUS "header names: ${TESTER_SOURCE}")
38 | 
39 | add_executable( example ${TESTER_SOURCE} )
40 | target_link_libraries( example  opencv_core  opencv_imgproc opencv_calib3d)
41 | 
42 | add_executable( simple SimpleMain.cpp )
43 | target_link_libraries( simple  opencv_core  opencv_imgproc opencv_calib3d)
44 | 
45 | add_library( absolute SHARED Library.cpp )
46 | target_link_libraries( absolute  opencv_core  opencv_imgproc opencv_calib3d)
47 | 
48 | 
49 | 
50 | 
51 | 
52 | 


--------------------------------------------------------------------------------
/Library.cpp:
--------------------------------------------------------------------------------
 1 | #define _USE_MATH_DEFINES
 2 | 
 3 | #include <Eigen/Core>
 4 | #include "AbsoluteOrientation.hpp"
 5 | #include "AOOnlyPoseAdapter.hpp"
 6 | #include "Simulator.hpp"
 7 | #include <iostream>
 8 | #include <string>
 9 | 
10 | #include <opencv2/opencv.hpp>
11 | 
12 | using namespace std;
13 | using namespace Eigen;
14 | 
15 | extern "C" {
16 |  
17 | 	void ao(float* x_w_, float* x_c_, int n_, float* R_cw_, float* t_)
18 | 	{
19 |         cout << "ao()" << endl;
20 |         MatrixXf Xw = Map<MatrixXf >(x_w_, 3, n_);
21 |         // cout << "Xw " << Xw << endl;
22 |         MatrixXf Xc = Map<MatrixXf >(x_c_, 3, n_);
23 |         // cout << "Xc " << Xc << endl;
24 |         AOOnlyPoseAdapter<float> adapter(Xc, Xw);
25 |         float f = 555.f;
26 |         adapter.setFocal(f, f);
27 | 
28 |         // int updated_iter = 1000;
29 |         // float thre_3d = 0.1;
30 |         // float confidence = 0.99999f;
31 |         // shinji_ransac2<float>(adapter, thre_3d, updated_iter, confidence);
32 |         // cout << "updated_iter = " << updated_iter << endl;
33 |         // cout << "inliers = " << adapter.getMaxVotes() << endl;
34 |         shinji_ls2<float>(adapter);
35 |         Matrix3f R_cw = adapter.getRcw().matrix();
36 |         Matrix3f Rp = R_cw.transpose();
37 |         float* data = Rp.data();
38 |         for (int i=0; i < 9; i++)
39 |             R_cw_[i] = data[i];
40 | 
41 |         Vector3f tw = adapter.gettw();
42 |         for (int i=0; i < 3; i++)
43 |             t_[i] = tw[i];
44 |         // cout << SE3_[0] << endl;
45 | 	}
46 | 
47 |     void ao_ransac(float* x_w_, float* x_c_, int n_, float* R_cw_, float* t_)
48 |     {
49 |         cout << "ao()" << endl;
50 |         MatrixXf Xw = Map<MatrixXf >(x_w_, 3, n_);
51 |         // cout << "Xw " << Xw << endl;
52 |         MatrixXf Xc = Map<MatrixXf >(x_c_, 3, n_);
53 |         // cout << "Xc " << Xc << endl;
54 |         AOOnlyPoseAdapter<float> adapter(Xc, Xw);
55 |         float f = 555.f;
56 |         adapter.setFocal(f, f);
57 | 
58 |         int updated_iter = 1000;
59 |         float thre_3d = 0.1;
60 |         float confidence = 0.99999f;
61 |         shinji_ransac2<float>(adapter, thre_3d, updated_iter, confidence);
62 |         cout << "updated_iter = " << updated_iter << endl;
63 |         cout << "inliers = " << adapter.getMaxVotes() << endl;
64 |         shinji_ls1<float>(adapter);
65 |         Matrix3f R_cw = adapter.getRcw().matrix();
66 |         Matrix3f Rp = R_cw.transpose();
67 |         float* data = Rp.data();
68 |         for (int i=0; i < 9; i++)
69 |             R_cw_[i] = data[i];
70 | 
71 |         Vector3f tw = adapter.gettw();
72 |         for (int i=0; i < 3; i++)
73 |             t_[i] = tw[i];
74 |         // cout << SE3_[0] << endl;
75 |     }
76 | 
77 |     void py2c(float* array, int N)
78 |     {
79 |         for (int i=0; i<N; i++) 
80 |             cout << array[i] << endl;
81 |     }
82 | }
83 | 
84 | 


--------------------------------------------------------------------------------
/Licenses.txt:
--------------------------------------------------------------------------------
 1 | Copyright (c) 2016 Shuda Li [lishuda1980@gmail.com]
 2 | 
 3 | Permission is hereby granted, free of charge, to any person obtaining a copy
 4 | of this software and associated documentation files (the "Software"), to deal
 5 | in the Software without restriction, including without limitation the rights
 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 7 | copies of the Software, and to permit persons to whom the Software is
 8 | furnished to do so, subject to the following conditions:
 9 | 
10 | The above copyright notice and this permission notice shall be included in
11 | all copies or substantial portions of the Software.
12 | 
13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
15 | FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
16 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 
19 | SOFTWARE.
20 | 


--------------------------------------------------------------------------------
/Parameters.yml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | #total # of points in world coordinate system 
 3 | total: 100
 4 | #outlier ratio of correspondences [0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9]
 5 | outlier: [0.1]
 6 | #noise level of 2-3 correspondences, unit pixel, [3, 6,  9,  12,  15]
 7 | noise_2d: [15 ]
 8 | #noise level of 3-3 correspondences, unit meter, [0.05, 0.075, 0.1, 0.125, 0.15] 
 9 | noise_3d: [0.05] 
10 | #noise level of N-N correspondences, unit radian, [2, 4, 6, 8, 10]
11 | noise_normal:[2]
12 | #RANSAC iterations: [200,1000,10000,100000]
13 | iteration: [300]
14 | #outlier rejection threshold for 2-3, unit radian, 0.0005
15 | thre_2d: 8
16 | #outlier rejection threshold for 3-3, unit meter, 0.1
17 | thre_3d: 0.2
18 | #outlier rejection threshold for N-N, unit radian, 0.001
19 | normal_thre: 0.1
20 | 
21 | #number of indepdent random test
22 | test_n:100 
23 | #The type of noise models
24 | #noise_model: Uniform
25 | noise_model: Gaussian
26 | #noise_model: Kinect
27 | 
28 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | rgbd_pose_estimation (Version 1.2 - 28th of Dec 2017)
 2 | ---------------------------------------
 3 | Update notes:
 4 | * Adapt to the latest Sophus library.
 5 | * Remove dependency on boost library.
 6 | * Fix a few bugs in the testing example.
 7 | 
 8 |   
 9 | Release highlights:
10 | --------------
11 | * It is a header only library. (No linking required)
12 | * Example codes available.
13 | * More accurate, precise and robust absolute pose estimation than using traditional Perspective-n-Point approaches or Absolute orientation approaches. 
14 | * Tested on Windows and Linux.
15 |   
16 | Build Notes:
17 | ------------
18 | Windows:
19 | 
20 | 	Requirements:
21 | 		1) Microsoft Visual C++
22 | 		   From: https://support.microsoft.com/en-us/kb/2977003/
23 | 		2) Eigen
24 | 		   From: http://eigen.tuxfamily.org/index.php?title=Main_Page
25 | 		   
26 | 	Optional requirements for the sample codes:
27 | 		1) OpenCV 3.0 
28 | 		   From: http://opencv.org/
29 | 	    2) CMake and CMake-GuI (To create project files)
30 | 		   From: http://www.cmake.org/
31 | 	
32 |     Building rgbd_pose_estimation:
33 | 	    As the library is composed of a bunch of headers of C++, no linking is needed.
34 | 	
35 | 	Building the example
36 | 	    1) Install the OpenCV 3.0 Library 
37 | 		2) Create a folder 'build' 
38 | 		3) Run CMake-Gui and specify the path to dependences. 
39 | 		4) Press 'configure' and then 'generate' button
40 | 		5) Open the 'RGBDPoseEstimation.sln' and compile it.
41 | 		
42 | Linux:
43 | 
44 | 	Requirements:
45 | 		1) Eigen
46 | 		   From: http://eigen.tuxfamily.org/index.php?title=Main_Page
47 | 		   
48 | 	Optional requirements for the sample codes:
49 | 		1) OpenCV 3.0 
50 | 		   From: http://opencv.org/
51 | 	    2) CMake and CMake-GuI (To create project files)
52 | 		   From: http://www.cmake.org/
53 | 	
54 |     Building rgbd_pose_estimation:
55 | 	    As the library is composed of a bunch of headers of C++, no linking is needed.
56 | 	
57 | 	Building the example
58 | 	    1) Install the OpenCV 3.0 Library 
59 | 		2) Create a folder 'build' 
60 | 		3) Run CMake-Gui and specify the path to dependences. 
61 | 		4) Press 'configure' and then 'generate' button
62 | 		5) make
63 | 	   
64 | 


--------------------------------------------------------------------------------
/SimpleMain.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #define _USE_MATH_DEFINES
  3 | 
  4 | #include <Eigen/Core>
  5 | #include "AbsoluteOrientation.hpp"
  6 | #include "AOOnlyPoseAdapter.hpp"
  7 | #include "Simulator.hpp"
  8 | #include <iostream>
  9 | #include <string>
 10 | 
 11 | #include <opencv2/opencv.hpp>
 12 | 
 13 | 
 14 | using namespace Eigen;
 15 | using namespace std;
 16 | using namespace cv;
 17 | //using namespace btl::utility;
 18 | 
 19 | #define data_type float
 20 | 
 21 | void test_3d_3d(){
 22 |     cout << "%test_3d_3d()" << endl;
 23 |     const Matrix<data_type, 3, 1> t = generate_random_translation_uniform<data_type>(5.0);
 24 |     const Sophus::SO3<data_type> R = generate_random_rotation<data_type>(M_PI / 2, false);
 25 | 
 26 |     //cout << t << endl;
 27 |     //cout << R << endl;
 28 | 
 29 |     //derive correspondences based on random point-cloud
 30 |     int total = 100;
 31 | 
 32 |     data_type  or_3d = 0.1; //outlier
 33 |     data_type  n3d = 0.1; //3-D noise
 34 | 
 35 |     int iteration = 100000;
 36 |     int updated_iter = iteration;
 37 |     int test_n = 10;
 38 |     string noise_model = "Gaussian";
 39 | 
 40 |     data_type thre_3d = 0.25; //meter
 41 | 
 42 |     data_type min_depth = 0.4;
 43 |     data_type max_depth = 8.; //for Gaussian or uniform noise
 44 |     data_type f = 585.;
 45 |     data_type confidence = 0.9999;
 46 |     Matrix<data_type, -1, -1> e_s(2, test_n);
 47 |     Matrix<data_type, -1, -1> e_l(2, test_n);
 48 | 
 49 |     cout << "total = " << total << endl;
 50 |     cout << "noise_thre_3d = " << thre_3d << endl;
 51 |     cout << "outlier ratio = " << or_3d << endl;
 52 |     cout << "3d noise= " << n3d << endl;
 53 | 
 54 |     for (int jj = 0; jj < test_n; jj++){
 55 |         Matrix<data_type, Dynamic, Dynamic> Q, P;
 56 |         Matrix<data_type, Dynamic, Dynamic> all_weights(total, 3);
 57 |         if (!noise_model.compare("Uniform")){
 58 |             simulate_3d_3d_correspondences<data_type>(R, t, total, n3d, or_3d,  min_depth, max_depth, f, false,
 59 |                 &Q, &P, &all_weights);
 60 |         }
 61 |         else if (!noise_model.compare("Gaussian")){
 62 |             simulate_3d_3d_correspondences<data_type>(R, t, total, n3d, or_3d,  min_depth, max_depth, f, true,
 63 |                 &Q, &P, &all_weights);
 64 |         }
 65 | 
 66 |         AOOnlyPoseAdapter<data_type> adapter(P, Q);
 67 |         adapter.setFocal(f, f);
 68 |         adapter.setWeights(all_weights);
 69 |         //estimate camera pose using shinji_ransac
 70 |         updated_iter = iteration;
 71 |         shinji_prosac<data_type>(adapter, thre_3d, updated_iter, confidence);
 72 |         cout << "prosac max " << adapter.getMaxVotes() << endl;
 73 |         cout << "prosac it " << updated_iter << endl;
 74 |         shinji_ls1<data_type>(adapter);
 75 |         e_s.col(jj) = calc_percentage_err<data_type>(R, t, &adapter);
 76 | 
 77 |         updated_iter = iteration;
 78 |         shinji_ransac2<data_type>(adapter, thre_3d, updated_iter, confidence);
 79 |         cout << "ransac max " << adapter.getMaxVotes() << endl;
 80 |         cout << "ransac it " << updated_iter << endl;
 81 |         cout << endl;
 82 |         shinji_ls1<data_type>(adapter);
 83 |         e_l.col(jj) = calc_percentage_err<data_type>(R, t, &adapter);
 84 |     }
 85 | 
 86 |     cout << "prosac t" << "_s =[" << e_s.row(0) << "]';" << endl;
 87 |     cout << "prosac r" << "_s =[" << e_s.row(1) << "]';" << endl;
 88 |     cout << "ransac t" << "_l =[" << e_l.row(0) << "]';" << endl;
 89 |     cout << "ransac r" << "_l =[" << e_l.row(1) << "]';" << endl;
 90 |     cout << endl;
 91 | 
 92 |     return;
 93 | }
 94 | 
 95 | void test_3d_2d(){
 96 |     cout << "%test_3d_2d()" << endl;
 97 |     const Matrix<data_type, 3, 1> t = generate_random_translation_uniform<data_type>(5.0);
 98 |     const Sophus::SO3<data_type> R = generate_random_rotation<data_type>(M_PI / 2, false);
 99 | 
100 |     //cout << t << endl;
101 |     //cout << R << endl;
102 | 
103 |     //derive correspondences based on random point-cloud
104 |     int total = 100;
105 |  
106 |     data_type  or_3d = 0.1; //outlier
107 |     data_type  n2d = 0.1; //3-D noise
108 | 
109 |     int iteration = 100000;
110 |     int updated_iter = iteration;
111 |     int test_n = 10;
112 |     string noise_model = "Gaussian";
113 | 
114 |     data_type thre_2d = 0.02;//1-cos alpha
115 | 
116 |     data_type min_depth = 0.4;
117 |     data_type max_depth = 8.; //for Gaussian or uniform noise
118 |     data_type f = 585.;
119 |     data_type confidence = 0.8;
120 |     Matrix<data_type, -1, -1> e_s(2, test_n);
121 |     Matrix<data_type, -1, -1> e_l(2, test_n);
122 | 
123 |     cout << "total = " << total << endl;
124 |     cout << "noise_thre_2d = " << thre_2d << endl;
125 |     cout << "outlier ratio = " << or_3d << endl;
126 |     cout << "3d noise= " << n2d << endl;
127 | 
128 |     for (int jj = 0; jj < test_n; jj++){
129 |         Matrix<data_type, Dynamic, Dynamic> Q, P, U;
130 |         Matrix<data_type, Dynamic, Dynamic> all_weights(total, 3);
131 |         if (!noise_model.compare("Uniform")){
132 |             simulate_2d_3d_correspondences<data_type>(R, t, total, n2d, or_3d, min_depth, max_depth, f, false,
133 |                 &Q, &U, &P, &all_weights);
134 |         }
135 |         else if (!noise_model.compare("Gaussian")){
136 |             simulate_2d_3d_correspondences<data_type>(R, t, total, n2d, or_3d, min_depth, max_depth, f, true,
137 |                 &Q, &U, &P, &all_weights);
138 |         }
139 | 
140 |         PnPPoseAdapter<data_type> adapter(U,Q);
141 |         adapter.setFocal(f, f);
142 |         adapter.setWeights(all_weights);
143 |         //estimate camera pose using shinji_ransac
144 |         updated_iter = iteration;
145 |         kneip_prosac<data_type>(adapter, thre_2d, updated_iter, confidence);
146 |         cout << "prosac max " << adapter.getMaxVotes() << endl;
147 |         cout << "prosac it " << updated_iter << endl;
148 |         e_s.col(jj) = calc_percentage_err<data_type>(R, t, &adapter);
149 | 
150 |         updated_iter = iteration;
151 |         kneip_ransac<data_type>(adapter, thre_2d, updated_iter, confidence);
152 |         cout << "ransac max " << adapter.getMaxVotes() << endl;
153 |         cout << "ransac it " << updated_iter << endl;
154 |         cout << endl;
155 |         e_l.col(jj) = calc_percentage_err<data_type>(R, t, &adapter);
156 |     }
157 | 
158 |     cout << "t" << "_s =[" << e_s.row(0) << "]';" << endl;
159 |     cout << "r" << "_s =[" << e_s.row(1) << "]';" << endl;
160 | 
161 |     cout << "t" << "_l =[" << e_l.row(0) << "]';" << endl;
162 |     cout << "r" << "_l =[" << e_l.row(1) << "]';" << endl;
163 |     cout << endl;
164 | 
165 |     return;
166 | }
167 | 
168 | 
169 | void test_3d_3d_2d(){
170 |     cout << "%test_3d_3d_2d()" << endl;
171 |     const Matrix<data_type, 3, 1> t = generate_random_translation_uniform<data_type>(5.0);
172 |     const Sophus::SO3<data_type> R = generate_random_rotation<data_type>(M_PI / 2, false);
173 | 
174 |     //cout << t << endl;
175 |     //cout << R << endl;
176 | 
177 |     //derive correspondences based on random point-cloud
178 |     int total = 100;
179 | 
180 |     data_type  or_3d = 0.1; //outlier
181 |     data_type  n3d = 0.1; //3-D noise
182 |     data_type  n2d = 0.1; //3-D noise
183 | 
184 |     int iteration = 100000;
185 |     int updated_iter = iteration;
186 |     int test_n = 10;
187 |     string noise_model = "Gaussian";
188 | 
189 |     data_type thre_3d = 0.25; //meter
190 |     data_type thre_2d = 0.25; //meter
191 | 
192 |     data_type min_depth = 0.4;
193 |     data_type max_depth = 8.; //for Gaussian or uniform noise
194 |     data_type f = 585.;
195 |     data_type confidence = 0.9999;
196 |     Matrix<data_type, -1, -1> e_l(2, test_n);
197 |     Matrix<data_type, -1, -1> e_s(2, test_n);
198 |     Matrix<data_type, -1, -1> e_k(2, test_n);
199 |     Matrix<data_type, -1, -1> e_sk(2, test_n);
200 | 
201 |     cout << "total = " << total << endl;
202 |     cout << "noise_thre_3d = " << thre_3d << endl;
203 |     cout << "outlier ratio = " << or_3d << endl;
204 |     cout << "3d noise= " << n3d << endl;
205 | 
206 |     for (int jj = 0; jj < test_n; jj++){
207 |         Matrix<data_type, Dynamic, Dynamic> Q, P, U;
208 |         Matrix<data_type, Dynamic, Dynamic> all_weights(total, 3);
209 |         if (!noise_model.compare("Uniform")){
210 |             simulate_2d_3d_3d_correspondences<data_type>(R, t, total, n2d, n3d, or_3d, min_depth, max_depth, f, false,
211 |                 &Q, &U, &P, &all_weights);
212 |         }
213 |         else if (!noise_model.compare("Gaussian")){
214 |             simulate_2d_3d_3d_correspondences<data_type>(R, t, total, n2d, n3d, or_3d, min_depth, max_depth, f, true,
215 |                 &Q, &U, &P, &all_weights);
216 |         }
217 | 
218 |         AOPoseAdapter<data_type> adapter(U, P, Q); //bv, Xg, Xc
219 |         adapter.setFocal(f, f);
220 |         adapter.setWeights(all_weights);
221 |         //estimate camera pose using shinji_ransac
222 |         updated_iter = iteration;
223 |         shinji_kneip_prosac<data_type>(adapter, thre_3d, thre_2d, updated_iter, confidence);
224 |         cout << "sk prosac max " << adapter.getMaxVotes() << endl;
225 |         cout << "sk prosac it " << updated_iter << endl;
226 |         shinji_ls<data_type>(adapter);
227 |         e_s.col(jj) = calc_percentage_err<data_type>(R, t, &adapter);
228 | 
229 |         updated_iter = iteration;
230 |         shinji_kneip_ransac<data_type>(adapter, thre_3d, thre_2d, updated_iter, confidence);
231 |         cout << "sk ransac max " << adapter.getMaxVotes() << endl;
232 |         cout << "sk ransac it " << updated_iter << endl;
233 |         cout << endl;
234 |         shinji_ls<data_type>(adapter);
235 |         e_l.col(jj) = calc_percentage_err<data_type>(R, t, &adapter);
236 | 
237 |         // updated_iter = iteration;
238 |         // shinji_ransac<data_type>(adapter, thre_3d, updated_iter, confidence);
239 |         // cout << "shinji ransac max " << adapter.getMaxVotes() << endl;
240 |         // cout << "shinji ransac it " << updated_iter << endl;
241 |         // cout << endl;
242 |         // shinji_ls<data_type>(adapter);
243 |         // e_s.col(jj) = calc_percentage_err<data_type>(R, t, &adapter);
244 | 
245 |         // updated_iter = iteration;
246 |         // kneip_ransac<data_type>(adapter, thre_2d, updated_iter, confidence);
247 |         // cout << "kneip ransac max " << adapter.getMaxVotes() << endl;
248 |         // cout << "kneip ransac it " << updated_iter << endl;
249 |         // cout << endl;
250 |         // e_k.col(jj) = calc_percentage_err<data_type>(R, t, &adapter);
251 |     }
252 | 
253 |     cout << "sk prosac t" << "_s =[" << e_s.row(0) << "]';" << endl;
254 |     cout << "sk prosac r" << "_s =[" << e_s.row(1) << "]';" << endl;
255 |     // cout << "kneip ransac t" << "_s =[" << e_k.row(0) << "]';" << endl;
256 |     // cout << "kneip ransac r" << "_s =[" << e_k.row(0) << "]';" << endl;
257 |     cout << "sk ransac t" << "_l =[" << e_l.row(0) << "]';" << endl;
258 |     cout << "sk ransac r" << "_l =[" << e_l.row(1) << "]';" << endl;
259 |     cout << endl;
260 | 
261 |     return;
262 | }
263 | 
264 | 
265 | void test_prosac(){
266 |     ProsacSampler<data_type> ps(4, 100);
267 |     for (int i = 0; i < 1000; ++i)
268 |     {
269 |         vector<int> select;
270 |         ps.sample(&select);
271 |         cout << i << " ";
272 |         for(int j=0; j < (int)select.size(); j++)
273 |             cout << select[j] << " ";
274 |         cout << endl;
275 |     }
276 | }
277 | 
278 | int main()
279 | {
280 |     // test_prosac();
281 |     // test_3d_2d();
282 |     // test_3d_3d();
283 |     test_3d_3d_2d();
284 | 
285 |     return 0;
286 | }
287 | 
288 | 


--------------------------------------------------------------------------------
/TestMain.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #define _USE_MATH_DEFINES
  3 | 
  4 | #include <Eigen/Core>
  5 | //#include "common/Converters.hpp"
  6 | #include "AbsoluteOrientation.hpp"
  7 | #include "AbsoluteOrientationNormal.hpp"
  8 | #include "PnPPoseAdapter.hpp"
  9 | #include "AOPoseAdapter.hpp"
 10 | #include "NormalAOPoseAdapter.hpp"
 11 | #include "P3P.hpp"
 12 | #include "MinimalSolvers.hpp"
 13 | #include "Simulator.hpp"
 14 | #include <iostream>
 15 | #include <string>
 16 | 
 17 | #include <opencv2/opencv.hpp>
 18 | 
 19 | 
 20 | using namespace Eigen;
 21 | using namespace std;
 22 | using namespace cv;
 23 | //using namespace btl::utility;
 24 | 
 25 | void cvtCV(const MatrixXd& Q_, const MatrixXd U_, const double& f_, Mat* m3D_, Mat* m2D_){
 26 | 	m2D_->create(1, U_.cols(), CV_64FC2);
 27 | 	m3D_->create(1, Q_.cols(), CV_64FC3);
 28 | 	double* p2D = (double*)m2D_->data;
 29 | 	double* p3D = (double*)m3D_->data;
 30 | 	for (int c = 0; c < U_.cols(); c++)	{
 31 | 		*p3D++ = Q_(0, c);
 32 | 		*p3D++ = Q_(1, c);
 33 | 		*p3D++ = Q_(2, c);
 34 | 
 35 | 		*p2D++ = U_(0, c) / U_(2, c) *f_;
 36 | 		*p2D++ = U_(1, c) / U_(2, c) *f_;
 37 | 	}
 38 | 	return;
 39 | }
 40 | 
 41 | void cvtEigen(const Mat& m, const Mat& t, Matrix3d* mm, Vector3d* tt) {
 42 | 	for (int r = 0; r < m.rows; r++){
 43 | 		for (int c = 0; c < m.cols; c++)
 44 | 		{
 45 | 			(*mm)(r, c) = m.at<double>(r, c);
 46 | 		}
 47 | 		(*tt)(r) = t.at<double>(r);
 48 | 	}
 49 | 	return;
 50 | }
 51 | 
 52 | void test_all(){
 53 | 	cout << "%test_all()" << endl;
 54 | 	const Vector3d t = generate_random_translation_uniform<double>(5.0);
 55 | 	const Sophus::SO3<double> R = generate_random_rotation<double>(M_PI / 2, false);
 56 | 
 57 | 	//cout << t << endl;
 58 | 	//cout << R << endl;
 59 | 
 60 | 	//derive correspondences based on random point-cloud
 61 | 	int total = 100;
 62 | 	vector<double> v_noise_2d;
 63 | 	vector<double> v_noise_normal;
 64 | 	vector<double> v_noise_3d;
 65 | 
 66 | 	vector<double> v_or;
 67 | 	vector<int> v_iter;
 68 | 	double or_2d = 0.8;
 69 | 	double or_3d = 0.8;
 70 | 	double or_nl = 0.1;
 71 | 
 72 | 	int iteration = 1000;
 73 | 	int updated_iter = iteration;
 74 | 	int test_n = 1;
 75 | 	string noise_model;
 76 | 
 77 | 	double thre_2d = 0.02;//1-cos alpha
 78 | 	double thre_3d = 0.03; //meter
 79 | 	double thre_nl = 0.01;
 80 | 
 81 | 	double min_depth = 0.4;
 82 | 	double max_depth = 8.; //for Gaussian or uniform noise
 83 | 	double f = 585.;
 84 | 	double confidence = 0.99999;
 85 | 
 86 | 
 87 | #ifdef __gnu_linux__
 88 | 	cv::FileStorage cFSRead("../Parameters.yml", cv::FileStorage::READ);
 89 | #elif _WIN32
 90 | 	cv::FileStorage cFSRead("..\\Parameters.yml", cv::FileStorage::READ);
 91 | #else 
 92 | #error "OS not supported!"
 93 | #endif
 94 | 	if (!cFSRead.isOpened()) {
 95 | 		std::cout << "Load test.yml failed.";
 96 | 		return;
 97 | 	}
 98 | 	cFSRead["total"] >> total;
 99 | 	cFSRead["outlier"] >> v_or;
100 | 	cFSRead["noise_2d"] >> v_noise_2d;
101 | 	cFSRead["noise_3d"] >> v_noise_3d;
102 | 	cFSRead["noise_normal"] >> v_noise_normal;
103 | 	cFSRead["iteration"] >> v_iter;
104 | 	cFSRead["thre_2d"] >> thre_2d;
105 | 	cFSRead["thre_3d"] >> thre_3d;
106 | 	cFSRead["normal_thre"] >> thre_nl;
107 | 	cFSRead["test_n"] >> test_n;
108 | 	cFSRead["noise_model"] >> noise_model;
109 | 
110 | 	vector<vector<int> > combination;
111 | 
112 | 	for (int a = 0; a < (int)v_noise_3d.size(); a++)	{
113 | 		vector<int> sub;
114 | 		sub.push_back(a);
115 | 		sub.push_back(a);
116 | 		sub.push_back(a);
117 | 		combination.push_back(sub);
118 | 	}
119 | 
120 | 	cout << "close all;" << endl;
121 | 	cout << "%total = " << total << endl;
122 | 	cout << "%noise_2d = " << v_noise_2d << " outlier = " << v_or << " noise_3d = " << v_noise_3d << " noise_normal = " << v_noise_normal << endl;
123 | 	cout << "%noise_thre_2d = " << thre_2d << " noise_thre_3d = " << thre_3d << " normal_thre = " << thre_nl << endl;
124 | 	MatrixXd e_nsk(2, test_n), e_sk(2, test_n), e_s(2, test_n), e_k(2, test_n), e_l(2, test_n);// m0.setZero(); m1.setZero(); m2.setZero(); m3.setZero();
125 | 	MatrixXd e_opt(2, test_n), e_ns(2, test_n), e_nk(2, test_n), e_dw(2, test_n);// m0.setZero(); m1.setZero(); m2.setZero(); m3.setZero();
126 | 	for (int cc = 0; cc < (int)combination.size(); cc++)
127 | 	{
128 | 		for (int oo = 0; oo < (int)v_or.size(); oo++)
129 | 		{
130 | 			for (int ii = 0; ii < (int)v_iter.size(); ii++)
131 | 			{
132 | 				iteration = v_iter[ii];
133 | 				cout << "%iteration = " << iteration << endl;
134 | 				
135 | 				or_2d = or_3d = or_nl = v_or[oo];
136 | 				cout << "%outlier ratio = " << or_2d << endl;
137 | 
138 | 				int idx_3d = combination[cc][0];
139 | 				int idx_2d = combination[cc][1];
140 | 				int idx_n = combination[cc][2];
141 | 
142 | 				double n2d = v_noise_2d[idx_2d]; //2-D noise 
143 | 				double n3d = v_noise_3d[idx_3d]; //3-D noise
144 | 				double nnl = v_noise_normal[idx_n] / 180.*M_PI;//normal noise
145 | 
146 | 				for (int jj = 0; jj < test_n; jj++){
147 | 					MatrixXd Q, P, U;
148 | 					MatrixXd M, N;
149 | 					Matrix<double, Dynamic, Dynamic> all_weights(total, 3);
150 | 					if (!noise_model.compare("Uniform")){
151 | 						simulate_2d_3d_nl_correspondences<double>(R, t, total, n2d, or_2d, n3d, or_3d, nnl, or_nl, min_depth, max_depth, f, false,
152 | 							&Q, &M, &P, &N, &U, &all_weights);
153 | 					}
154 | 					else if (!noise_model.compare("Gaussian")){
155 | 						simulate_2d_3d_nl_correspondences<double>(R, t, total, n2d, or_2d, n3d, or_3d, nnl, or_nl, min_depth, max_depth, f, true,
156 | 							&Q, &M, &P, &N, &U, &all_weights);
157 | 					}
158 | 					else if (!noise_model.compare("Kinect")){
159 | 						max_depth = 3.;
160 | 						simulate_kinect_2d_3d_nl_correspondences<double>(R, t, total, n2d, or_2d, or_3d, nnl, or_nl, min_depth, max_depth, f,
161 | 							&Q, &M, &P, &N, &U, &all_weights);
162 | 					}
163 | 					NormalAOPoseAdapter<double> adapter(U, P, N, Q, M);
164 | 					adapter.setFocal(f, f);
165 | 					//convert to opencv data structure
166 | 					updated_iter = iteration;
167 | 					Mat m2D, m3D, cameraMatrix, inliers, rvec, tvec;
168 | 					cameraMatrix.create(3, 3, CV_64FC1); cameraMatrix.setTo(0.);
169 | 					cameraMatrix.at<double>(0, 0) = f;
170 | 					cameraMatrix.at<double>(1, 1) = f;
171 | 					cameraMatrix.at<double>(2, 2) = 1.;
172 | 					cvtCV(Q, U, f, &m3D, &m2D);
173 | 					cv::solvePnPRansac(m3D, m2D, cameraMatrix, Mat(), rvec, tvec, false, updated_iter, 8.f, confidence, inliers, cv::SOLVEPNP_EPNP);
174 | 					cv::solvePnPRansac(m3D, m2D, cameraMatrix, Mat(), rvec, tvec, true, updated_iter, 8.f, confidence, inliers, cv::SOLVEPNP_ITERATIVE);
175 | 					//convert to rotation matrix
176 | 					Mat cvmRw;	Rodrigues(rvec, cvmRw);
177 | 					Matrix3d RR;
178 | 					Vector3d tt;
179 | 					cvtEigen(cvmRw, tvec, &RR, &tt);
180 | 					adapter.setRcw(RR);
181 | 					adapter.sett(tt);
182 | 					e_l.col(jj) = calc_percentage_err<double>(R, t, &adapter);
183 | 
184 | 					//estimate camera pose using kneip_ransac
185 | 					updated_iter = iteration;
186 | 					kneip_ransac<double>(adapter, thre_2d, updated_iter, confidence);
187 | 					e_k.col(jj) = calc_percentage_err<double>(R, t, &adapter);
188 | 
189 | 					//estimate camera pose using shinji_ransac
190 | 					updated_iter = iteration;
191 | 					shinji_ransac<double>(adapter, thre_3d, updated_iter, confidence);
192 | 					e_s.col(jj) = calc_percentage_err<double>(R, t, &adapter);
193 | 
194 | 					//estimate camera pose using shinji_kneip_ransac
195 | 					updated_iter = iteration;
196 | 					shinji_kneip_ransac<double>(adapter, thre_3d, thre_2d, updated_iter, confidence);
197 | 					adapter.getInlierIdx();
198 | 					e_sk.col(jj) = calc_percentage_err<double>(R, t, &adapter);
199 | 						
200 | 					//estimate camera pose using nl_kneip_ransac
201 | 					updated_iter = iteration;
202 | 					nl_kneip_ransac<double>(adapter, thre_2d, thre_nl, updated_iter, confidence);
203 | 					e_nk.col(jj) = calc_percentage_err<double>(R, t, &adapter);
204 | 
205 | 					//estimate camera pose using nl_shinji_ransac
206 | 					updated_iter = iteration;
207 | 					nl_shinji_ransac<double>(adapter, thre_3d, thre_nl, updated_iter, confidence);
208 | 					e_ns.col(jj) = calc_percentage_err<double>(R, t, &adapter);
209 | 
210 | 					updated_iter = iteration;
211 | 					nl_shinji_kneip_ransac<double>(adapter, thre_3d, thre_2d, thre_nl, updated_iter, confidence);
212 | 					e_nsk.col(jj) = calc_percentage_err<double>(R, t, &adapter);
213 | 
214 | 					//estimate least square optimized solutions
215 | 					nl_shinji_kneip_ls<double>(adapter);
216 | 					e_opt.col(jj) = calc_percentage_err<double>(R, t, &adapter);
217 | 
218 | 					//estimate using dynamic weights
219 | 					adapter.setWeights(all_weights);
220 | 					nl_shinji_kneip_ls<double>(adapter);
221 | 					e_dw.col(jj) = calc_percentage_err<double>(R, t, &adapter);
222 | 				}
223 | 
224 | 				cout << endl;
225 | 				cout << "t" << cc << "_dw=[" << e_dw.row(0) << "]';" << endl;  //row(0) translation error
226 | 				cout << "t" << cc << "_opt=[" << e_opt.row(0) << "]';" << endl;
227 | 				cout << "t" << cc << "_nsk =[" << e_nsk.row(0) << "]';" << endl;
228 | 				cout << "t" << cc << "_ns=[" << e_ns.row(0) << "]';" << endl;
229 | 				cout << "t" << cc << "_nk=[" << e_nk.row(0) << "]';" << endl;
230 | 				cout << "t" << cc << "_sk =[" << e_sk.row(0) << "]';" << endl;
231 | 				cout << "t" << cc << "_s =[" << e_s.row(0) << "]';" << endl;
232 | 				cout << "t" << cc << "_k =[" << e_k.row(0) << "]';" << endl;
233 | 				cout << "t" << cc << "_l =[" << e_l.row(0) << "]';" << endl;
234 | 				cout << endl;
235 | 				cout << "r" << cc << "_dw=[" << e_dw.row(1) << "]';" << endl; //row(1) rotational error
236 | 				cout << "r" << cc << "_opt=[" << e_opt.row(1) << "]';" << endl;
237 | 				cout << "r" << cc << "_nsk =[" << e_nsk.row(1) << "]';" << endl;
238 | 				cout << "r" << cc << "_ns=[" << e_ns.row(1) << "]';" << endl;
239 | 				cout << "r" << cc << "_nk=[" << e_nk.row(1) << "]';" << endl;
240 | 				cout << "r" << cc << "_sk =[" << e_sk.row(1) << "]';" << endl;
241 | 				cout << "r" << cc << "_s =[" << e_s.row(1) << "]';" << endl;
242 | 				cout << "r" << cc << "_k =[" << e_k.row(1) << "]';" << endl;
243 | 				cout << "r" << cc << "_l =[" << e_l.row(1) << "]';" << endl;
244 | 				cout << endl;
245 | 
246 | 				cout << "figure;" << endl;
247 | 				cout << "boxplot([t" << cc << "_dw, t" << cc << "_opt, t" << cc << "_nsk, t" << cc << "_ns, t" << cc << "_nk, t" << cc << "_sk, t" << cc << "_s, t" << cc << "_k, t" << cc << "_l] ," <<
248 | 				          "{ 'dw', 'opt', 'nl+s+k', 'nl+s', 'nl+k', 's+k', 's', 'k', 'l' }); " << endl;
249 | 				cout << "axis([0.5,9.5,0,30]);" << endl;
250 | 				cout << "xlabel('approaches') " << endl;
251 | 				cout << "ylabel('translational error (%)')" << endl;
252 | 				cout << "title('noise_{3d} = " << n3d << "; noise_{2d} = " << n2d << "; noise_{nl} = " << nnl << " " << noise_model << "')" << endl;
253 | 				cout << "set(gcf, 'Position', [0 0 400 480], 'PaperSize', [400 600]); " << endl;
254 | 				//cout << "print('tfig" << cc << ".eps','-depsc');" << endl;
255 | 				cout << endl;
256 | 				cout << "figure;" << endl;
257 | 				cout << "boxplot([r" << cc << "_dw, r" << cc << "_opt, r" << cc << "_nsk, r" << cc << "_ns, r" << cc << "_nk, r" << cc << "_sk, r" << cc << "_s, r" << cc << "_k, t" << cc << "_l] ," << 
258 | 						"{'dw', 'op','nl+s+k','nl+s','nl+k','s+k','s','k', 'l'});" << endl;
259 | 				cout << "axis([0.5,9.5,0,30]);" << endl;
260 | 				cout << "xlabel('approaches') " << endl;
261 | 				cout << "ylabel('rotational error (%)')" << endl;
262 | 				cout << "title('noise_{3d} = " << n3d << "; noise_{2d} = " << n2d << "; noise_{nl} = " << nnl << " " << noise_model << "')" << endl;
263 | 				cout << "set(gcf, 'Position', [400 0 400 480], 'PaperSize', [400 600]); " << endl;
264 | 				//cout << "print('rfig" << cc << ".eps','-depsc');" << endl;
265 | 				cout << endl;
266 | 			}
267 | 
268 | 		}
269 | 
270 | 	}
271 | 
272 | 	cout << "SimpT = [";
273 | 	for (int cc = 0; cc < (int)combination.size(); cc++)
274 | 	{
275 | 		//cout << "t" << cc << "_dw ";
276 | 		cout << "t" << cc << "_opt ";
277 | 		cout << "t" << cc << "_nsk ";
278 | 		cout << "t" << cc << "_s ";
279 | 		cout << "t" << cc << "_k ";
280 | 		cout << "t" << cc << "_l ";
281 | 	}
282 | 	cout << "];" << endl;
283 | 
284 | 	cout << "FullT = [";
285 | 	for (int cc = 0; cc < (int)combination.size(); cc++)
286 | 	{
287 | 		//cout << "t" << cc << "_dw ";
288 | 		cout << "t" << cc << "_opt ";
289 | 		cout << "t" << cc << "_nsk ";
290 | 		cout << "t" << cc << "_ns ";
291 | 		cout << "t" << cc << "_nk ";
292 | 		cout << "t" << cc << "_sk ";
293 | 		cout << "t" << cc << "_s ";
294 | 		cout << "t" << cc << "_k ";
295 | 		cout << "t" << cc << "_l ";
296 | 	}
297 | 	cout << "];" << endl;
298 | 
299 | 	cout << "SimpR = [";
300 | 	for (int cc = 0; cc < (int)combination.size(); cc++)
301 | 	{
302 | 		//cout << "t" << cc << "_dw ";
303 | 		cout << "r" << cc << "_opt ";
304 | 		cout << "r" << cc << "_nsk ";
305 | 		cout << "r" << cc << "_s ";
306 | 		cout << "r" << cc << "_k ";
307 | 		cout << "r" << cc << "_l ";
308 | 	}
309 | 	cout << "];" << endl;
310 | 
311 | 	cout << "FullR = [";
312 | 	for (int cc = 0; cc < (int)combination.size(); cc++)
313 | 	{
314 | 		//cout << "r" << cc << "_dw ";
315 | 		cout << "r" << cc << "_opt ";
316 | 		cout << "r" << cc << "_nsk ";
317 | 		cout << "r" << cc << "_ns ";
318 | 		cout << "r" << cc << "_nk ";
319 | 		cout << "r" << cc << "_sk ";
320 | 		cout << "r" << cc << "_s ";
321 | 		cout << "r" << cc << "_k ";
322 | 		cout << "r" << cc << "_l ";
323 | 	}
324 | 	cout << "];" << endl;
325 | 	return;
326 | }
327 | 
328 | 
329 | 
330 | int main()
331 | {
332 | 
333 | 	test_all();
334 | 	
335 | 
336 | 	return 0;
337 | }
338 | 
339 | 


--------------------------------------------------------------------------------
/pose/AOOnlyPoseAdapter.hpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * \file CentralAbsoluteAdapter.hpp
  3 |  * \brief Adapter-class for passing bearing-vector-to-point correspondences to
  4 |  *        the central absolute-pose algorithms. It maps opengv types
  5 |  *        back to opengv types.
  6 |  */
  7 | 
  8 | #ifndef _AO_ONLY_POSE_ADAPTER_HEADER_
  9 | #define _AO_ONLY_POSE_ADAPTER_HEADER_
 10 | 
 11 | #include <stdlib.h>
 12 | #include <vector>
 13 | #include <PoseAdapterBase.hpp>
 14 | 
 15 | using namespace std;
 16 | using namespace Eigen;
 17 | /**
 18 |  * \brief The namespace for the absolute pose methods.
 19 |  */
 20 | 
 21 | /**
 22 |  * Check the documentation of the parent-class to understand the meaning of
 23 |  * an AbsoluteAdapter. This child-class is for the central case and holds data
 24 |  * in form of references to opengv-types.
 25 |  */
 26 | template<typename Tp>
 27 | class AOOnlyPoseAdapter : public PoseAdapterBase<Tp>
 28 | {
 29 | 
 30 | protected:
 31 | 	using PoseAdapterBase<Tp>::_t_w;
 32 | 	using PoseAdapterBase<Tp>::_R_cw;
 33 | 
 34 | public:
 35 | 	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 36 | 	typedef typename PoseAdapterBase<Tp>::Vector3 Vector3;
 37 | 	typedef typename PoseAdapterBase<Tp>::SO3_T SO3_T;
 38 | 	typedef typename PoseAdapterBase<Tp>::Point3 Point3;
 39 | 	
 40 |     typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
 41 | 
 42 |   /**
 43 |    * \brief Constructor. See protected class-members to understand parameters
 44 |    */
 45 |    AOOnlyPoseAdapter(
 46 | 	  const MatrixX & points_c,
 47 | 	  const MatrixX & points_g);
 48 |   /**
 49 |    * \brief Constructor. See protected class-members to understand parameters
 50 |    */
 51 |   AOOnlyPoseAdapter(
 52 | 	  const MatrixX & points_c,
 53 | 	  const MatrixX & points_g,
 54 |       const SO3_T & R );
 55 |   /**
 56 |    * \brief Constructor. See protected class-members to understand parameters
 57 |    */
 58 |   AOOnlyPoseAdapter(
 59 | 	  const MatrixX & points_c,
 60 | 	  const MatrixX & points_g,
 61 |       const Vector3 & t,
 62 |       const SO3_T & R );
 63 |   /**
 64 |    * Destructor
 65 |    */
 66 |   virtual ~AOOnlyPoseAdapter();
 67 | 
 68 |   //Access of correspondences
 69 |   
 70 |   bool isInlier33(int index) const;
 71 |   Tp weight33(int index) const;
 72 |   /** See parent-class */
 73 |   virtual Point3 getBearingVector(int index) const { return Point3();} 
 74 |   virtual Point3 getPointCurr( int index ) const;
 75 |   virtual Point3 getPointGlob(int index) const;
 76 |   virtual Tp getWeight(int index) const{ return Tp(1.); }
 77 |   virtual int getNumberCorrespondences() const { return _points_g.cols(); }
 78 | 
 79 |   void setMaxVotes(int votes){  _max_votes = votes;  };
 80 | 
 81 |   int getMaxVotes() { return _max_votes; };
 82 | 
 83 | 
 84 |   virtual bool isValid( int index ) const;
 85 |   virtual void setInlier(const Matrix<short, Dynamic, Dynamic>& inliers);
 86 |   virtual void setWeights(const Matrix<Tp, Dynamic, Dynamic>& weights);
 87 |   virtual void printInlier() const;
 88 |   const vector<short>& getInlierIdx() const { return _vInliersAO; }
 89 |   void cvtInlier();
 90 |   void sortIdx();
 91 |   void getSortedIdx(vector<int>& select_) const;
 92 | protected:
 93 |   /** Reference to the 3-D points in the camera-frame */
 94 |   const MatrixX & _points_c; 
 95 |   const MatrixX & _points_g; 
 96 |   /** flags of inliers. */
 97 |   Matrix<short, Dynamic, 1> _inliers_3d;
 98 |   Matrix<Tp, Dynamic, 1> _weights_3d;
 99 |   vector<int> _idx; //stores the sorted index 
100 |   vector<short> _vInliersAO;
101 |   int _max_votes;
102 | };
103 | 
104 | 
105 | template<typename Tp>
106 | AOOnlyPoseAdapter<Tp>::AOOnlyPoseAdapter(
107 | 	const MatrixX & points_c,
108 | 	const MatrixX & points_g) :
109 | 	PoseAdapterBase<Tp>(),
110 | 	_points_c(points_c), _points_g(points_g)
111 | {
112 | 	_inliers_3d.resize(points_c.cols());
113 | 	_inliers_3d.setOnes();
114 | }
115 | 
116 | template<typename Tp>
117 | AOOnlyPoseAdapter<Tp>::AOOnlyPoseAdapter(
118 | 	const MatrixX & points_c,
119 | 	const MatrixX & points_g,
120 | 	const SO3_T & R) :
121 | 	PoseAdapterBase<Tp>(R),
122 | 	_points_c(points_c), _points_g(points_g)
123 | {
124 | 	_inliers_3d.resize(points_c.cols());
125 | 	_inliers_3d.setOnes();
126 | }
127 | 
128 | template<typename Tp>
129 | AOOnlyPoseAdapter<Tp>::AOOnlyPoseAdapter(
130 | 	const MatrixX & points_c,
131 | 	const MatrixX & points_g,
132 | 	const Vector3 & t,
133 | 	const SO3_T & R) :
134 | 	PoseAdapterBase<Tp>(t, R),
135 | 	_points_c(points_c), _points_g(points_g)
136 | {
137 | 	_inliers_3d.resize(points_c.cols());
138 | 	_inliers_3d.setOnes();
139 | }
140 | 
141 | template<typename Tp>
142 | AOOnlyPoseAdapter<Tp>::~AOOnlyPoseAdapter()
143 | {}
144 | 
145 | template<typename Tp>
146 | typename AOOnlyPoseAdapter<Tp>::Point3 AOOnlyPoseAdapter<Tp>::getPointCurr(
147 | 	int index) const
148 | {
149 | 	assert(index < _points_c.cols() );
150 | 	return _points_c.col(index);
151 | }
152 | 
153 | template <typename Tp>
154 | typename AOOnlyPoseAdapter<Tp>::Point3 AOOnlyPoseAdapter<Tp>::getPointGlob(
155 | 	int index) const
156 | {
157 | 	assert(index < _points_c.cols());
158 | 	return _points_g.col(index);
159 | }
160 | 
161 | template<typename Tp>
162 | bool AOOnlyPoseAdapter<Tp>::isValid( int index ) const
163 | {
164 | 	assert(index < _points_c.cols());
165 | 	return _points_c.col(index)(0) == _points_c.col(index)(0) || _points_c.col(index)(1) == _points_c.col(index)(1) || _points_c.col(index)(2) == _points_c.col(index)(2);
166 | }
167 | 
168 | template<typename Tp>
169 | bool AOOnlyPoseAdapter<Tp>::isInlier33(int index) const
170 | {
171 | 	assert(index < _inliers_3d.rows());
172 | 	return _inliers_3d(index) == 1;
173 | }
174 | 
175 | template<typename Tp>
176 | Tp AOOnlyPoseAdapter<Tp>::weight33(int index) const
177 | {
178 | 	if (!_weights_3d.rows()) return Tp(1.0);
179 | 	else{
180 | 		assert(index < _weights_3d.rows());
181 | 		return _weights_3d(index);
182 | 	}
183 | }
184 | 
185 | template <typename Tp>
186 | void AOOnlyPoseAdapter<Tp>::setInlier(const Matrix<short, Dynamic, Dynamic>& inliers)
187 | {
188 | 	assert(inliers.rows() == _inliers_3d.rows());
189 | 	if (inliers.cols() == 1){
190 | 		// PnPPoseAdapter<Tp>::setInlier(inliers);
191 | 	}
192 | 	else{
193 | 		// PnPPoseAdapter<Tp>::setInlier(inliers);
194 | 		_inliers_3d = inliers.col(1);
195 | 		//memcpy(_inliers_3d.data(), inliers.col(1).data(), inliers.rows() * 2);
196 | 	}
197 | 	return;
198 | }
199 | 
200 | template <typename Tp>
201 | void AOOnlyPoseAdapter<Tp>::setWeights(const Matrix<Tp, Dynamic, Dynamic>& weights)
202 | {
203 | 	if (weights.rows() == 1){
204 | 		// PnPPoseAdapter<Tp>::setWeights(weights);
205 | 	}
206 | 	else{
207 | 		// PnPPoseAdapter<Tp>::setWeights(weights);
208 | 		_weights_3d = weights.col(1);
209 | 		//memcpy(_weights_3d.data(), weights.col(1).data(), weights.rows() * 2);
210 | 	}
211 | 	return;
212 | }
213 | 
214 | template <typename Tp>
215 | void AOOnlyPoseAdapter<Tp>::printInlier() const
216 | {
217 | 	// PnPPoseAdapter<Tp>::printInlier();
218 | 	cout << _inliers_3d.transpose() << endl;
219 | }
220 | 
221 | template <typename Tp>
222 | void AOOnlyPoseAdapter<Tp>::cvtInlier()
223 | {
224 | 	_vInliersAO.clear();
225 | 	_vInliersAO.reserve(this->getNumberCorrespondences());
226 | 	for (short r = 0; r < (short)_inliers_3d.rows(); r++) {
227 | 		if (1 == _inliers_3d[r]){
228 | 			_vInliersAO.push_back(r);
229 | 		}
230 | 	}
231 | }
232 | 
233 | template <typename Tp>
234 | void AOOnlyPoseAdapter<Tp>::sortIdx(){
235 |     //sort the index according to weights 
236 |     vector<Tp> weigh(_weights_3d.data(), _weights_3d.data() + _weights_3d.rows() * _weights_3d.cols());
237 |     _idx = sortIndexes<Tp>( weigh );
238 |     // for (int i = 0; i < _idx.size(); ++i)
239 |     // {
240 |     //     cout << _weights_3d.data()[_idx[i]] << " ";
241 |     // }
242 |     // cout << endl;
243 | }
244 | 
245 | template <typename Tp>
246 | void AOOnlyPoseAdapter<Tp>::getSortedIdx(vector<int>& select_) const{
247 |     //sort the index according to weights 
248 |     for (int i = 0; i < (int)select_.size(); ++i)
249 |     {
250 |         int j = select_[i];
251 |         if(j < (int)_idx.size())
252 |             select_[i] = _idx[j];
253 |     }
254 | }
255 | 
256 | 
257 | #endif 
258 | 


--------------------------------------------------------------------------------
/pose/AOPoseAdapter.hpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * \file CentralAbsoluteAdapter.hpp
  3 |  * \brief Adapter-class for passing bearing-vector-to-point correspondences to
  4 |  *        the central absolute-pose algorithms. It maps opengv types
  5 |  *        back to opengv types.
  6 |  */
  7 | 
  8 | #ifndef _AO_POSE_ADAPTER_HEADER_
  9 | #define _AO_POSE_ADAPTER_HEADER_
 10 | 
 11 | #include <stdlib.h>
 12 | #include <vector>
 13 | #include <PnPPoseAdapter.hpp>
 14 | 
 15 | using namespace std;
 16 | using namespace Eigen;
 17 | /**
 18 |  * \brief The namespace for the absolute pose methods.
 19 |  */
 20 | 
 21 | /**
 22 |  * Check the documentation of the parent-class to understand the meaning of
 23 |  * an AbsoluteAdapter. This child-class is for the central case and holds data
 24 |  * in form of references to opengv-types.
 25 |  */
 26 | template<typename Tp>
 27 | class AOPoseAdapter : public PnPPoseAdapter<Tp>
 28 | {
 29 | 
 30 | protected:
 31 | 	using PoseAdapterBase<Tp>::_t_w;
 32 | 	using PoseAdapterBase<Tp>::_R_cw;
 33 | 	using PnPPoseAdapter<Tp>::_bearingVectors;
 34 | 	using PnPPoseAdapter<Tp>::_points_g;
 35 | 
 36 | public:
 37 | 	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 38 | 	typedef typename PoseAdapterBase<Tp>::Vector3 Vector3;
 39 | 	typedef typename PoseAdapterBase<Tp>::SO3_T SO3_T;
 40 | 	typedef typename PoseAdapterBase<Tp>::Point3 Point3;
 41 | 	
 42 | 	typedef typename PnPPoseAdapter<Tp>::MatrixX MatrixX;
 43 | 
 44 |   /**
 45 |    * \brief Constructor. See protected class-members to understand parameters
 46 |    */
 47 |    AOPoseAdapter(
 48 |       const MatrixX & bearingVectors,
 49 | 	  const MatrixX & points_c,
 50 | 	  const MatrixX & points_g);
 51 |   /**
 52 |    * \brief Constructor. See protected class-members to understand parameters
 53 |    */
 54 |   AOPoseAdapter(
 55 | 	  const MatrixX & bearingVectors,
 56 | 	  const MatrixX & points_c,
 57 | 	  const MatrixX & points_g,
 58 |       const SO3_T & R );
 59 |   /**
 60 |    * \brief Constructor. See protected class-members to understand parameters
 61 |    */
 62 |   AOPoseAdapter(
 63 | 	  const MatrixX & bearingVectors,
 64 | 	  const MatrixX & points_c,
 65 | 	  const MatrixX & points_g,
 66 |       const Vector3 & t,
 67 |       const SO3_T & R );
 68 |   /**
 69 |    * Destructor
 70 |    */
 71 |   virtual ~AOPoseAdapter();
 72 | 
 73 |   //Access of correspondences
 74 |   
 75 |   bool isInlier33(int index) const;
 76 |   Tp weight33(int index) const;
 77 |   /** See parent-class */
 78 |   virtual Point3 getPointCurr( int index ) const;
 79 |   virtual bool isValid( int index ) const;
 80 |   virtual void setInlier(const Matrix<short, Dynamic, Dynamic>& inliers);
 81 |   virtual void setWeights(const Matrix<Tp, Dynamic, Dynamic>& weights);
 82 |   virtual void printInlier() const;
 83 |   const vector<short>& getInlierIdx() const { return _vInliersAO; }
 84 |   void cvtInlier();
 85 | 
 86 | protected:
 87 |   /** Reference to the 3-D points in the camera-frame */
 88 |   const MatrixX & _points_c; 
 89 |   /** flags of inliers. */
 90 |   Matrix<short, Dynamic, 1> _inliers_3d;
 91 |   Matrix<Tp, Dynamic, 1> _weights_3d;
 92 |   vector<short> _vInliersAO;
 93 | };
 94 | 
 95 | 
 96 | template<typename Tp>
 97 | AOPoseAdapter<Tp>::AOPoseAdapter(
 98 | 	const MatrixX & bearingVectors,
 99 | 	const MatrixX & points_c,
100 | 	const MatrixX & points_g) :
101 | 	PnPPoseAdapter<Tp>(bearingVectors, points_g),
102 | 	_points_c(points_c)
103 | {
104 | 	_inliers_3d.resize(_bearingVectors.cols());
105 | 	_inliers_3d.setOnes();
106 | }
107 | 
108 | template<typename Tp>
109 | AOPoseAdapter<Tp>::AOPoseAdapter(
110 | 	const MatrixX & bearingVectors,
111 | 	const MatrixX & points_c,
112 | 	const MatrixX & points_g,
113 | 	const SO3_T & R) :
114 | 	PnPPoseAdapter<Tp>(bearingVectors, points_g, R),
115 | 	_points_c(points_c)
116 | {
117 | 	_inliers_3d.resize(_bearingVectors.cols());
118 | 	_inliers_3d.setOnes();
119 | }
120 | 
121 | template<typename Tp>
122 | AOPoseAdapter<Tp>::AOPoseAdapter(
123 | 	const MatrixX & bearingVectors,
124 | 	const MatrixX & points_c,
125 | 	const MatrixX & points_g,
126 | 	const Vector3 & t,
127 | 	const SO3_T & R) :
128 | 	PnPPoseAdapter<Tp>(bearingVectors, points_g, t, R),
129 | 	_points_c(points_c)
130 | {
131 | 	_inliers_3d.resize(_bearingVectors.cols());
132 | 	_inliers_3d.setOnes();
133 | }
134 | 
135 | template<typename Tp>
136 | AOPoseAdapter<Tp>::~AOPoseAdapter()
137 | {}
138 | 
139 | template<typename Tp>
140 | typename AOPoseAdapter<Tp>::Point3 AOPoseAdapter<Tp>::getPointCurr(
141 | 	int index) const
142 | {
143 | 	assert(index < _bearingVectors.cols() );
144 | 	return _points_c.col(index);
145 | }
146 | 
147 | template<typename Tp>
148 | bool AOPoseAdapter<Tp>::isValid( int index ) const
149 | {
150 | 	assert(index < _bearingVectors.cols());
151 | 	return _points_c.col(index)(0) == _points_c.col(index)(0) || _points_c.col(index)(1) == _points_c.col(index)(1) || _points_c.col(index)(2) == _points_c.col(index)(2);
152 | }
153 | 
154 | template<typename Tp>
155 | bool AOPoseAdapter<Tp>::isInlier33(int index) const
156 | {
157 | 	assert(index < _inliers_3d.rows());
158 | 	return _inliers_3d(index) == 1;
159 | }
160 | 
161 | template<typename Tp>
162 | Tp AOPoseAdapter<Tp>::weight33(int index) const
163 | {
164 | 	if (!_weights_3d.rows()) return Tp(1.0);
165 | 	else{
166 | 		assert(index < _weights_3d.rows());
167 | 		return Tp(_weights_3d(index)) / numeric_limits<short>::max();
168 | 	}
169 | }
170 | 
171 | template <typename Tp>
172 | void AOPoseAdapter<Tp>::setInlier(const Matrix<short, Dynamic, Dynamic>& inliers)
173 | {
174 | 	assert(inliers.rows() == _inliers_3d.rows());
175 | 	if (inliers.cols() == 1){
176 | 		PnPPoseAdapter<Tp>::setInlier(inliers);
177 | 	}
178 | 	else{
179 | 		PnPPoseAdapter<Tp>::setInlier(inliers);
180 | 		_inliers_3d = inliers.col(1);
181 | 		//memcpy(_inliers_3d.data(), inliers.col(1).data(), inliers.rows() * 2);
182 | 	}
183 | 	return;
184 | }
185 | 
186 | template <typename Tp>
187 | void AOPoseAdapter<Tp>::setWeights(const Matrix<Tp, Dynamic, Dynamic>& weights)
188 | {
189 | 	if (weights.rows() == 1){
190 | 		PnPPoseAdapter<Tp>::setWeights(weights);
191 | 	}
192 | 	else{
193 | 		PnPPoseAdapter<Tp>::setWeights(weights);
194 | 		_weights_3d = weights.col(1);
195 | 		//memcpy(_weights_3d.data(), weights.col(1).data(), weights.rows() * 2);
196 | 	}
197 | 	return;
198 | }
199 | 
200 | template <typename Tp>
201 | void AOPoseAdapter<Tp>::printInlier() const
202 | {
203 | 	PnPPoseAdapter<Tp>::printInlier();
204 | 	cout << _inliers_3d.transpose() << endl;
205 | }
206 | 
207 | template <typename Tp>
208 | void AOPoseAdapter<Tp>::cvtInlier()
209 | {
210 | 	_vInliersAO.clear();
211 | 	_vInliersAO.reserve(this->getNumberCorrespondences());
212 | 	for (short r = 0; r < (short)_inliers_3d.rows(); r++) {
213 | 		if (1 == _inliers_3d[r]){
214 | 			_vInliersAO.push_back(r);
215 | 		}
216 | 	}
217 | }
218 | 
219 | #endif 
220 | 


--------------------------------------------------------------------------------
/pose/AbsoluteOrientation.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef BTL_AO_POSE_HEADER
  2 | #define BTL_AO_POSE_HEADER
  3 | 
  4 | #include <Eigen/Dense>
  5 | #include "AOPoseAdapter.hpp"
  6 | #include "AOOnlyPoseAdapter.hpp"
  7 | #include "P3P.hpp"
  8 | 
  9 | using namespace Eigen;
 10 | 
 11 | template< typename Tp >
 12 | Matrix<Tp, 2, 1> calc_percentage_err(const Sophus::SO3<Tp>& R_cw_, const Matrix<Tp, 3, 1>& t_w_, const PoseAdapterBase<Tp>* p_ad){
 13 | 	Matrix<Tp, 3, 1> te = R_cw_*t_w_ - p_ad->getRcw() * p_ad->gettw();
 14 | 	Tp t_e = te.norm() / p_ad->gettw().norm() * 100;
 15 | 
 16 | 	//Quaternion<POSE_T> q_gt(R_cw_.matrix());
 17 | 	Quaternion<Tp> q_gt = R_cw_.unit_quaternion();
 18 | 	Quaternion<Tp> q_est = p_ad->getRcw().unit_quaternion();
 19 | 
 20 | 	q_gt.w() -= q_est.w();
 21 | 	q_gt.x() -= q_est.x();
 22 | 	q_gt.y() -= q_est.y();
 23 | 	q_gt.z() -= q_est.z();
 24 | 
 25 | 	Tp r_e = q_gt.norm() / q_est.norm() * 100;
 26 | 	return Matrix<Tp, 2, 1>(t_e, r_e);
 27 | }
 28 | 
 29 | template< typename Tp >
 30 | Matrix<Tp, 2, 1> calc_err(const Matrix<Tp, 4, 4>& Diff){
 31 | 	Matrix<Tp, 3, 3> R = Diff.block(0,0, 3,3);
 32 | 	AngleAxis<Tp> aa(R);
 33 | 	Matrix<Tp, 3, 1> tr = Diff.block(0,3, 3,1);
 34 | 	Tp t_e = tr.norm();
 35 | 	Tp r_e = aa.angle();
 36 | 	return Matrix<Tp, 2, 1>(t_e, r_e); 
 37 | }
 38 | 
 39 | template< typename Tp >
 40 | Matrix<Tp, 2, 1> calc_err(const Matrix<Tp, 4, 4>& GT_cw_,  const Matrix<Tp, 4, 4>& SE_cw_){
 41 | 	Matrix<Tp, 4, 4> Diff = SE_cw_ * GT_cw_.inverse() ;
 42 | 	return calc_err(Diff);
 43 | }
 44 | 
 45 | 
 46 | 
 47 | template< typename Tp >
 48 | Sophus::SE3<Tp> shinji(
 49 | 	const Matrix<Tp, Dynamic, Dynamic> & X_w_, 
 50 | 	const Matrix<Tp, Dynamic, Dynamic>&  X_c_, int K){
 51 | 	// X_c_ = R_cw * X_w_ + Tw; //R_cw and tw is defined in world and transform a point in world to camera coordinate system
 52 | 	// main references: Shinji, U. (1991). Least-squares estimation of transformation parameters between two point patterns. PAMI.
 53 | 	assert(3 <= K && K <= X_w_.cols() && X_w_.cols() == X_c_.cols() && X_w_.rows() == X_c_.rows() && X_w_.rows() == 3);
 54 | 	//Compute the centroid of each point set
 55 | 	Matrix<Tp, 3, 1> Cw(0, 0, 0), Cc(0, 0, 0); //Matrix<float,3,1,0,3,1> = Vector3f
 56 | 	for (int nCount = 0; nCount < K; nCount++){
 57 | 		Cw += X_w_.col(nCount);
 58 | 		Cc += X_c_.col(nCount);
 59 | 	}
 60 | 	Cw /= (Tp)K;
 61 | 	Cc /= (Tp)K;
 62 | 
 63 | 	// transform coordinate
 64 | 	Matrix<Tp, 3, 1> Aw, Ac;
 65 | 	Matrix<Tp, 3, 3> M; M.setZero();
 66 | 	Matrix<Tp, 3, 3> N;
 67 | 	Tp sigma_w_sqr = 0, sigma_c_sqr = 0;
 68 | 	for (int nC = 0; nC < K; nC++){
 69 | 		Aw = X_w_.col(nC) - Cw; sigma_w_sqr += Aw.norm();
 70 | 		Ac = X_c_.col(nC) - Cc; sigma_c_sqr += Ac.norm();
 71 | 		N = Ac * Aw.transpose();
 72 | 		M += N;
 73 | 	}
 74 | 
 75 | 	M /= (Tp) X_w_.cols() ;
 76 | 	sigma_w_sqr /= (Tp)K;
 77 | 	sigma_c_sqr /= (Tp)K;
 78 | 
 79 | 	JacobiSVD<Matrix<Tp, -1, -1, 0, -1, -1> > svd(M, ComputeFullU | ComputeFullV);
 80 | 	//[U S V]=svd(M);
 81 | 	//R=U*V';
 82 | 	Matrix<Tp, 3, 3> U = svd.matrixU();
 83 | 	Matrix<Tp, 3, 3> V = svd.matrixV();
 84 | 	// Matrix<Tp, 3, 1> D = svd.singularValues();
 85 | 	Matrix<Tp, 3, 3> Tmp = U*V.transpose();
 86 | 	Tp d = Tmp.determinant();
 87 | 	Sophus::SO3<Tp> R_tmp;
 88 | 	if (d < Tp(0)) {
 89 | 		Matrix<Tp, 3, 3> I = Matrix<Tp, 3, 3>::Identity(); I(2, 2) = -1;
 90 | 		R_tmp = Sophus::SO3<Tp>(U*I*V.transpose());
 91 | 	}
 92 | 	else{
 93 | 		R_tmp = Sophus::SO3<Tp>(U*V.transpose());
 94 | 	}
 95 | 	Matrix< Tp, 3, 1> T_tmp = Cc - R_tmp * Cw;
 96 | 	Sophus::SE3<Tp> solution(R_tmp, T_tmp);
 97 | 	
 98 | 	return solution;
 99 | }
100 | 
101 | template< typename Tp >
102 | void shinji_ransac(AOPoseAdapter<Tp>& adapter, 
103 | 	const Tp dist_thre_3d_, int& Iter, Tp confidence = 0.99){
104 | 	typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
105 | 	typedef Matrix<Tp, 3, 1> Point3;
106 | 	typedef Sophus::SE3<Tp> RT;
107 | 
108 | 	RandomElements<int> re((int)adapter.getNumberCorrespondences());
109 | 	const int K = 3;
110 | 
111 | 	adapter.setMaxVotes(-1);
112 | 	for (int ii = 0; ii < Iter; ii++)	{
113 | 		//randomly select K candidates
114 | 		vector<int> selected_cols;
115 | 		re.run(K, &selected_cols);
116 | 		MatrixX eimX_world(3, K), eimX_cam(3, K);
117 | 		bool invalid_sample = false;
118 | 		for (int nSample = 0; nSample < K; nSample++) {
119 | 			eimX_world.col(nSample) = adapter.getPointGlob(selected_cols[nSample]);
120 | 			if (adapter.isValid(selected_cols[nSample])){
121 | 				eimX_cam.col(nSample) = adapter.getPointCurr(selected_cols[nSample]);
122 | 			}
123 | 			else{
124 | 				invalid_sample = true;
125 | 			}
126 | 		}
127 | 
128 | 		if (invalid_sample) continue;
129 | 		//calc R&t		
130 | 		RT solution = shinji<Tp>(eimX_world, eimX_cam, K);
131 | 			
132 | 		//collect votes
133 | 		int votes = 0;
134 | 		Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(),2); inliers.setZero();
135 | 		for (int c = 0; c < adapter.getNumberCorrespondences(); c++) {
136 | 			if (adapter.isValid(c)){
137 | 				Point3 eivE = adapter.getPointCurr(c) - (solution.so3() * adapter.getPointGlob(c) + solution.translation());
138 | 				if (eivE.norm() < dist_thre_3d_){
139 | 					inliers(c,1) = 1;
140 | 					votes++;
141 | 				}
142 | 			}
143 | 		}
144 | 
145 | 		if (votes > adapter.getMaxVotes()){
146 | 			adapter.setMaxVotes( votes );
147 | 			adapter.setRcw(solution.so3());
148 | 			adapter.sett(solution.translation());
149 | 			adapter.setInlier(inliers);
150 | 			Iter = RANSACUpdateNumIters(confidence, (Tp)(adapter.getNumberCorrespondences() - votes) / adapter.getNumberCorrespondences(), K, Iter);
151 | 		}
152 | 	}
153 | 	adapter.cvtInlier();
154 | 
155 | 	return;
156 | }
157 | 
158 | template< typename Tp >
159 | void shinji_ransac2(AOOnlyPoseAdapter<Tp>& adapter, 
160 | 	const Tp dist_thre_3d_, int& Iter, Tp confidence = 0.99){
161 | 	typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
162 | 	typedef Matrix<Tp, 3, 1> Point3;
163 | 	typedef Sophus::SE3<Tp> RT;
164 | 
165 | 	RandomElements<int> re((int)adapter.getNumberCorrespondences());
166 | 	const int K = 3;
167 | 
168 | 	adapter.setMaxVotes(-1);
169 | 	for (int ii = 0; ii < Iter; ii++)	{
170 | 		//randomly select K candidates
171 | 		vector<int> selected_cols;
172 | 		re.run(K, &selected_cols);
173 | 		MatrixX eimX_world(3, K), eimX_cam(3, K), bv(3, K);
174 | 		bool invalid_sample = false;
175 | 		for (int nSample = 0; nSample < K; nSample++) {
176 | 			eimX_world.col(nSample) = adapter.getPointGlob(selected_cols[nSample]);
177 | 			if (adapter.isValid(selected_cols[nSample])){
178 | 				eimX_cam.col(nSample) = adapter.getPointCurr(selected_cols[nSample]);
179 | 			}
180 | 			else{
181 | 				invalid_sample = true;
182 | 			}
183 | 		}
184 | 
185 | 		if (invalid_sample) continue;
186 | 		//calc R&t		
187 | 		RT solution = shinji<Tp>(eimX_world, eimX_cam, K);
188 | 			
189 | 		//collect votes
190 | 		int votes = 0;
191 | 		Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(),2); inliers.setZero();
192 | 		for (int c = 0; c < adapter.getNumberCorrespondences(); c++) {
193 | 			if (adapter.isValid(c)){
194 | 				Point3 eivE = adapter.getPointCurr(c) - (solution.so3() * adapter.getPointGlob(c) + solution.translation());
195 | 				if (eivE.norm() < dist_thre_3d_){
196 | 					inliers(c,1) = 1;
197 | 					votes++;
198 | 				}
199 | 			}
200 | 		}
201 | 
202 | 		if (votes > adapter.getMaxVotes()){
203 | 			adapter.setMaxVotes( votes );
204 | 			adapter.setRcw(solution.so3());
205 | 			adapter.sett(solution.translation());
206 | 			adapter.setInlier(inliers);
207 | 			Iter = RANSACUpdateNumIters(confidence, (Tp)(adapter.getNumberCorrespondences() - votes) / adapter.getNumberCorrespondences(), K, Iter);
208 | 		}
209 | 	}
210 | 	adapter.cvtInlier();
211 | 
212 | 	return;
213 | }
214 | 
215 | template< typename Tp >
216 | void shinji_prosac(AOOnlyPoseAdapter<Tp>& adapter, const Tp dist_thre_3d_, int& Iter, Tp confidence = 0.99){
217 | 	typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
218 | 	typedef Sophus::SE3<Tp> RT;
219 | 	typedef Matrix<Tp, 3, 1> Point3;
220 | 
221 | 	adapter.sortIdx();
222 | 	const int K = 3;
223 | 	ProsacSampler<Tp> ps(K, adapter.getNumberCorrespondences());
224 | 	adapter.setMaxVotes(-1);
225 | 	for (int ii = 0; ii < Iter; ii++)	{
226 | 		//randomly select K candidates
227 | 		vector<int> selected_cols;
228 | 		ps.sample(&selected_cols);
229 | 		adapter.getSortedIdx(selected_cols);
230 | 
231 | 		MatrixX eimX_world(3, K), eimX_cam(3, K), bv(3, K);
232 | 		bool invalid_sample = false;
233 | 		for (int nSample = 0; nSample < K; nSample++) {
234 | 			eimX_world.col(nSample) = adapter.getPointGlob(selected_cols[nSample]);
235 | 			if (adapter.isValid(selected_cols[nSample])){
236 | 				eimX_cam.col(nSample) = adapter.getPointCurr(selected_cols[nSample]);
237 | 			}
238 | 			else{
239 | 				invalid_sample = true;
240 | 			}
241 | 		}
242 | 
243 | 		if (invalid_sample) continue;
244 | 		//calc R&t		
245 | 		RT solution = shinji<Tp>(eimX_world, eimX_cam, K);
246 | 			
247 | 		//collect votes
248 | 		int votes = 0;
249 | 		Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(),2); inliers.setZero();
250 | 		for (int c = 0; c < adapter.getNumberCorrespondences(); c++) {
251 | 			if (adapter.isValid(c)){
252 | 				Point3 eivE = adapter.getPointCurr(c) - (solution.so3() * adapter.getPointGlob(c) + solution.translation());
253 | 				if (eivE.norm() < dist_thre_3d_){
254 | 					inliers(c,1) = 1;
255 | 					votes++;
256 | 				}
257 | 			}
258 | 		}
259 | 
260 | 		if (votes > adapter.getMaxVotes()){
261 | 			adapter.setMaxVotes( votes );
262 | 			adapter.setRcw(solution.so3());
263 | 			adapter.sett(solution.translation());
264 | 			adapter.setInlier(inliers);
265 | 			Iter = RANSACUpdateNumIters(confidence, (Tp)(adapter.getNumberCorrespondences() - votes) / adapter.getNumberCorrespondences(), K, Iter);
266 | 		}
267 | 	}
268 | 	adapter.cvtInlier();
269 | 
270 | 	return;
271 | }
272 | 
273 | template< typename Tp >
274 | void shinji_ls(AOPoseAdapter<Tp>& adapter){
275 | 	typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
276 | 	// typedef Matrix<Tp, 3, 1> Point3;
277 | 	typedef Sophus::SE3<Tp> RT;
278 | 
279 | 	const vector<short>& vInliers = adapter.getInlierIdx();
280 | 	int K = vInliers.size();
281 | 	MatrixX Xw(3, K);
282 | 	MatrixX Xc(3, K);
283 | 
284 | 	for (int ii = 0; ii < K; ii++)	{
285 | 		short idx = vInliers[ii];
286 | 		Xw.col(ii) = adapter.getPointGlob(idx);
287 | 		Xc.col(ii) = adapter.getPointCurr(idx);
288 | 	}
289 | 
290 | 	RT solution = shinji<Tp>(Xw, Xc, K);
291 | 
292 | 	adapter.setRcw(solution.so3());
293 | 	adapter.sett(solution.translation());
294 | 
295 | 	return;
296 | }
297 | 
298 | template< typename Tp >
299 | void shinji_ls1(AOOnlyPoseAdapter<Tp>& adapter){
300 | 	typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
301 | 	typedef Sophus::SE3<Tp> RT;
302 | 
303 | 	const vector<short>& vInliers = adapter.getInlierIdx();
304 | 	int K = vInliers.size();
305 | 	MatrixX Xw(3, K);
306 | 	MatrixX Xc(3, K);
307 | 
308 | 	for (int ii = 0; ii < K; ii++)	{
309 | 		short idx = vInliers[ii];
310 | 		Xw.col(ii) = adapter.getPointGlob(idx);
311 | 		Xc.col(ii) = adapter.getPointCurr(idx);
312 | 	}
313 | 
314 | 	RT solution = shinji<Tp>(Xw, Xc, K);
315 | 
316 | 	adapter.setRcw(solution.so3());
317 | 	adapter.sett(solution.translation());
318 | 
319 | 	return;
320 | }
321 | 
322 | template< typename Tp >
323 | void shinji_ls2(AOOnlyPoseAdapter<Tp>& adapter){
324 | 	typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
325 | 	typedef Sophus::SE3<Tp> RT;
326 | 
327 | 	int K = adapter.getNumberCorrespondences();
328 | 	MatrixX Xw(3, K);
329 | 	MatrixX Xc(3, K);
330 | 
331 | 	for (int ii = 0; ii < K; ii++)	{
332 | 		Xw.col(ii) = adapter.getPointGlob(ii);
333 | 		Xc.col(ii) = adapter.getPointCurr(ii);
334 | 	}
335 | 
336 | 	RT solution = shinji<Tp>(Xw, Xc, K);
337 | 
338 | 	adapter.setRcw(solution.so3());
339 | 	adapter.sett(solution.translation());
340 | 
341 | 	return;
342 | }
343 | 
344 | template< typename Tp >
345 | bool assign_sample(const AOPoseAdapter<Tp>& adapter, const vector<int>& selected_cols_, 
346 | 	Matrix<Tp, Dynamic, Dynamic>* p_X_w_, Matrix<Tp, Dynamic, Dynamic>* p_X_c_, Matrix<Tp, Dynamic, Dynamic>* p_bv_){
347 | 	int K = (int)selected_cols_.size() - 1;
348 | 	bool use_shinji = false;
349 | 	int nValid = 0;
350 | 	for (int nSample = 0; nSample < K; nSample++) {
351 | 		p_X_w_->col(nSample) = adapter.getPointGlob(selected_cols_[nSample]);
352 | 		p_bv_->col(nSample) = adapter.getBearingVector(selected_cols_[nSample]);
353 | 		if (adapter.isValid(selected_cols_[nSample])){
354 | 			p_X_c_->col(nSample) = adapter.getPointCurr(selected_cols_[nSample]);
355 | 			nValid++;
356 | 		}
357 | 	}
358 | 	if (nValid == K)
359 | 		use_shinji = true;
360 | 	//assign the fourth elements for 
361 | 	p_X_w_->col(3) = adapter.getPointGlob(selected_cols_[3]);
362 | 	p_bv_->col(3)     = adapter.getBearingVector(selected_cols_[3]);
363 | 
364 | 	return use_shinji;
365 | }
366 | 
367 | template< typename Tp >
368 | void shinji_kneip_ransac(AOPoseAdapter<Tp>& adapter, const Tp dist_thre_3d_, const Tp thre_2d_, int& Iter, Tp confidence = 0.99){
369 | 	typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
370 | 	typedef Matrix<Tp, 3, 1> Point3;
371 | 	typedef Sophus::SE3<Tp> RT;
372 | 
373 | 	const Tp cos_thr = cos(atan(thre_2d_ / adapter.getFocal()));
374 | 
375 | 	RandomElements<int> re((int)adapter.getNumberCorrespondences());
376 | 	const int K = 3;
377 | 	MatrixX X_w(3, K + 1), X_c(3, K + 1), bv(3, K + 1);
378 | 	Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(), 2);
379 | 
380 | 	adapter.setMaxVotes(-1);
381 | 	for (int ii = 0; ii < Iter; ii++)	{
382 | 		//randomly select K candidates
383 | 		RT solution_kneip, solution_shinji;
384 | 		vector<RT> v_solutions;
385 | 		vector<int> selected_cols;
386 | 		re.run(K + 1, &selected_cols);
387 | 
388 | 		if (assign_sample<Tp>(adapter, selected_cols, &X_w, &X_c, &bv)){
389 | 			solution_shinji = shinji<Tp>(X_w, X_c, K);
390 | 			v_solutions.push_back(solution_shinji);
391 | 		}
392 | 
393 | 		if (kneip<Tp>(X_w, bv, &solution_kneip)){
394 | 			v_solutions.push_back(solution_kneip);
395 | 		}
396 | 
397 | 		for (typename vector<RT>::iterator itr = v_solutions.begin(); itr != v_solutions.end(); ++itr) {
398 | 			//collect votes
399 | 			int votes = 0;
400 | 			inliers.setZero();
401 | 			Point3 eivE; Point3 pc; Tp cos_a;
402 | 
403 | 			for (int c = 0; c < adapter.getNumberCorrespondences(); c++) {
404 | 				// voted by 3-3 correspondences
405 | 				if (adapter.isValid(c)){
406 | 					eivE = adapter.getPointCurr(c) - (itr->so3() * adapter.getPointGlob(c) + itr->translation());
407 | 					if (eivE.norm() < dist_thre_3d_){
408 | 						inliers(c, 1) = 1;
409 | 						votes++;
410 | 					}
411 | 				}
412 | 				// voted by 2-3 correspondences
413 | 				pc = itr->so3() * adapter.getPointGlob(c) + itr->translation();// transform pw into pc
414 | 				pc = pc / pc.norm(); //normalize pc
415 | 
416 | 				//compute the score
417 | 				cos_a = pc.transpose() * adapter.getBearingVector(c);
418 | 				if (cos_a > cos_thr){
419 | 					inliers(c, 0) = 1;
420 | 					votes++;
421 | 				}
422 | 			}
423 | 
424 | 			if (votes > adapter.getMaxVotes()){
425 | 				adapter.setMaxVotes(votes);
426 | 				adapter.setRcw(itr->so3());
427 | 				adapter.sett(itr->translation());
428 | 				adapter.setInlier(inliers);
429 | 				Iter = RANSACUpdateNumIters(confidence, (Tp)(adapter.getNumberCorrespondences() * 2 - votes) / adapter.getNumberCorrespondences() / 2, K, Iter);
430 | 			}
431 | 		}
432 | 	}
433 | 	PnPPoseAdapter<Tp>* pAdapter = &adapter;
434 | 	pAdapter->cvtInlier();
435 | 	adapter.cvtInlier();
436 | 
437 | 	return;
438 | }
439 | 
440 | template< typename Tp >
441 | void shinji_kneip_prosac(AOPoseAdapter<Tp>& adapter, const Tp dist_thre_3d_, const Tp thre_2d_, int& Iter, Tp confidence = 0.99){
442 | 	typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
443 | 	typedef Matrix<Tp, 3, 1> Point3;
444 | 	typedef Sophus::SE3<Tp> RT;
445 | 
446 | 	const Tp cos_thr = cos(atan(thre_2d_ / adapter.getFocal()));
447 | 
448 | 	RandomElements<int> re((int)adapter.getNumberCorrespondences());
449 | 	const int K = 3;
450 | 	MatrixX X_w(3, K + 1), X_c(3, K + 1), bv(3, K + 1);
451 | 	Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(), 2);
452 | 
453 | 	adapter.sortIdx();
454 | 	ProsacSampler<Tp> ps(K+1, adapter.getNumberCorrespondences());
455 | 
456 | 	adapter.setMaxVotes(-1);
457 | 	for (int ii = 0; ii < Iter; ii++)	{
458 | 		RT solution_kneip, solution_shinji;
459 | 		vector<RT> v_solutions;
460 | 		//randomly select K candidates
461 | 		vector<int> selected_cols;
462 | 		ps.sample(&selected_cols);
463 | 		adapter.getSortedIdx(selected_cols);
464 | 
465 | 		if (assign_sample<Tp>(adapter, selected_cols, &X_w, &X_c, &bv)){
466 | 			solution_shinji = shinji<Tp>(X_w, X_c, K);
467 | 			v_solutions.push_back(solution_shinji);
468 | 		}
469 | 
470 | 		if (kneip<Tp>(X_w, bv, &solution_kneip)){
471 | 			v_solutions.push_back(solution_kneip);
472 | 		}
473 | 
474 | 		for (typename vector<RT>::iterator itr = v_solutions.begin(); itr != v_solutions.end(); ++itr) {
475 | 			//collect votes
476 | 			int votes = 0;
477 | 			inliers.setZero();
478 | 			Point3 eivE; Point3 pc; Tp cos_a;
479 | 
480 | 			for (int c = 0; c < adapter.getNumberCorrespondences(); c++) {
481 | 				// voted by 3-3 correspondences
482 | 				if (adapter.isValid(c)){
483 | 					eivE = adapter.getPointCurr(c) - (itr->so3() * adapter.getPointGlob(c) + itr->translation());
484 | 					if (eivE.norm() < dist_thre_3d_){
485 | 						inliers(c, 1) = 1;
486 | 						votes++;
487 | 					}
488 | 				}
489 | 				// voted by 2-3 correspondences
490 | 				pc = itr->so3() * adapter.getPointGlob(c) + itr->translation();// transform pw into pc
491 | 				pc = pc / pc.norm(); //normalize pc
492 | 
493 | 				//compute the score
494 | 				cos_a = pc.transpose() * adapter.getBearingVector(c);
495 | 				if (cos_a > cos_thr){
496 | 					inliers(c, 0) = 1;
497 | 					votes++;
498 | 				}
499 | 			}
500 | 
501 | 			if (votes > adapter.getMaxVotes()){
502 | 				adapter.setMaxVotes(votes);
503 | 				adapter.setRcw(itr->so3());
504 | 				adapter.sett(itr->translation());
505 | 				adapter.setInlier(inliers);
506 | 				Iter = RANSACUpdateNumIters(confidence, (Tp)(adapter.getNumberCorrespondences() * 2 - votes) / adapter.getNumberCorrespondences() / 2, K, Iter);
507 | 			}
508 | 		}
509 | 	}
510 | 	PnPPoseAdapter<Tp>* pAdapter = &adapter;
511 | 	pAdapter->cvtInlier();
512 | 	adapter.cvtInlier();
513 | 
514 | 	return;
515 | }
516 | 
517 | #endif
518 | 


--------------------------------------------------------------------------------
/pose/AbsoluteOrientationNormal.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef BTL_AO_NORM_POSE_HEADER
  2 | #define BTL_AO_NORM_POSE_HEADER
  3 | 
  4 | //#include "common/OtherUtil.hpp"
  5 | #include <limits>
  6 | #include <Eigen/Dense>
  7 | #include "NormalAOPoseAdapter.hpp"
  8 | #include "AbsoluteOrientation.hpp"
  9 | 
 10 | using namespace Eigen;
 11 | using namespace std;
 12 | 
 13 | template<typename Tp>
 14 | Matrix<Tp, 3, 1> find_opt_cc(NormalAOPoseAdapter<Tp>& adapter)
 15 | {
 16 | 	//the R has been fixed, we need to find optimal cc, camera center, given n pairs of 2-3 correspondences
 17 | 	//Slabaugh, G., Schafer, R., & Livingston, M. (2001). Optimal Ray Intersection For Computing 3D Points From N -View Correspondences.
 18 | 	typedef Matrix<Tp, 3, 1> V3;
 19 | 	typedef Matrix<Tp, 3, 3> M3;
 20 | 
 21 | 	M3 Rwc = adapter.getRcw().inverse().matrix();
 22 | 	M3 AA; AA.setZero();
 23 | 	V3 bb; bb.setZero();
 24 | 	for (int i = 0; i < adapter.getNumberCorrespondences(); i++)
 25 | 	{
 26 | 		if (adapter.isInlier23(i)){
 27 | 			V3 vr_w = Rwc * adapter.getBearingVector(i);
 28 | 			M3 A;
 29 | 			A(0,0) = 1 - vr_w(0)*vr_w(0);
 30 | 			A(1,0) = A(0,1) = - vr_w(0)*vr_w(1);
 31 | 			A(2,0) = A(0,2) = - vr_w(0)*vr_w(2);
 32 | 			A(1,1) = 1 - vr_w(1)*vr_w(1);
 33 | 			A(2,1) = A(1,2) = - vr_w(1)*vr_w(2);
 34 | 			A(2,2) = 1 - vr_w(2)*vr_w(2);
 35 | 			V3 b = A * adapter.getPointGlob(i);
 36 | 			AA += A;
 37 | 			bb += b;
 38 | 		}
 39 | 	}
 40 | 	V3 c_w;
 41 | 	if (fabs(AA.determinant()) < Tp(0.0001))
 42 | 		c_w = V3(numeric_limits<Tp>::quiet_NaN(), numeric_limits<Tp>::quiet_NaN(), numeric_limits<Tp>::quiet_NaN());
 43 | 	else
 44 | 		c_w = AA.jacobiSvd(ComputeFullU | ComputeFullV).solve(bb);
 45 | 	return c_w;
 46 | }
 47 | 
 48 | template< typename Tp >
 49 | bool assign_sample(const NormalAOPoseAdapter<Tp>& adapter, 
 50 | 	const vector<int>& selected_cols_, 
 51 | 	Matrix<Tp, Dynamic, Dynamic>* p_X_w_, Matrix<Tp, Dynamic, Dynamic>* p_N_w_, 
 52 | 	Matrix<Tp, Dynamic, Dynamic>* p_X_c_, Matrix<Tp, Dynamic, Dynamic>* p_N_c_, Matrix<Tp, Dynamic, Dynamic>* p_bv_){
 53 | 	
 54 | 	int K = (int)selected_cols_.size() - 1;
 55 | 	bool use_shinji = false;
 56 | 	int nValid = 0;
 57 | 	for (int nSample = 0; nSample < K; nSample++) {
 58 | 		p_X_w_->col(nSample) = adapter.getPointGlob(selected_cols_[nSample]);
 59 | 		p_N_w_->col(nSample) = adapter.getNormalGlob(selected_cols_[nSample]);
 60 | 		p_bv_->col(nSample) = adapter.getBearingVector(selected_cols_[nSample]);
 61 | 		if (adapter.isValid(selected_cols_[nSample])){
 62 | 			p_X_c_->col(nSample) = adapter.getPointCurr(selected_cols_[nSample]);
 63 | 			p_N_c_->col(nSample) = adapter.getNormalCurr(selected_cols_[nSample]);
 64 | 			nValid++;
 65 | 		}
 66 | 	}
 67 | 	if (nValid == K)
 68 | 		use_shinji = true;
 69 | 	//assign the fourth elements for 
 70 | 	p_X_w_->col(3) = adapter.getPointGlob(selected_cols_[3]);
 71 | 	p_N_w_->col(3) = adapter.getNormalGlob(selected_cols_[3]);
 72 | 	p_bv_->col(3) = adapter.getBearingVector(selected_cols_[3]);
 73 | 
 74 | 	return use_shinji;
 75 | }
 76 | 
 77 | template<typename Tp>
 78 | void nl_2p( const Matrix<Tp,3,1>& pt1_c, const Matrix<Tp,3,1>& nl1_c, const Matrix<Tp,3,1>& pt2_c, 
 79 | 			const Matrix<Tp,3,1>& pt1_w, const Matrix<Tp,3,1>& nl1_w, const Matrix<Tp,3,1>& pt2_w, 
 80 | 			Sophus::SE3<Tp>* p_solution){
 81 | 	//Drost, B., Ulrich, M., Navab, N., & Ilic, S. (2010). Model globally, match locally: Efficient and robust 3D object recognition. In CVPR (pp. 998?005). Ieee. http://doi.org/10.1109/CVPR.2010.5540108
 82 | 	// typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
 83 | 	typedef Matrix<Tp, 3, 1> V3;
 84 | 	typedef Sophus::SO3<Tp> ROTATION;
 85 | 	// typedef Sophus::SE3<Tp> RT;
 86 | 
 87 | 	V3 c_w = pt1_w; // c_w is the origin of coordinate g w.r.t. world
 88 | 
 89 | 	Tp alpha = acos(nl1_w(0)); // rotation nl1_c to x axis (1,0,0)
 90 | 	V3 axis( 0, nl1_w(2), -nl1_w(1)); //rotation axis between nl1_c to x axis (1,0,0) i.e. cross( nl1_w, x );
 91 | 	axis.normalize();
 92 | 
 93 | 	//verify quaternion and rotation matrix
 94 | 	Quaternion<Tp> q_g_f_w(AngleAxis<Tp>(alpha, axis));
 95 | 	//cout << q_g_f_w << endl;
 96 | 	ROTATION R_g_f_w(q_g_f_w);
 97 | 	//cout << R_g_f_w << endl;
 98 | 
 99 | 	// V3 nl_x = R_g_f_w * nl1_w;
100 | 	axis.normalize();
101 | 
102 | 	V3 c_c = pt1_c;
103 | 	Tp beta = acos(nl1_c(0)); //rotation nl1_w to x axis (1,0,0)
104 | 	V3 axis2( 0, nl1_c(2), -nl1_c(1) ); //rotation axis between nl1_m to x axis (1,0,0) i.e. cross( nl1_w, x );
105 | 	axis2.normalize();
106 | 
107 | 	Quaternion<Tp> q_gp_f_c(AngleAxis<Tp>(beta, axis2));
108 | 	//cout << q_gp_f_c << endl;
109 | 	ROTATION R_gp_f_c(q_gp_f_c);
110 | 	//cout << R_gp_f_c << endl;
111 | 	//{
112 | 	//	Vector3 nl_x = R_gp_f_c * nl1_c;
113 | 	//	print<T, Vector3>(nl_x);
114 | 	//}
115 | 
116 | 	V3 pt2_g = R_g_f_w * (pt2_w - c_w); pt2_g(0) = Tp(0);  pt2_g.normalize();
117 | 	V3 pt2_gp = R_gp_f_c * (pt2_c - c_c); pt2_gp(0) = Tp(0);  pt2_gp.normalize();
118 | 
119 | 	Tp gamma = acos(pt2_g.dot(pt2_gp)); //rotate pt2_g to pt2_gp;
120 | 	V3 axis3(1,0,0); 
121 | 
122 | 	Quaternion< Tp > q_gp_f_g(AngleAxis<Tp>(gamma, axis3));
123 | 	//cout << q_gp_f_g << endl;
124 | 	ROTATION R_gp_f_g ( q_gp_f_g );
125 | 	//cout << R_gp_f_g << endl;
126 | 
127 | 	ROTATION R_c_f_gp = R_gp_f_c.inverse();
128 | 	p_solution->so3() = R_c_f_gp * R_gp_f_g * R_g_f_w;
129 | 	//{
130 | 	//	T3 pt = *R_cfw * (pt2_w - c_w) + c_c;
131 | 	//	cout << norm<T, T3>( pt - pt2_c ) << endl;
132 | 	//}
133 | 	//{
134 | 	//	cout << norm<T, T3>(nl1_w) << endl;
135 | 	//	cout << norm<T, T3>(nl1_c) << endl;
136 | 	//	cout << norm<T, T3>(*R_cfw * nl1_w) << endl;
137 | 	//	cout << norm<T, T3>(nl1_c - *R_cfw * nl1_w) << endl;
138 | 	//}
139 | 	p_solution->translation() = c_c - p_solution->so3() * c_w;
140 | 
141 | 	return;
142 | }
143 | 
144 | 
145 | template< typename Tp > /*Matrix<float,-1,-1,0,-1,-1> = MatrixXf*/
146 | Sophus::SE3<Tp>  nl_shinji_ls(const Matrix<Tp, Dynamic, Dynamic> & Xw_, const Matrix<Tp, Dynamic, Dynamic>&  Xc_,
147 | 	const Matrix<Tp, Dynamic, Dynamic> & Nw_, const Matrix<Tp, Dynamic, Dynamic>&  Nc_, const int K) {
148 | 	typedef Sophus::SE3<Tp> RT;
149 | 
150 | 	assert(Xw_.rows() == 3);
151 | 
152 | 	//Compute the centroid of each point set
153 | 	Matrix<Tp, 3, 1> Cw(0, 0, 0), Cc(0, 0, 0); //Matrix<float,3,1,0,3,1> = Vector3f
154 | 	for (int nCount = 0; nCount < K; nCount++){
155 | 		Cw += Xw_.col(nCount);
156 | 		Cc += Xc_.col(nCount);
157 | 	}
158 | 	Cw /= (Tp)K;
159 | 	Cc /= (Tp)K;
160 | 
161 | 	// transform coordinate
162 | 	Matrix<Tp, 3, 1> Aw, Ac;
163 | 	Matrix<Tp, 3, 3> M; M.setZero();
164 | 	Matrix<Tp, 3, 3> N;
165 | 	Tp sigma_w_sqr = 0;
166 | 	for (int nC = 0; nC < K; nC++){
167 | 		Aw = Xw_.col(nC) - Cw; sigma_w_sqr += Aw.squaredNorm();
168 | 		Ac = Xc_.col(nC) - Cc; 
169 | 		N = Ac * Aw.transpose();
170 | 		M += N;
171 | 	}
172 | 
173 | 	M /= (Tp)K;
174 | 	sigma_w_sqr /= (Tp)K;
175 | 
176 | 	Matrix<Tp, 3, 3> M_n; M_n.setZero();
177 | 	for (int nC = 0; nC < K; nC++){
178 | 		//pure imaginary Shortcuts
179 | 		Aw = Nw_.col(nC);
180 | 		Ac = Nc_.col(nC);
181 | 		N = Ac * Aw.transpose();
182 | 		M_n += (sigma_w_sqr*N);
183 | 	}
184 | 
185 | 	M_n /= (Tp)K;
186 | 	M += M_n;
187 | 
188 | 	JacobiSVD<Matrix<Tp, 3,3> > svd(M, ComputeFullU | ComputeFullV);
189 | 	//[U S V]=svd(M);
190 | 	//R=U*V';
191 | 	Matrix<Tp, 3, 3> U = svd.matrixU();
192 | 	Matrix<Tp, 3, 3> V = svd.matrixV();
193 | 	Matrix<Tp, 3, 1> D = svd.singularValues();
194 | 	Sophus::SO3<Tp> R_tmp;
195 | 	Matrix<Tp, 3, 1> TMP = U*V.transpose();
196 | 	Tp d = TMP.determinant();
197 | 	if (d < 0) {
198 | 		Matrix<Tp, 3, 3> I = Matrix<Tp, 3, 3>::Identity(); I(2, 2) = -1; D(2) *= -1;
199 | 		R_tmp = Sophus::SO3<Tp>(U*I*V.transpose());
200 | 	}
201 | 	else{
202 | 		R_tmp = Sophus::SO3<Tp>(U*V.transpose());
203 | 	}
204 | 	//T=ccent'-R*wcent';
205 | 	Matrix< Tp, 3, 1> T_tmp = Cc - R_tmp * Cw;
206 | 
207 | 	RT solution = RT( R_tmp, T_tmp) ;
208 | 
209 | 	//T tr = D.sum();
210 | 	//T dE_sqr = sigma_c_sqr - tr*tr / sigma_w_sqr;
211 | 	//PRINT(dE_sqr);
212 | 	return solution; // dE_sqr;
213 | }
214 | 
215 | template< typename Tp > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
216 | void nl_kneip_ransac(NormalAOPoseAdapter<Tp>& adapter, const Tp thre_2d_, const Tp nl_thre, 
217 | 	int& Iter, Tp confidence = 0.99){
218 | 	typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
219 | 	typedef Matrix<Tp, 3, 1> Point3;
220 | 	typedef Sophus::SE3<Tp> RT;
221 | 
222 | 	Tp cos_thr = cos(atan(thre_2d_ / adapter.getFocal()));
223 | 	Tp cos_nl_thre = cos(nl_thre);
224 | 
225 | 	RandomElements<int> re((int)adapter.getNumberCorrespondences());
226 | 	const int K = 3;
227 | 	MatrixX Xw(3, K + 1), Xc(3, K + 1), bv(3, K + 1);
228 | 	MatrixX Nw(3, K + 1), Nc(3, K + 1);
229 | 	Matrix<short, Dynamic, Dynamic> inliers_kneip(adapter.getNumberCorrespondences(), 3); 
230 | 
231 | 	adapter.setMaxVotes(-1);
232 | 	for (int ii = 0; ii < Iter; ii++)	{
233 | 		//randomly select K candidates
234 | 		RT solution_kneip;
235 | 		vector<int> selected_cols;
236 | 		re.run(K + 1, &selected_cols);
237 | 
238 | 		assign_sample<Tp>(adapter, selected_cols, &Xw, &Nw, &Xc, &Nc, &bv);
239 | 		if (!kneip<Tp>(Xw, bv, &solution_kneip))	continue;
240 | 
241 | 		//collect votes
242 | 		int votes_kneip = 0;
243 | 		inliers_kneip.setZero();
244 | 		Point3 eivE; Point3 pc; Tp cos_a;
245 | 		for (int c = 0; c < adapter.getNumberCorrespondences(); c++) {
246 | 			if (adapter.isValid(c)){
247 | 				//with normal data
248 | 				Tp cos_alpha = adapter.getNormalCurr(c).dot(solution_kneip.so3() * adapter.getNormalGlob(c));
249 | 				if (cos_alpha > cos_nl_thre){
250 | 					inliers_kneip(c, 2) = 1;
251 | 					votes_kneip++;
252 | 				}
253 | 			}
254 | 			//with 2d
255 | 			pc = solution_kneip.so3() * adapter.getPointGlob(c) + solution_kneip.translation();// transform pw into pc
256 | 			pc = pc / pc.norm(); //normalize pc
257 | 
258 | 			//compute the score
259 | 			cos_a = pc.dot( adapter.getBearingVector(c) );
260 | 			if (cos_a > cos_thr){
261 | 				inliers_kneip(c, 0) = 1;
262 | 				votes_kneip++;
263 | 			}
264 | 		}
265 | 		//cout << endl;
266 | 
267 | 		if ( votes_kneip > adapter.getMaxVotes() ){
268 | 			assert(votes_kneip == inliers_kneip.sum());
269 | 			adapter.setMaxVotes(votes_kneip);
270 | 			adapter.setRcw(solution_kneip.so3());
271 | 			adapter.sett(solution_kneip.translation());
272 | 			adapter.setInlier(inliers_kneip);
273 | 
274 | 			//cout << inliers_kneip.inverse() << endl << endl;
275 | 			//adapter.printInlier();
276 | 			Iter = RANSACUpdateNumIters(confidence, (Tp)(adapter.getNumberCorrespondences() * 2 - votes_kneip) / adapter.getNumberCorrespondences() / 2, K, Iter);
277 | 		}
278 | 	}
279 | 	PnPPoseAdapter<Tp>* pAdapter = &adapter;
280 | 	pAdapter->cvtInlier();
281 | 	adapter.cvtInlier();
282 | 
283 | 	return;
284 | }
285 | 
286 | template< typename Tp > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
287 | void nl_shinji_ransac(NormalAOPoseAdapter<Tp>& adapter, const Tp thre_3d_, const Tp nl_thre, 
288 | 	int& Iter, Tp confidence = 0.99){
289 | 	// eimXc_ = R * eimXw_ + T; //R and t is defined in world and transform a point in world to local
290 | 	typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
291 | 	typedef Matrix<Tp, 3, 1> Point3;
292 | 	typedef Sophus::SE3<Tp> RT;
293 | 
294 | 	Tp cos_nl_thre = cos(nl_thre);
295 | 
296 | 	RandomElements<int> re((int)adapter.getNumberCorrespondences());
297 | 	const int K = 3;
298 | 	MatrixX Xw(3, K + 1), Xc(3, K + 1), bv(3, K + 1);
299 | 	MatrixX Nw(3, K + 1), Nc(3, K + 1);
300 | 	Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(), 3);
301 | 
302 | 	adapter.setMaxVotes(-1);
303 | 	for (int ii = 0; ii < Iter; ii++)	{
304 | 		//randomly select K candidates
305 | 		RT solution_shinji, solution_nl;
306 | 		vector<int> selected_cols;
307 | 		re.run(K + 1, &selected_cols);
308 | 		vector<RT> v_solutions;
309 | 
310 | 		if (assign_sample<Tp>(adapter, selected_cols, &Xw, &Nw, &Xc, &Nc, &bv)){
311 | 			solution_shinji = shinji<Tp>(Xw, Xc, K);
312 | 			v_solutions.push_back(solution_shinji);
313 | 		}
314 | 
315 | 		nl_2p<Tp>(Xc.col(0), Nc.col(0), Xc.col(1), Xw.col(0), Nw.col(0), Xw.col(1), &solution_nl);
316 | 		v_solutions.push_back(solution_nl);
317 | 		for (typename vector<RT>::iterator itr = v_solutions.begin(); itr != v_solutions.end(); ++itr) {
318 | 			//collect votes
319 | 			int votes = 0;
320 | 			inliers.setZero();
321 | 			Point3 eivE; Point3 pc; 
322 | 			for (int c = 0; c < adapter.getNumberCorrespondences(); c++) {
323 | 				if (adapter.isValid(c)){
324 | 					//voted by N-N correspondences
325 | 					Tp cos_alpha = adapter.getNormalCurr(c).dot(itr->so3() * adapter.getNormalGlob(c));
326 | 					if (cos_alpha > cos_nl_thre){
327 | 						inliers(c, 2) = 1;
328 | 						votes++;
329 | 					}
330 | 					//voted by 3-3 correspondences
331 | 					eivE = adapter.getPointCurr(c) - (itr->so3() * adapter.getPointGlob(c) + itr->translation());
332 | 					if (eivE.norm() < thre_3d_){
333 | 						inliers(c, 1) = 1;
334 | 						votes++;
335 | 					}
336 | 				}
337 | 			}
338 | 			//cout << endl;
339 | 			if (votes > adapter.getMaxVotes() ){
340 | 				assert(votes == inliers.sum());
341 | 				adapter.setMaxVotes(votes);
342 | 				adapter.setRcw(itr->so3());
343 | 				adapter.sett(itr->translation());
344 | 				adapter.setInlier(inliers);
345 | 				//adapter.printInlier();
346 | 				Iter = RANSACUpdateNumIters(confidence, (Tp)(adapter.getNumberCorrespondences() * 2 - votes) / adapter.getNumberCorrespondences() / 2, K, Iter);
347 | 			}
348 | 		}
349 | 	}
350 | 	AOPoseAdapter<Tp>* pAdapter = &adapter;
351 | 	pAdapter->cvtInlier();
352 | 	adapter.cvtInlier();
353 | 	return;
354 | }
355 | 
356 | template< typename Tp > 
357 | void nl_shinji_kneip_ransac(NormalAOPoseAdapter<Tp>& adapter, 
358 | 	const Tp thre_3d_, const Tp thre_2d_, const Tp nl_thre, int& Iter, Tp confidence = 0.99){
359 | 	typedef Matrix<Tp, Dynamic, Dynamic> MX;
360 | 	typedef Matrix<Tp, 3, 1> P3;
361 | 	typedef Sophus::SE3<Tp> RT;
362 | 
363 | 	Tp cos_thr = cos(atan(thre_2d_ / adapter.getFocal()));
364 | 	Tp cos_nl_thre = cos(nl_thre);
365 | 
366 | 	RandomElements<int> re((int)adapter.getNumberCorrespondences());
367 | 	const int K = 3;
368 | 	MX Xw(3, K + 1), Xc(3, K + 1), bv(3, K + 1);
369 | 	MX Nw(3, K + 1), Nc(3, K + 1);
370 | 	Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(), 3);
371 | 
372 | 	adapter.setMaxVotes(-1);
373 | 	for (int ii = 0; ii < Iter; ii++)	{
374 | 		//randomly select K candidates
375 | 		RT solution_kneip, solution_shinji, solution_nl;
376 | 		vector<RT> v_solutions;
377 | 		vector<int> selected_cols;
378 | 		re.run(K + 1, &selected_cols);
379 | 
380 | 		if (assign_sample<Tp>(adapter, selected_cols, &Xw, &Nw, &Xc, &Nc, &bv)){
381 | 			solution_shinji = shinji<Tp>(Xw, Xc, K);
382 | 			v_solutions.push_back(solution_shinji);
383 | 		}
384 | 
385 | 		if (kneip<Tp>(Xw, bv, &solution_kneip)){
386 | 			v_solutions.push_back(solution_kneip);
387 | 		}
388 | 
389 | 		nl_2p<Tp>(Xc.col(0), Nc.col(0), Xc.col(1), Xw.col(0), Nw.col(0), Xw.col(1), &solution_nl);
390 | 		v_solutions.push_back(solution_nl);
391 | 
392 | 		for (typename vector<RT>::iterator itr = v_solutions.begin(); itr != v_solutions.end(); ++itr) {
393 | 			//collect votes
394 | 			int votes = 0;
395 | 			inliers.setZero();
396 | 			P3 eivE; P3 pc; Tp cos_a;
397 | 			for (int c = 0; c < adapter.getNumberCorrespondences(); c++) {
398 | 				if (adapter.isValid(c)){
399 | 					//with normal data
400 | 					Tp cos_alpha = adapter.getNormalCurr(c).dot(itr->so3() * adapter.getNormalGlob(c));
401 | 					if (cos_alpha > cos_nl_thre){
402 | 						inliers(c, 2) = 1;
403 | 						votes++;
404 | 					}
405 | 				
406 | 					//with 3d data
407 | 					eivE = adapter.getPointCurr(c) - (itr->so3() * adapter.getPointGlob(c) + itr->translation());
408 | 					if (eivE.norm() < thre_3d_){
409 | 						inliers(c, 1) = 1;
410 | 						votes++;
411 | 					}
412 | 				}
413 | 				//with 2d
414 | 				pc = itr->so3() * adapter.getPointGlob(c) + itr->translation();// transform pw into pc
415 | 				pc = pc / pc.norm(); //normalize pc
416 | 
417 | 				//compute the score
418 | 				cos_a = pc.dot( adapter.getBearingVector(c) );
419 | 				if (cos_a > cos_thr){
420 | 					inliers(c, 0) = 1;
421 | 					votes++;
422 | 				}
423 | 			}
424 | 			//cout << endl;
425 | 
426 | 			if (votes > adapter.getMaxVotes() ){
427 | 				assert(votes == inliers.sum());
428 | 				adapter.setMaxVotes(votes);
429 | 				adapter.setRcw(itr->so3());
430 | 				adapter.sett(itr->translation());
431 | 				adapter.setInlier(inliers);
432 | 
433 | 				//cout << inliers.inverse() << endl << endl;
434 | 				//adapter.printInlier();
435 | 				Iter = RANSACUpdateNumIters(confidence, (Tp)(adapter.getNumberCorrespondences() * 3 - votes) / adapter.getNumberCorrespondences() / 3, K, Iter);
436 | 			}
437 | 		}//for(vector<RT>::iterator itr = v_solutions.begin() ...
438 | 	}//for(int ii = 0; ii < Iter; ii++)
439 | 	PnPPoseAdapter<Tp>* pAdapterPnP = &adapter;
440 | 	pAdapterPnP->cvtInlier();
441 | 	AOPoseAdapter<Tp>* pAdapterAO = &adapter;
442 | 	pAdapterAO->cvtInlier();
443 | 	adapter.cvtInlier();
444 | 	return;
445 | }
446 | 
447 | template< typename Tp > 
448 | void nl_shinji_kneip_ls(NormalAOPoseAdapter<Tp>& adapter)
449 | {
450 | 	// typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
451 | 	typedef Matrix<Tp, 3, 1> V3;
452 | 	typedef Matrix<Tp, 3, 3> M3;
453 | 	// typedef Matrix<Tp, 3, 4> RT;
454 | 	if(adapter.getMaxVotes() == 0) return;
455 | 
456 | 	//Compute the centroid of each point set
457 | 	V3 Cw(0, 0, 0), Cc(0, 0, 0); int N = 0; Tp TV = 0;
458 | 	for (int nCount = 0; nCount < adapter.getNumberCorrespondences(); nCount++){
459 | 		if (adapter.isInlier33(nCount)){
460 | 			Tp v = adapter.weight33(nCount);
461 | 			Cw += (v * adapter.getPointGlob(nCount));
462 | 			Cc += (v * adapter.getPointCurr(nCount));
463 | 			TV += v; N++;
464 | 		}
465 | 	}
466 | 	if (N > 2){
467 | 		Cw /= TV;
468 | 		Cc /= TV;
469 | 	}
470 | 
471 | 	// transform coordinate
472 | 	V3 Aw, Ac;
473 | 	M3 M33; M33.setZero();
474 | 	M3 MNN; MNN.setZero();
475 | 	M3 M23; M23.setZero();
476 | 	int M = 0; Tp TL = 0;
477 | 	int K = 0; Tp TW = 0;
478 | 
479 | 	V3 c_opt = adapter.getRcw().inverse() * (-adapter.gettw()); //camera centre in WRS
480 | 	Sophus::SO3<Tp> R_opt;
481 | 	for (int ii = 0; ii < 3; ii++)
482 | 	{
483 | 		Tp sigma_w_sqr = 0.;
484 | 		for (int nC = 0; nC < adapter.getNumberCorrespondences(); nC++){
485 | 			if (adapter.isInlier23(nC)){
486 | 				Tp w = adapter.weight23(nC);
487 | 				Aw = adapter.getPointGlob(nC) - c_opt; Aw.normalize();
488 | 				Ac = adapter.getBearingVector(nC);
489 | 				M23 += (w * Ac * Aw.transpose());
490 | 				TW += w;  K++;
491 | 			}
492 | 			if (adapter.isInlier33(nC)){
493 | 				Tp v = adapter.weight33(nC);
494 | 				Aw = adapter.getPointGlob(nC) - Cw;
495 | 				Ac = adapter.getPointCurr(nC) - Cc; sigma_w_sqr += (v*Ac.squaredNorm());
496 | 				M33 += (v * Ac * Aw.transpose());
497 | 			}
498 | 			if (adapter.isInlierNN(nC)){
499 | 				Tp lambda = adapter.weightNN(nC);
500 | 				Aw = adapter.getNormalGlob(nC);
501 | 				Ac = adapter.getNormalCurr(nC);
502 | 				MNN += (lambda * Ac * Aw.transpose());
503 | 				TL += lambda;  M++;
504 | 			}
505 | 		}
506 | 		if (N > 2) { M33 /= TV; sigma_w_sqr /= TV; } else { M33.setZero(); sigma_w_sqr = 1.;}
507 | 		if (M > 0) { MNN /= TL; } else { MNN.setZero(); }
508 | 		if (K > 0) { M23 /= TW; } else { M23.setZero(); }
509 | 
510 | 		M33 += sigma_w_sqr*( M23 + MNN );
511 | 
512 | 		JacobiSVD< M3 > svd(M33, ComputeFullU | ComputeFullV);
513 | 		//[U S V]=svd(M);
514 | 		//R=U*V';
515 | 		
516 | 		
517 | 		M3 U = svd.matrixU();
518 | 		M3 V = svd.matrixV();
519 | 		// V3 D = svd.singularValues();
520 | 		M3 TMP = U*V.transpose();
521 | 		Tp d = TMP.determinant();
522 | 		if (d < 0) {
523 | 			M3 I = M3::Identity(); I(2, 2) = -1; //D(2) *= -1;
524 | 			R_opt = Sophus::SO3<Tp>(U*I*V.transpose());
525 | 		}
526 | 		else{
527 | 			R_opt = Sophus::SO3<Tp>(U*V.transpose());
528 | 		}
529 | 		//T=ccent'-R*wcent';
530 | 		V3 c = Cw - R_opt.inverse() * Cc;
531 | 		V3 cp = find_opt_cc<Tp>(adapter);
532 | 		if (N > 2 ){
533 | 			if (cp[0] == cp[0])
534 | 				c_opt = Tp(K) / (K + N)*cp + Tp(N) / (K + N)*c;
535 | 			else
536 | 				c_opt = c;
537 | 		}
538 | 		else{
539 | 			if (cp[0] == cp[0])
540 | 				c_opt = cp;
541 | 			else
542 | 				break;
543 | 		}
544 | 	}//for iterations
545 | 	adapter.setRcw(R_opt);
546 | 	adapter.sett(R_opt*(-c_opt));
547 | 
548 | 	//T tr = D.sum();
549 | 	//T dE_sqr = sigma_c_sqr - tr*tr / sigma_w_sqr;
550 | 	//PRINT(dE_sqr);
551 | 	return;
552 | }
553 | 
554 | 
555 | 
556 | #endif
557 | 


--------------------------------------------------------------------------------
/pose/MinimalSolvers.hpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #ifndef BTL_MIN_SOLV_HEADER
  3 | #define BTL_MIN_SOLV_HEADER
  4 | 
  5 | #include <Eigen/Dense>
  6 | #include <se3.hpp>
  7 | 
  8 | using namespace Eigen;
  9 | 
 10 | template< typename T > 
 11 | void ms(const Matrix< T, 3, 1 >& Aw_, const Matrix< T, 3, 1 >& Bw_,
 12 | 	const Matrix< T, 3, 1 >& Nw_, const Matrix< T, 3, 1 >& Mw_,
 13 | 	const Matrix< T, 3, 1 >& Ac_, const Matrix< T, 3, 1 >& Bc_,
 14 | 	const Matrix< T, 3, 1 >& Nc_, const Matrix< T, 3, 1 >& Mc_,
 15 | 	Sophus::SO3< T >* pR_cw_, Matrix< T, 3, 1 >* pTw_){
 16 | 	// the minimal solver to solve the 2 pairs of correspondences
 17 | 	// where each correspondence contains a 3-D location as well as a surface normal
 18 | 	// Xc_ = R_cw * Xw_ + t_w; //R_cw and t_w is defined in world and transform a point in world to local
 19 | 
 20 | 	//1
 21 | 	Matrix< T, 3, 1 > Cw = (Aw_ + Bw_) / T(2); // centre of AwBw
 22 | 	Matrix< T, 3, 1 > Cc = (Ac_ + Bc_) / T(2); // centre of AcBc
 23 | 
 24 | 	//2 estimate the translation
 25 | 	*pTw_ = Cc - Cw;
 26 | 
 27 | 	//3 translate 
 28 | 	Aw_ += *pTw_;
 29 | 	Bw_ += *pTw_;
 30 | 
 31 | 	//4 rotate around the axis perpendicular to the plane determined by AwBw and AcBc
 32 | 	Matrix< T, 3, 1 > ABw = Aw_ - Bw_;
 33 | 	Matrix< T, 3, 1 > ABc = Ac_ - Bc_;
 34 | 
 35 | 	Matrix< T, 3, 1 > A1 = ABw.cross(ABc);
 36 | 	T theta = asin(A1.norm() / ABw.norm() / ABc.norm());
 37 | 	Matrix< T, 3, 3 > R1 = AngleAxis<T>(A1.normalize(), theta).toRotationMatrix();
 38 | 
 39 | 	Aw_ = R1*Aw_;
 40 | 	Bw_ = R1*Bw_;
 41 | 	Nw_ = R1*Nw_;
 42 | 	Mw_ = R1*Mw_;
 43 | 
 44 | 	//5 rotation around axis ABc
 45 | 
 46 | 	return;
 47 | }
 48 | 
 49 | template< class T > 
 50 | void ev(const Matrix<T, 3, 3>& M_, Matrix<T, 3, 1>* pE_, Matrix<T, 3, 3>* pV_=NULL)
 51 | {
 52 | 	//source http://en.wikipedia.org/wiki/Eigenvalue_algorithm
 53 | 	T p1 = M_(0, 1)*M_(0, 1) + M_(0, 2)*M_(0, 2) + M_(1, 2)*M_(1, 2);
 54 | 	if (fabs(p1) < 0.00001){
 55 | 		// A is diagonal.
 56 | 		(*pE_)(0) = M_(0, 0);
 57 | 		(*pE_)(1) = M_(1, 1);
 58 | 		(*pE_)(2) = M_(2, 2);
 59 | 	}
 60 | 	else{
 61 | 		T q = M_(0, 0) + M_(1, 1) + M_(2, 2); q /= 3;
 62 | 		T t1 = M_(0, 0) - q;
 63 | 		T t2 = M_(1, 1) - q;
 64 | 		T t3 = M_(2, 2) - q;
 65 | 		T p2 = t1*t1 + t2*t2 + t3*t3 + 2 * p1;
 66 | 		T p = sqrt(p2 / 6);
 67 | 		Matrix<T, 3, 3> B = (1 / p)*(M_ - q*Matrix<T, 3, 3>::Identity());
 68 | 		T r = B.determinant() / 2;
 69 | 
 70 | 		// In exact arithmetic for a symmetric matrix - 1 <= r <= 1
 71 | 		// but computation error can leave it slightly outside this range.
 72 | 		T phi;
 73 | 		if (r <= -1)
 74 | 			phi = T(3.141592653589793238) / 3;
 75 | 		else if (r >= 1)
 76 | 			phi = 0;
 77 | 		else
 78 | 			phi = acos(r) / 3;
 79 | 
 80 | 		// the eigenvalues satisfy eig3 <= eig2 <= eig1
 81 | 		(*pE_)(0) = q + 2 * p * cos(phi);
 82 | 		(*pE_)(2) = q + 2 * p * cos(phi + (2 * T(3.141592653589793238) / 3));
 83 | 		(*pE_)(1) = 3 * q - (*pE_)(0) - (*pE_)(2);  //since trace(A) = eig1 + eig2 + eig3
 84 | 	}
 85 | 	//solve out eigen vectors
 86 | 	if (pV_){
 87 | 		Matrix<T, 3, 3> s0 = ((*pE_)(0)*Matrix<T, 3, 3>::Identity() - M_);
 88 | 		Matrix<T, 3, 3> s1 = ((*pE_)(1)*Matrix<T, 3, 3>::Identity() - M_);
 89 | 		Matrix<T, 3, 3> s2 = ((*pE_)(2)*Matrix<T, 3, 3>::Identity() - M_);
 90 | 		{
 91 | 			Matrix<T, 3, 3> s = s0 * s1;
 92 | 			(*pV_).col(2) = s.col(2).normalize();
 93 | 		}
 94 | 		{
 95 | 			Matrix<T, 3, 3> s = s0 * s2;
 96 | 			(*pV_).col(1) = s.col(1).normalize();
 97 | 		}
 98 | 		{
 99 | 			Matrix<T, 3, 3> s = s1 * s2;
100 | 			(*pV_).col(0) = s.col(0).normalize();
101 | 		}
102 | 	}
103 | 	return;
104 | }
105 | 
106 | #endif
107 | 


--------------------------------------------------------------------------------
/pose/NormalAOPoseAdapter.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef _NORMAL_AO_POSE_ADAPTER_HEADER_
  2 | #define _NORMAL_AO_POSE_ADAPTER_HEADER_
  3 | 
  4 | #include <stdlib.h>
  5 | #include <vector>
  6 | #include <AOPoseAdapter.hpp>
  7 | #include <limits>
  8 | 
  9 | using namespace std;
 10 | using namespace Eigen;
 11 | /**
 12 |  * \brief The namespace for the absolute pose methods.
 13 |  */
 14 | 
 15 | 
 16 | template<typename Tp>
 17 | class NormalAOPoseAdapter : public AOPoseAdapter<Tp>
 18 | {
 19 | 
 20 | protected:
 21 | 	using PoseAdapterBase<Tp>::_t_w;
 22 | 	using PoseAdapterBase<Tp>::_R_cw;
 23 | 	using PnPPoseAdapter<Tp>::_bearingVectors;
 24 | 	using PnPPoseAdapter<Tp>::_points_g;
 25 | 	using AOPoseAdapter<Tp>::_points_c;
 26 | 	typedef typename PoseAdapterBase<Tp>::Point3 Point3;
 27 | 
 28 | public:
 29 | 	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 30 | 	typedef typename PoseAdapterBase<Tp>::Vector3 Vector3;
 31 | 	typedef typename PoseAdapterBase<Tp>::SO3_T SO3_T;
 32 | 	typedef typename PnPPoseAdapter<Tp>::MatrixX MatrixX;
 33 | 
 34 |   /**
 35 |    * \brief Constructor. See protected class-members to understand parameters
 36 |    */
 37 |    NormalAOPoseAdapter(
 38 |       const MatrixX & bearingVectors,
 39 | 	  const MatrixX & points_c,
 40 | 	  const MatrixX & normal_c,
 41 | 	  const MatrixX & points_g,
 42 | 	  const MatrixX & normal_g);
 43 |   /**
 44 |    * \brief Constructor. See protected class-members to understand parameters
 45 |    */
 46 |    NormalAOPoseAdapter(
 47 | 	  const MatrixX & bearingVectors,
 48 | 	  const MatrixX & points_c,
 49 | 	  const MatrixX & normal_c,
 50 | 	  const MatrixX & points_g,
 51 | 	  const MatrixX & normal_g,
 52 | 	  const SO3_T & R);
 53 |   /**
 54 |    * \brief Constructor. See protected class-members to understand parameters
 55 |    */
 56 |   NormalAOPoseAdapter(
 57 | 	  const MatrixX & bearingVectors,
 58 | 	  const MatrixX & points_c,
 59 | 	  const MatrixX & normal_c,
 60 | 	  const MatrixX & points_g,
 61 | 	  const MatrixX & normal_g,
 62 | 	  const Vector3 & t,
 63 |       const SO3_T & R );
 64 |   /**
 65 |    * Destructor
 66 |    */
 67 |   virtual ~NormalAOPoseAdapter();
 68 | 
 69 |   //Access of correspondences
 70 |   bool isInlierNN(int index) const;
 71 |   Tp weightNN(int index) const;
 72 |   /** See parent-class */
 73 |   virtual Point3 getNormalCurr( int index ) const;
 74 |   virtual Point3 getNormalGlob( int index ) const;
 75 |   virtual void setInlier(const Matrix<short, Dynamic, Dynamic>& inliers);
 76 |   virtual void setWeights(const Matrix<Tp, Dynamic, Dynamic>& weights);
 77 |   virtual void printInlier() const;
 78 |   const vector<short>& getInlierIdx() const { return _vInliersNN; }
 79 |   void cvtInlier();
 80 | 
 81 | protected:
 82 |   /** Reference to the 3-D points in the camera-frame */
 83 | 	const MatrixX & _normal_c;
 84 | 	const MatrixX & _normal_g;
 85 | 
 86 | 	/** flags of inliers. */
 87 | 	Matrix<short, Dynamic, 1> _inliers_nl;
 88 | 	Matrix<Tp, Dynamic, 1> _weights_nl;
 89 | 
 90 | 	vector<short> _vInliersNN;
 91 | };
 92 | 
 93 | 
 94 | template<typename Tp>
 95 | NormalAOPoseAdapter<Tp>::NormalAOPoseAdapter(
 96 | 	const MatrixX & bearingVectors,
 97 | 	const MatrixX & points_c,
 98 | 	const MatrixX & normal_c,
 99 | 	const MatrixX & points_g,
100 | 	const MatrixX & normal_g ) :
101 | 	AOPoseAdapter<Tp>(bearingVectors, points_c, points_g),
102 | 	_normal_c(normal_c),
103 | 	_normal_g(normal_g)
104 | {
105 | 	_inliers_nl.resize(_bearingVectors.cols());
106 | 	_inliers_nl.setOnes();
107 | }
108 | 
109 | template<typename Tp>
110 | NormalAOPoseAdapter<Tp>::NormalAOPoseAdapter(
111 | 	const MatrixX & bearingVectors,
112 | 	const MatrixX & points_c,
113 | 	const MatrixX & normal_c,
114 | 	const MatrixX & points_g,
115 | 	const MatrixX & normal_g,
116 | 	const SO3_T & R) :
117 | 	AOPoseAdapter<Tp>(bearingVectors, points_c, points_g, R),
118 | 	_normal_c(normal_c),
119 | 	_normal_g(normal_g)
120 | {
121 | 	_inliers_nl.resize(_bearingVectors.cols());
122 | 	_inliers_nl.setOnes();
123 | }
124 | 
125 | template<typename Tp>
126 | NormalAOPoseAdapter<Tp>::NormalAOPoseAdapter(
127 | 	const MatrixX & bearingVectors,
128 | 	const MatrixX & points_c,
129 | 	const MatrixX & normal_c,
130 | 	const MatrixX & points_g,
131 | 	const MatrixX & normal_g, 
132 | 	const Vector3 & t,
133 | 	const SO3_T & R) :
134 | 	AOPoseAdapter<Tp>(bearingVectors, points_c, points_g, t, R),
135 | 	_normal_c(normal_c),
136 | 	_normal_g(normal_g)
137 | {
138 | 	_inliers_nl.resize(_bearingVectors.cols());
139 | 	_inliers_nl.setOnes();
140 | }
141 | 
142 | template<typename Tp>
143 | NormalAOPoseAdapter<Tp>::~NormalAOPoseAdapter()
144 | {}
145 | 
146 | template<typename Tp>
147 | bool NormalAOPoseAdapter<Tp>::isInlierNN(int index) const
148 | {
149 | 	assert(index < _inliers_nl.rows());
150 | 	return _inliers_nl(index) == 1;
151 | }
152 | 
153 | template<typename Tp>
154 | Tp NormalAOPoseAdapter<Tp>::weightNN(int index) const
155 | {
156 | 	if (!_weights_nl.rows()) return Tp(1.0);
157 | 	else{
158 | 		assert(index < _weights_nl.rows());
159 | 		return Tp(_weights_nl(index)) / numeric_limits<short>::max();
160 | 	}
161 | }
162 | 
163 | template<typename Tp>
164 | typename NormalAOPoseAdapter<Tp>::Point3 NormalAOPoseAdapter<Tp>::getNormalCurr(
165 | 	int index) const
166 | {
167 | 	assert(index < _bearingVectors.cols() );
168 | 	return _normal_c.col(index);
169 | }
170 | 
171 | template<typename Tp>
172 | typename NormalAOPoseAdapter<Tp>::Point3 NormalAOPoseAdapter<Tp>::getNormalGlob(
173 | 	int index) const
174 | {
175 | 	assert(index < _bearingVectors.cols());
176 | 	return _normal_g.col(index);
177 | }
178 | 
179 | template <typename Tp>
180 | void NormalAOPoseAdapter<Tp>::setInlier(const Matrix<short, Dynamic, Dynamic>& inliers)
181 | {
182 | 	assert(inliers.rows() == _inliers_nl.rows());
183 | 	if (inliers.cols() == 1){
184 | 		PnPPoseAdapter<Tp>::setInlier(inliers);
185 | 	}
186 | 	if (inliers.cols() == 2){
187 | 		AOPoseAdapter<Tp>::setInlier(inliers);
188 | 	}
189 | 	if (inliers.cols() == 3){
190 | 		AOPoseAdapter<Tp>::setInlier(inliers);
191 | 		//_inliers_nl = inliers.col(2);
192 | 		memcpy(_inliers_nl.data(), inliers.col(2).data(), inliers.rows() * 2);
193 | 	}
194 | 	return;
195 | }
196 | 
197 | template <typename Tp>
198 | void NormalAOPoseAdapter<Tp>::setWeights(const Matrix<Tp, Dynamic, Dynamic>& weights)
199 | {
200 | 	if (weights.cols() == 1){
201 | 		PnPPoseAdapter<Tp>::setWeights(weights);
202 | 	}
203 | 	if (weights.cols() == 2){
204 | 		AOPoseAdapter<Tp>::setWeights(weights);
205 | 	}
206 | 	if (weights.cols() == 3){
207 | 		AOPoseAdapter<Tp>::setWeights(weights);
208 | 		_weights_nl = weights.col(2);
209 | 		//memcpy(_weights_nl.data(), weights.col(2).data(), weights.rows() * 2);
210 | 	}
211 | 	return;
212 | }
213 | 
214 | template <typename Tp>
215 | void NormalAOPoseAdapter<Tp>::printInlier() const
216 | {
217 | 	AOPoseAdapter<Tp>::printInlier();
218 | 	cout << _inliers_nl.transpose() << endl;
219 | }
220 | 
221 | template <typename Tp>
222 | void NormalAOPoseAdapter<Tp>::cvtInlier()
223 | {
224 | 	_vInliersNN.clear();
225 | 	_vInliersNN.reserve(this->getNumberCorrespondences());
226 | 	for (short r = 0; r < (short)_inliers_nl.rows(); r++) {
227 | 		if (1 == _inliers_nl[r]){
228 | 			_vInliersNN.push_back(r);
229 | 		}
230 | 	}
231 | }
232 | 
233 | #endif 
234 | 


--------------------------------------------------------------------------------
/pose/P3P.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef BTL_P3P_POSE_HEADER
  2 | #define BTL_P3P_POSE_HEADER
  3 | 
  4 | 
  5 | #include <Eigen/Dense>
  6 | #include "Utility.hpp"
  7 | #include "PnPPoseAdapter.hpp"
  8 | 
  9 | using namespace Eigen;
 10 | 
 11 | template< typename Tp >
 12 | std::vector<Tp> o4_roots(const Matrix<Tp, Dynamic, Dynamic> & p_)
 13 | {
 14 | 	Tp A = p_(0, 0);
 15 | 	Tp B = p_(1, 0);
 16 | 	Tp C = p_(2, 0);
 17 | 	Tp D = p_(3, 0);
 18 | 	Tp E = p_(4, 0);
 19 | 
 20 | 	Tp A_pw2 = A*A;
 21 | 	Tp B_pw2 = B*B;
 22 | 	Tp A_pw3 = A_pw2*A;
 23 | 	Tp B_pw3 = B_pw2*B;
 24 | 	Tp A_pw4 = A_pw3*A;
 25 | 	Tp B_pw4 = B_pw3*B;
 26 | 
 27 | 	Tp alpha = -3 * B_pw2 / (8 * A_pw2) + C / A;
 28 | 	Tp beta = B_pw3 / (8 * A_pw3) - B*C / (2 * A_pw2) + D / A;
 29 | 	Tp gamma = -3 * B_pw4 / (256 * A_pw4) + B_pw2*C / (16 * A_pw3) - B*D / (4 * A_pw2) + E / A;
 30 | 
 31 | 	Tp alpha_pw2 = alpha*alpha;
 32 | 	Tp alpha_pw3 = alpha_pw2*alpha;
 33 | 
 34 | 	std::complex<Tp> P(-alpha_pw2 / 12 - gamma, 0);
 35 | 	std::complex<Tp> Q(-alpha_pw3 / 108 + alpha*gamma / 3 - pow(beta, 2) / 8, 0);
 36 | 	std::complex<Tp> R = -Q / Tp(2.0) + sqrt(pow(Q, Tp(2.)) / Tp(4.) + pow(P, Tp(3.)) / Tp(27.));
 37 | 
 38 | 	std::complex<Tp> U = pow(R, Tp(1.0 / 3.0));
 39 | 	std::complex<Tp> y;
 40 | 
 41 | 	if (U.real() == 0)
 42 | 		y = -Tp(5.0)*alpha / Tp(6.) - pow(Q, Tp(1.0 / 3.0));
 43 | 	else
 44 | 		y = -Tp(5.0)*alpha / Tp(6.) - P / (Tp(3.)*U) + U;
 45 | 
 46 | 	std::complex<Tp> w = sqrt(alpha + Tp(2.)*y);
 47 | 
 48 | 	std::vector<Tp> realRoots;
 49 | 	std::complex<Tp> temp;
 50 | 	temp = -B / (Tp(4.)*A) + Tp(0.5)*(w + sqrt(-(Tp(3.)*alpha + Tp(2.)*y + Tp(2.)*beta / w)));
 51 | 	realRoots.push_back(temp.real());
 52 | 	temp = -B / (Tp(4.)*A) + Tp(0.5)*(w - sqrt(-(Tp(3.)*alpha + Tp(2.)*y + Tp(2.)*beta / w)));
 53 | 	realRoots.push_back(temp.real());
 54 | 	temp = -B / (Tp(4.)*A) + Tp(0.5)*(-w + sqrt(-(Tp(3.)*alpha + Tp(2.)*y - Tp(2.)*beta / w)));
 55 | 	realRoots.push_back(temp.real());
 56 | 	temp = -B / (Tp(4.)*A) + Tp(0.5)*(-w - sqrt(-(Tp(3.)*alpha + Tp(2.)*y - Tp(2.)*beta / w)));
 57 | 	realRoots.push_back(temp.real());
 58 | 
 59 | 	return realRoots;
 60 | }
 61 | 
 62 | 
 63 | template< typename Tp > 
 64 | void kneip_main(const Matrix<Tp, Dynamic, Dynamic>& X_w, const Matrix<Tp, Dynamic, Dynamic>& bv, vector< Sophus::SE3<Tp> >* p_solutions_){
 65 | 
 66 | 	p_solutions_->clear();
 67 | 
 68 | 	Matrix<Tp, 3, 1> P1 = X_w.col(0);
 69 | 	Matrix<Tp, 3, 1> P2 = X_w.col(1);
 70 | 	Matrix<Tp, 3, 1> P3 = X_w.col(2);
 71 | 
 72 | 	Matrix<Tp, 3, 1> temp1 = P2 - P1;
 73 | 	Matrix<Tp, 3, 1> temp2 = P3 - P1;
 74 | 
 75 | 	if (temp1.cross(temp2).norm() == 0)
 76 | 		return;
 77 | 
 78 | 	Matrix<Tp, 3, 1> f1 = bv.col(0);
 79 | 	Matrix<Tp, 3, 1> f2 = bv.col(1);
 80 | 	Matrix<Tp, 3, 1> f3 = bv.col(2);
 81 | 
 82 | 	Matrix<Tp, 3, 1> e1 = f1;
 83 | 	Matrix<Tp, 3, 1> e3 = f1.cross(f2);
 84 | 	e3 = e3 / e3.norm();
 85 | 	Matrix<Tp, 3, 1> e2 = e3.cross(e1);
 86 | 
 87 | 	Matrix<Tp, 3, 3> RR;
 88 | 	RR.row(0) = e1.transpose();
 89 | 	RR.row(1) = e2.transpose();
 90 | 	RR.row(2) = e3.transpose();
 91 | 
 92 | 	f3 = RR*f3;
 93 | 
 94 | 	if (f3(2, 0) > 0)
 95 | 	{
 96 | 		f1 = bv.col(1);
 97 | 		f2 = bv.col(0);
 98 | 		f3 = bv.col(2);
 99 | 
100 | 		e1 = f1;
101 | 		e3 = f1.cross(f2);
102 | 		e3 = e3 / e3.norm();
103 | 		e2 = e3.cross(e1);
104 | 
105 | 		RR.row(0) = e1.transpose();
106 | 		RR.row(1) = e2.transpose();
107 | 		RR.row(2) = e3.transpose();
108 | 
109 | 		f3 = RR*f3;
110 | 
111 | 		P1 = X_w.col(1);
112 | 		P2 = X_w.col(0);
113 | 		P3 = X_w.col(2);
114 | 	}
115 | 
116 | 	Matrix<Tp, 3, 1> n1 = P2 - P1;
117 | 	n1 = n1 / n1.norm();
118 | 	Matrix<Tp, 3, 1> n3 = n1.cross(P3 - P1);
119 | 	n3 = n3 / n3.norm();
120 | 	Matrix<Tp, 3, 1> n2 = n3.cross(n1);
121 | 
122 | 	Matrix<Tp, 3, 3> N;
123 | 	N.row(0) = n1.transpose();
124 | 	N.row(1) = n2.transpose();
125 | 	N.row(2) = n3.transpose();
126 | 
127 | 	P3 = N*(P3 - P1);
128 | 
129 | 	Tp d_12 = temp1.norm();
130 | 	Tp f_1 = f3(0, 0) / f3(2, 0);
131 | 	Tp f_2 = f3(1, 0) / f3(2, 0);
132 | 	Tp p_1 = P3(0, 0);
133 | 	Tp p_2 = P3(1, 0);
134 | 
135 | 	Tp cos_beta = f1.dot(f2);
136 | 	Tp b = 1 / (1 - pow(cos_beta, 2)) - 1;
137 | 
138 | 	if (cos_beta < 0)
139 | 		b = -sqrt(b);
140 | 	else
141 | 		b = sqrt(b);
142 | 
143 | 	Tp f_1_pw2 = pow(f_1, 2);
144 | 	Tp f_2_pw2 = pow(f_2, 2);
145 | 	Tp p_1_pw2 = pow(p_1, 2);
146 | 	Tp p_1_pw3 = p_1_pw2 * p_1;
147 | 	Tp p_1_pw4 = p_1_pw3 * p_1;
148 | 	Tp p_2_pw2 = pow(p_2, 2);
149 | 	Tp p_2_pw3 = p_2_pw2 * p_2;
150 | 	Tp p_2_pw4 = p_2_pw3 * p_2;
151 | 	Tp d_12_pw2 = pow(d_12, 2);
152 | 	Tp b_pw2 = pow(b, 2);
153 | 
154 | 	Eigen::Matrix<Tp, 5, 1> factors;
155 | 
156 | 	factors(0, 0) = -f_2_pw2*p_2_pw4
157 | 		- p_2_pw4*f_1_pw2
158 | 		- p_2_pw4;
159 | 
160 | 	factors(1, 0) = 2 * p_2_pw3*d_12*b
161 | 		+ 2 * f_2_pw2*p_2_pw3*d_12*b
162 | 		- 2 * f_2*p_2_pw3*f_1*d_12;
163 | 
164 | 	factors(2, 0) = -f_2_pw2*p_2_pw2*p_1_pw2
165 | 		- f_2_pw2*p_2_pw2*d_12_pw2*b_pw2
166 | 		- f_2_pw2*p_2_pw2*d_12_pw2
167 | 		+ f_2_pw2*p_2_pw4
168 | 		+ p_2_pw4*f_1_pw2
169 | 		+ 2 * p_1*p_2_pw2*d_12
170 | 		+ 2 * f_1*f_2*p_1*p_2_pw2*d_12*b
171 | 		- p_2_pw2*p_1_pw2*f_1_pw2
172 | 		+ 2 * p_1*p_2_pw2*f_2_pw2*d_12
173 | 		- p_2_pw2*d_12_pw2*b_pw2
174 | 		- 2 * p_1_pw2*p_2_pw2;
175 | 
176 | 	factors(3, 0) = 2 * p_1_pw2*p_2*d_12*b
177 | 		+ 2 * f_2*p_2_pw3*f_1*d_12
178 | 		- 2 * f_2_pw2*p_2_pw3*d_12*b
179 | 		- 2 * p_1*p_2*d_12_pw2*b;
180 | 
181 | 	factors(4, 0) = -2 * f_2*p_2_pw2*f_1*p_1*d_12*b
182 | 		+ f_2_pw2*p_2_pw2*d_12_pw2
183 | 		+ 2 * p_1_pw3*d_12
184 | 		- p_1_pw2*d_12_pw2
185 | 		+ f_2_pw2*p_2_pw2*p_1_pw2
186 | 		- p_1_pw4
187 | 		- 2 * f_2_pw2*p_2_pw2*p_1*d_12
188 | 		+ p_2_pw2*f_1_pw2*p_1_pw2
189 | 		+ f_2_pw2*p_2_pw2*d_12_pw2*b_pw2;
190 | 
191 | 	std::vector<Tp> realRoots = o4_roots<Tp>(factors);
192 | 
193 | 	for (int i = 0; i < 4; i++)
194 | 	{
195 | 		if (realRoots[i] != realRoots[i]) continue;
196 | 		Tp cot_alpha =
197 | 			(-f_1*p_1 / f_2 - realRoots[i] * p_2 + d_12*b) /
198 | 			(-f_1*realRoots[i] * p_2 / f_2 + p_1 - d_12);
199 | 
200 | 		Tp cos_theta = realRoots[i]; if (cos_theta > Tp(1) || cos_theta < Tp(-1)) continue;
201 | 		Tp sin_theta = sqrt(1 - realRoots[i] * realRoots[i]);
202 | 		Tp sin_alpha = sqrt(1 / (cot_alpha*cot_alpha + 1));
203 | 		Tp cos_alpha = sqrt(1 - sin_alpha* sin_alpha);
204 | 
205 | 		if (cot_alpha < 0)
206 | 			cos_alpha = -cos_alpha;
207 | 
208 | 		Matrix<Tp, 3, 1> C;
209 | 		C(0, 0) = d_12*cos_alpha*(sin_alpha*b + cos_alpha);
210 | 		C(1, 0) = cos_theta*d_12*sin_alpha*(sin_alpha*b + cos_alpha);
211 | 		C(2, 0) = sin_theta*d_12*sin_alpha*(sin_alpha*b + cos_alpha);
212 | 
213 | 		C = P1 + N.transpose() *C;
214 | 
215 | 		Matrix<Tp, 3, 3> R;
216 | 		R(0, 0) = -cos_alpha;
217 | 		R(0, 1) = -sin_alpha*cos_theta;
218 | 		R(0, 2) = -sin_alpha*sin_theta;
219 | 		R(1, 0) = sin_alpha;
220 | 		R(1, 1) = -cos_alpha*cos_theta;
221 | 		R(1, 2) = -cos_alpha*sin_theta;
222 | 		R(2, 0) = 0.0;
223 | 		R(2, 1) = -sin_theta;
224 | 		R(2, 2) = cos_theta;
225 | 
226 | 		//R = N.transpose()*R.transpose()*RR;
227 | 		R = RR.transpose()*R*N;
228 | 		if ( R(0,0)!=R(0,0) ) continue;
229 | 
230 | 		p_solutions_->push_back(Sophus::SE3<Tp>(Sophus::SO3<Tp>(R), -R*C));
231 | 	}
232 | }
233 | 
234 | template< typename Tp > /*Matrix<float,-1,-1,0,-1,-1> = MatrixXf*/
235 | vector< Sophus::SE3<Tp> > kneip(PnPPoseAdapter<Tp>& adapter, int i0 = 0, int i1 = 1, int i2 = 2){
236 | 	Matrix<Tp, Dynamic, Dynamic> bv(3,3);
237 | 	bv.col(0) = adapter.getBearingVector(i0);
238 | 	bv.col(1) = adapter.getBearingVector(i1);
239 | 	bv.col(2) = adapter.getBearingVector(i2);
240 | 	Matrix<Tp, Dynamic, Dynamic> X_w(3, 3);
241 | 	X_w.col(0) = adapter.getPointGlob(i0);
242 | 	X_w.col(1) = adapter.getPointGlob(i1);
243 | 	X_w.col(2) = adapter.getPointGlob(i2);
244 | 	
245 | 	vector< Sophus::SE3<Tp> > solutions;
246 | 	kneip_main<Tp>(X_w, bv, &solutions);
247 | 	return solutions;
248 | }
249 | 
250 | template< typename Tp > 
251 | bool kneip(const Matrix<Tp, Dynamic, Dynamic>& X_w_, const Matrix<Tp, Dynamic, Dynamic>& bv_, 
252 | 	Sophus::SE3<Tp>* p_sol_){
253 | 	typedef Matrix<Tp, 3, 1> Point3;
254 | 
255 | 	vector< Sophus::SE3<Tp> > v_solutions;
256 | 	kneip_main<Tp>(X_w_, bv_, &v_solutions);
257 | 	
258 | 	Tp minScore = numeric_limits<Tp>::max();
259 | 	int minIndex = -1;
260 | 	for (int i = 0; i < (int)v_solutions.size(); i++)
261 | 	{
262 | 		Point3 pc = v_solutions[i].so3().matrix() * X_w_.col(3) + v_solutions[i].translation();// transform pw into pc
263 | 
264 | 		//compute the score
265 | 		//Vector3 E = pc - adapter.getPointCurr(selected_cols[0]);
266 | 		//T dist = E.norm();
267 | 		//check for best solution
268 | 		//if (dist < minScore) {
269 | 		//	minScore = dist;
270 | 		//	minIndex = i;
271 | 		//}
272 | 
273 | 		pc = pc / pc.norm(); //normalize pc
274 | 
275 | 		//compute the score
276 | 		Tp score = 1.0 - pc.transpose() * bv_.col(3);
277 | 
278 | 		//check for best solution
279 | 		if (score < minScore) {
280 | 			minScore = score;
281 | 			minIndex = i;
282 | 		}
283 | 	}
284 | 
285 | 
286 | 	if (minIndex != -1){
287 | 		*p_sol_ = v_solutions[minIndex];
288 | 		return true;
289 | 	}
290 | 	else
291 | 	{
292 | 		return false;
293 | 	}
294 | }
295 | 
296 | template<typename T>
297 | int RANSACUpdateNumIters(T p, T ep, const int modelPoints, const int maxIters)
298 | {
299 | 	p = std::max(p, T(0.));
300 | 	p = std::min(p, T(1.));
301 | 	ep = std::max(ep, T(0.));
302 | 	ep = std::min(ep, T(1.));
303 | 
304 | 	// avoid inf's & nan's
305 | 	T num = std::max(T(1. - p), std::numeric_limits<T>::epsilon());
306 | 	T denom = T(1.) - std::pow(T(1. - ep), modelPoints);
307 | 	if (denom < std::numeric_limits<T>::epsilon())
308 | 		return 0;
309 | 
310 | 	num = std::log(num);
311 | 	denom = std::log(denom);
312 | 	// cout << "p " << p << endl;
313 | 	// cout << "ep " << ep << endl;
314 | 	// cout << "denom " << denom << endl;
315 | 	// cout << "num " << num << endl;
316 | 	// cout << "maxIters " << maxIters << endl;
317 | 	return denom >= 0 || -num >= maxIters*(-denom) ? maxIters : int(num / denom+0.5f);
318 | }
319 | 
320 | template< typename Tp > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
321 | void kneip_ransac( PnPPoseAdapter<Tp>& adapter,	const Tp thre_2d_, int& Iter, Tp confidence = 0.99){
322 | 
323 | 	Tp cos_thr = cos(atan(thre_2d_ / adapter.getFocal()));
324 | 
325 | 	typedef Sophus::SE3<Tp> RT;
326 | 	typedef Matrix<Tp, 3, 1> Point3;
327 | 
328 | 	RandomElements<int> re(adapter.getNumberCorrespondences());
329 | 	const int K = 4;
330 | 	adapter.setMaxVotes(-1);
331 | 	for (int i = 0; i < Iter; i++)	{
332 | 		//randomly select K candidates
333 | 		vector<int> selected_cols;
334 | 		re.run(K, &selected_cols);
335 | 
336 | 		vector< RT > solutions = kneip<Tp>(adapter, selected_cols[0], selected_cols[1], selected_cols[2]);
337 | 		//use the fourth point to verify the estimation.
338 | 		Tp minScore = 1000000.0;
339 | 		int minIndex = -1;
340 | 		for (int i = 0; i < (int)solutions.size(); i++)
341 | 		{
342 | 			Point3 pw = adapter.getPointGlob(selected_cols[3]);
343 | 			Point3 pc = solutions[i].so3().matrix() * pw + solutions[i].translation();// transform pw into pc
344 | 			pc = pc / pc.norm(); //normalize pc
345 | 
346 | 			//compute the score
347 | 			Tp score = 1.0 - pc.transpose() * adapter.getBearingVector(selected_cols[3]);
348 | 
349 | 			//check for best solution
350 | 			if (score < minScore) {
351 | 				minScore = score;
352 | 				minIndex = i;
353 | 			}
354 | 		}
355 | 
356 | 		if (minIndex != -1){
357 | 			const RT& outModel = solutions[minIndex];
358 | 
359 | 			Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(), 1);
360 | 			inliers.setZero();
361 | 			int votes = 0;
362 | 			for (int i = 0; i < adapter.getNumberCorrespondences(); i++)
363 | 			{
364 | 				Point3 Xw = adapter.getPointGlob(i);
365 | 				Point3 Xc = outModel.so3().matrix() * Xw + outModel.translation();// transform pw into pc
366 | 				Xc = Xc / Xc.norm(); //normalize pc
367 | 
368 | 				//compute the score
369 | 				Tp cos_a = Xc.dot( adapter.getBearingVector(i) );
370 | 
371 | 				//check for best solution
372 | 				if (cos_a > cos_thr) {
373 | 					inliers(i) = 1;
374 | 					votes++;
375 | 				}
376 | 			}
377 | 
378 | 			if (votes > adapter.getMaxVotes()){
379 | 				adapter.setMaxVotes(votes);
380 | 				adapter.setRcw(outModel.so3());
381 | 				adapter.sett(outModel.translation());
382 | 				adapter.setInlier(inliers);
383 | 				Iter = RANSACUpdateNumIters(confidence, (Tp)(adapter.getNumberCorrespondences() - votes) / adapter.getNumberCorrespondences(), K, Iter);
384 | 				// cout << "ransac " << Iter << endl;
385 | 			}
386 | 		}
387 | 		//update iterations
388 | 	}
389 | 	adapter.cvtInlier();
390 | 
391 | 	return;
392 | }
393 | 
394 | 
395 | template< typename Tp > 
396 | void kneip_prosac( PnPPoseAdapter<Tp>& adapter,	const Tp thre_2d_, int& Iter, Tp confidence = 0.99){
397 | 
398 | 	Tp cos_thr = cos(atan(thre_2d_ / adapter.getFocal()));
399 | 
400 | 	typedef Sophus::SE3<Tp> RT;
401 | 	typedef Matrix<Tp, 3, 1> Point3;
402 | 
403 | 	adapter.sortIdx();
404 | 	const int K = 4;
405 | 	ProsacSampler<Tp> ps(K, adapter.getNumberCorrespondences());
406 | 	adapter.setMaxVotes(-1);
407 | 	for (int i = 0; i < Iter; i++)	{
408 | 		//randomly select K candidates
409 | 		vector<int> selected_cols;
410 | 		ps.sample(&selected_cols);
411 | 		adapter.getSortedIdx(selected_cols);
412 | 
413 | 		vector< RT > solutions = kneip<Tp>(adapter, selected_cols[0], selected_cols[1], selected_cols[2]);
414 | 		//use the fourth point to verify the estimation.
415 | 		Tp minScore = 1000000.0;
416 | 		int minIndex = -1;
417 | 		for (int i = 0; i < (int)solutions.size(); i++)
418 | 		{
419 | 			Point3 pw = adapter.getPointGlob(selected_cols[3]);
420 | 			Point3 pc = solutions[i].so3().matrix() * pw + solutions[i].translation();// transform pw into pc
421 | 			pc = pc / pc.norm(); //normalize pc
422 | 
423 | 			//compute the score
424 | 			Tp score = 1.0 - pc.transpose() * adapter.getBearingVector(selected_cols[3]);
425 | 
426 | 			//check for best solution
427 | 			if (score < minScore) {
428 | 				minScore = score;
429 | 				minIndex = i;
430 | 			}
431 | 		}
432 | 
433 | 		if (minIndex != -1){
434 | 			const RT& outModel = solutions[minIndex];
435 | 
436 | 			Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(), 1);
437 | 			inliers.setZero();
438 | 			int votes = 0;
439 | 			for (int i = 0; i < adapter.getNumberCorrespondences(); i++)
440 | 			{
441 | 				Point3 Xw = adapter.getPointGlob(i);
442 | 				Point3 Xc = outModel.so3() * Xw + outModel.translation();// transform pw into pc
443 | 				Xc = Xc / Xc.norm(); //normalize pc
444 | 
445 | 				//compute the score
446 | 				Tp cos_a = Xc.dot( adapter.getBearingVector(i) );
447 | 
448 | 				//check for best solution
449 | 				if (cos_a > cos_thr) {
450 | 					inliers(i) = 1;
451 | 					votes++;
452 | 				}
453 | 			}
454 | 
455 | 			if (votes > adapter.getMaxVotes()){
456 | 				adapter.setMaxVotes(votes);
457 | 				adapter.setRcw(outModel.so3());
458 | 				adapter.sett(outModel.translation());
459 | 				adapter.setInlier(inliers);
460 | 				Iter = RANSACUpdateNumIters(confidence, (Tp)(adapter.getNumberCorrespondences() - votes) / adapter.getNumberCorrespondences(), K, Iter);
461 | 				// cout << "prosac " << Iter << endl;
462 | 			}
463 | 		}
464 | 		//update iterations
465 | 	}
466 | 	adapter.cvtInlier();
467 | 
468 | 	return;
469 | }
470 | 
471 | 
472 | template< typename Tp > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
473 | void lsq_pnp( PnPPoseAdapter<Tp>& adapter){
474 | 	// typedef Sophus::SE3<Tp> RT;
475 | 	typedef Matrix<Tp, 3, 1> Point3;
476 | 
477 | 	int total = adapter.getNumberCorrespondences();
478 | 	Tp total_err = 0.;
479 | 	//POSE_T* pErr = new POSE_T[total_err];
480 | 	for (int i = 0; i < total; i++)
481 | 	{
482 | 		Point3 Xc = adapter.getRcw()*adapter.getPointGlob(i) + adapter.gettw();
483 | 		Xc.normalize();
484 | 		Tp err = Xc.cross( adapter.getBearingVector(i) ).norm();
485 | 		total_err += err;
486 | 	}
487 | 
488 | 	//int itr = 0;
489 | 	//while (itr>=2){
490 | 	//	itr = ceil(itr / 2);
491 | 	//	for (int i = 0; i < itr; i++)
492 | 	//	{
493 | 	//		if (itr * 2 + i < total){
494 | 	//			pErr[i] += pErr[itr * 2 + i];
495 | 
496 | 	//		}
497 | 	//	}
498 | 	//}
499 | 	cout << total_err << endl;
500 | 
501 | 	return;
502 | }
503 | 
504 | 
505 | #endif
506 | 


--------------------------------------------------------------------------------
/pose/PnPPoseAdapter.hpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * \file CentralAbsoluteAdapter.hpp
  3 |  * \brief Adapter-class for passing bearing-vector-to-point correspondences to
  4 |  *        the central absolute-pose algorithms. It maps opengv types
  5 |  *        back to opengv types.
  6 |  */
  7 | 
  8 | #ifndef _PNP_POSE_ADAPTER_HEADER_
  9 | #define _PNP_POSE_ADAPTER_HEADER_
 10 | 
 11 | #include <stdlib.h>
 12 | #include <vector>
 13 | #include <PoseAdapterBase.hpp>
 14 | #include <Utility.hpp>
 15 | 
 16 | using namespace std;
 17 | using namespace Eigen;
 18 | /**
 19 |  * \brief The namespace for the absolute pose methods.
 20 |  */
 21 | 
 22 | /**
 23 |  * Check the documentation of the parent-class to understand the meaning of
 24 |  * an AbsoluteAdapter. This child-class is for the central case and holds data
 25 |  * in form of references to opengv-types.
 26 |  */
 27 | template<typename Tp>
 28 | class PnPPoseAdapter : public PoseAdapterBase<Tp>
 29 | {
 30 | protected:
 31 | 	using PoseAdapterBase<Tp>::_t_w;
 32 | 	using PoseAdapterBase<Tp>::_R_cw;
 33 | 
 34 | public:
 35 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 36 |   typedef typename PoseAdapterBase<Tp>::Vector3 Vector3;
 37 |   typedef typename PoseAdapterBase<Tp>::SO3_T SO3_T;
 38 |   typedef typename PoseAdapterBase<Tp>::Point3 Point3;
 39 | 
 40 |   typedef Matrix<Tp, Dynamic, Dynamic> MatrixX;
 41 | 
 42 |   /**
 43 |    * \brief Constructor. See protected class-members to understand parameters
 44 |    */
 45 |   PnPPoseAdapter(
 46 |       const MatrixX & bearingVectors,
 47 | 	  const MatrixX & points );
 48 |   /**
 49 |    * \brief Constructor. See protected class-members to understand parameters
 50 |    */
 51 |   PnPPoseAdapter(
 52 | 	  const MatrixX & bearingVectors,
 53 | 	  const MatrixX & points,
 54 |       const SO3_T & R );
 55 |   /**
 56 |    * \brief Constructor. See protected class-members to understand parameters
 57 |    */
 58 |   PnPPoseAdapter(
 59 | 	  const MatrixX & bearingVectors,
 60 | 	  const MatrixX & points,
 61 |       const Vector3 & t,
 62 |       const SO3_T & R );
 63 |   /**
 64 |    * Destructor
 65 |    */
 66 |   virtual ~PnPPoseAdapter(){};
 67 | 
 68 |   //Access of correspondences
 69 |   
 70 |   /** See parent-class */
 71 |   virtual Point3 getBearingVector( int index ) const;
 72 |   /** See parent-class */
 73 |   virtual Tp getWeight( int index ) const;
 74 |   /** See parent-class */
 75 |   virtual Point3 getPointGlob( int index ) const;
 76 |   /** See parent-class */
 77 |   virtual int getNumberCorrespondences() const;
 78 | 
 79 |   virtual void setInlier(const Matrix<short, Dynamic, Dynamic>& inliers);
 80 |   virtual void setWeights(const Matrix<Tp, Dynamic, Dynamic>& weights);
 81 |   virtual void printInlier() const;
 82 |   const vector<short>& getInlierIdx() const { return _vInliersPnP;}
 83 |   void cvtInlier();
 84 | 
 85 |   Tp getError(int index) const;
 86 | 
 87 |   void setMaxVotes(int votes){  _max_votes = votes;  };
 88 | 
 89 |   int getMaxVotes() { return _max_votes; };
 90 | 
 91 |   bool isInlier23(int index) const;
 92 |   Tp weight23(int index) const;
 93 |   void sortIdx();
 94 |   void getSortedIdx(vector<int>& select_) const;
 95 | protected:
 96 | 	/** Reference to the bearing-vectors expressed in the camera-frame */
 97 | 	const MatrixX & _bearingVectors; //normalized 2d homogeneous coordinate
 98 | 	/** Reference to the points expressed in the world/global-frame. */
 99 | 	const MatrixX & _points_g;
100 | 	/** flags of inliers. */
101 | 	Matrix<short, Dynamic, 1> _inliers;
102 | 	Matrix<Tp, Dynamic, 1> _weights;
103 |     vector<int> _idx; //stores the sorted index 
104 | 	vector<short> _vInliersPnP;
105 | 	/** max votes. */
106 | 	int _max_votes;
107 | };
108 | 
109 | 
110 | template <typename Tp>
111 | PnPPoseAdapter<Tp>::PnPPoseAdapter(
112 | 	const MatrixX & bearingVectors,
113 | 	const MatrixX & points) :
114 | 	PoseAdapterBase<Tp>(),
115 | 	_bearingVectors(bearingVectors),
116 | 	_points_g(points)
117 | {
118 | 	_inliers.resize(_bearingVectors.cols());
119 | 	_inliers.setOnes();
120 | }
121 | 
122 | template <typename Tp>
123 | PnPPoseAdapter<Tp>::PnPPoseAdapter(
124 | 	const MatrixX & bearingVectors,
125 | 	const MatrixX & points,
126 | 	const SO3_T & R) :
127 | 	PoseAdapterBase<Tp>(R),
128 | 	_bearingVectors(bearingVectors),
129 | 	_points_g(points)
130 | {
131 | 	_inliers.resize(_bearingVectors.cols());
132 | 	_inliers.setOnes();
133 | }
134 | 
135 | template <typename Tp>
136 | PnPPoseAdapter<Tp>::PnPPoseAdapter(
137 | 	const MatrixX & bearingVectors,
138 | 	const MatrixX & points,
139 | 	const Vector3 & t,
140 | 	const SO3_T & R) :
141 | 	PoseAdapterBase<Tp>(t, R),
142 | 	_bearingVectors(bearingVectors),
143 | 	_points_g(points)
144 | {
145 | 	_inliers.resize(_bearingVectors.cols());
146 | 	_inliers.setOnes();
147 | }
148 | 
149 | template <typename Tp>
150 | typename PnPPoseAdapter<Tp>::Point3 PnPPoseAdapter<Tp>::
151 | getBearingVector(int index) const
152 | {
153 | 	assert(index < _bearingVectors.cols());
154 | 	return _bearingVectors.col(index);
155 | }
156 | 
157 | 
158 | template <typename Tp>
159 | Tp PnPPoseAdapter<Tp>::
160 | getWeight(int index) const
161 | {
162 | 	return Tp(1.);
163 | }
164 | 
165 | template <typename Tp>
166 | typename PnPPoseAdapter<Tp>::Point3 PnPPoseAdapter<Tp>::
167 | getPointGlob(int index) const
168 | {
169 | 	assert(index < _bearingVectors.cols());
170 | 	return _points_g.col(index);
171 | }
172 | 
173 | template<typename Tp>
174 | bool PnPPoseAdapter<Tp>::isInlier23(int index) const
175 | {
176 | 	assert(index < _inliers.rows());
177 | 	return _inliers(index) == 1;
178 | }
179 | 
180 | template<typename Tp>
181 | Tp PnPPoseAdapter<Tp>::weight23(int index) const
182 | {
183 | 	if (!_weights.rows()) return Tp(1.0);
184 | 	else{
185 | 		assert(index < _weights.rows());
186 | 		return _weights(index);
187 | 	}
188 | }
189 | 
190 | template <typename Tp>
191 | int PnPPoseAdapter<Tp>::getNumberCorrespondences() const
192 | {
193 | 	return _bearingVectors.cols();
194 | }
195 | 
196 | template <typename Tp>
197 | void PnPPoseAdapter<Tp>::setInlier(const Matrix<short, Dynamic, Dynamic>& inliers)
198 | {
199 | 	assert(inliers.rows() == _inliers.rows());
200 | 	//_inliers = inliers.col(0);
201 | 	memcpy(_inliers.data(), inliers.data(), inliers.rows()*2);
202 | }
203 | 
204 | template <typename Tp>
205 | Tp PnPPoseAdapter<Tp>::getError(int index) const
206 | {
207 | 	Point3 Xc = _R_cw * getPointGlob(index) + _t_w;
208 | 	Xc.normalize();
209 | 	return Xc.cross(getBearingVector(index)).norm();
210 | }
211 | 
212 | template <typename Tp>
213 | void PnPPoseAdapter<Tp>::setWeights(const Matrix<Tp, Dynamic, Dynamic>& weights)
214 | {
215 | 	_weights = weights.col(0);
216 | 	//memcpy(_weights.data(), weights.data(), weights.rows() * 2);
217 | 
218 | 	return;
219 | }
220 | 
221 | template <typename Tp>
222 | void PnPPoseAdapter<Tp>::printInlier() const
223 | {
224 | 	cout << _inliers.transpose() << endl;
225 | }
226 | 
227 | template <typename Tp>
228 | void PnPPoseAdapter<Tp>::cvtInlier()
229 | {
230 | 	_vInliersPnP.clear();
231 | 	_vInliersPnP.reserve(getNumberCorrespondences());
232 | 	for (short r = 0; r < (short)_inliers.rows(); r++) {
233 | 		if (1 == _inliers[r]){
234 | 			_vInliersPnP.push_back(r);
235 | 		}
236 | 	}
237 | }
238 | 
239 | template <typename Tp>
240 | void PnPPoseAdapter<Tp>::sortIdx(){
241 |     //sort the index according to weights 
242 |     vector<Tp> weigh(_weights.data(), _weights.data() + _weights.rows() * _weights.cols());
243 |     _idx = sortIndexes<Tp>( weigh );
244 | }
245 | 
246 | template <typename Tp>
247 | void PnPPoseAdapter<Tp>::getSortedIdx(vector<int>& select_) const{
248 |     //sort the index according to weights 
249 |     for (int i = 0; i < (int)select_.size(); ++i)
250 |     {
251 |     	int j = select_[i];
252 |     	if(j < (int)_idx.size())
253 |     		select_[i] = _idx[j];
254 |     }
255 | }
256 | 
257 | #endif 
258 | 


--------------------------------------------------------------------------------
/pose/PoseAdapterBase.hpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * \file based on Kneip's opengv AbsoluteAdapterBase.hpp
  3 |  * \brief Adapter-class for passing bearing-vector-to-point correspondences to
  4 |  *        the absolute-pose algorithms.
  5 |  */
  6 | 
  7 | #ifndef _POSE_ADAPTERBASE_HEADER_
  8 | #define _POSE_ADAPTERBASE_HEADER_
  9 | 
 10 | #include <stdlib.h>
 11 | #include <vector>
 12 | #include <Eigen/Eigen>
 13 | #include <Eigen/src/Core/util/DisableStupidWarnings.h>
 14 | #include <se3.hpp>
 15 | 
 16 | using namespace Eigen;
 17 | /**
 18 |  * \brief The namespace for the absolute pose methods.
 19 |  */
 20 | 
 21 | /**
 22 |  * The PoseAdapterBase is the base-class for the visitors to absolute pose 
 23 |  * algorithms. It provides a unified interface to opengv-methods to access 
 24 |  * bearing-vectors, world points, priors or known variables for the absolute 
 25 |  * pose, Derived classes may hold the data in any user-specific
 26 |  * format, and adapt to opengv-types.
 27 |  */
 28 | template<typename Tp>
 29 | class PoseAdapterBase
 30 | {
 31 | public:
 32 | 	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 33 | 	typedef Matrix<Tp, 3, 1> Vector3;
 34 |   typedef Sophus::SO3<Tp> SO3_T;
 35 | 	typedef Sophus::SE3<Tp> SE3_T;
 36 | 
 37 | 	typedef Matrix<Tp, 3, 1> Point3;
 38 | 
 39 | 	/**
 40 |    * \brief Constructor.
 41 |    */
 42 |   PoseAdapterBase() :
 43 |       _t_w(Vector3::Zero()),
 44 | 	  _cx(0), _cy(0){};
 45 |   /**
 46 |    * \brief Constructor.
 47 |    * \param[in] R A prior or known value for the rotation from the viewpoint
 48 |    *              to the world frame.
 49 |    */
 50 |   PoseAdapterBase( const SO3_T& R ) :
 51 |       _t_w(Vector3::Zero()),
 52 |       _R_cw(R),
 53 | 	  _cx(0), _cy(0){};
 54 | 
 55 |   /**
 56 |    * \brief Constructor.
 57 |    * \param[in] t A prior or known value for the position of the viewpoint seen
 58 |    *              from the world frame.
 59 |    * \param[in] R A prior or known value for the rotation from the viewpoint
 60 |    *              to the world frame.
 61 |    */
 62 |   PoseAdapterBase(
 63 |       const Vector3 & t,
 64 |       const SO3_T & R ) :
 65 |       _t_w(t),
 66 |       _R_cw(R),
 67 | 	  _cx(0), _cy(0){};
 68 | 
 69 |   /**
 70 |    * \brief Destructor.
 71 |    */
 72 |   virtual ~PoseAdapterBase() {};
 73 | 
 74 |   //Access of correspondences
 75 |   
 76 |   /**
 77 |    * \brief Retrieve the bearing vector of a correspondence.
 78 |    * \param[in] index The serialized index of the correspondence.
 79 |    * \return The corresponding bearing vector.
 80 |    */
 81 |   virtual Point3 getBearingVector(int index) const = 0;
 82 |   /**
 83 |    * \brief Retrieve the weight of a correspondence. The weight is supposed to
 84 |    *        reflect the quality of a correspondence, and typically is between
 85 |    *        0 and 1.
 86 |    * \param[in] index The serialized index of the correspondence.
 87 |    * \return The corresponding weight.
 88 |    */
 89 |   virtual Tp getWeight( int index ) const = 0;
 90 | 
 91 |   /**
 92 |    * \brief Retrieve the world point of a correspondence.
 93 |    * \param[in] index The serialized index of the correspondence.
 94 |    * \return The corresponding world point.
 95 |    */
 96 |   virtual Point3 getPointGlob(int index) const = 0;
 97 |   /**
 98 |    * \brief Retrieve the number of correspondences.
 99 |    * \return The number of correspondences.
100 |    */
101 |   virtual int getNumberCorrespondences() const = 0;
102 | 
103 |   //Access of priors or known values
104 |   
105 |   /**
106 |    * \brief Retrieve the prior or known value for the position.
107 |    * \return The prior or known value for the position.
108 |    */
109 |   Vector3 gettw() const { return _t_w; };
110 |   /**
111 |    * \brief Set the prior or known value for the position.
112 |    * \param[in] t The prior or known value for the position.
113 |    */
114 |   void sett(const Vector3 & t) { _t_w = t; };
115 |   /**
116 |    * \brief Retrieve the prior or known value for the rotation.
117 |    * \return The prior or known value for the rotation.
118 |    */
119 |   SO3_T getRcw() const { return _R_cw; };
120 |   /**
121 |    * \brief Set the prior or known value for the rotation.
122 |    * \param[in] R The prior or known value for the rotation.
123 |    */
124 |   void setRcw(const SO3_T& R) { _R_cw = R; };
125 | 
126 |   void setFocal(const Tp fx, const Tp fy) { _fx = fx; _fy = fy; };
127 | 
128 |   Tp getFocal() const { return (_fx + _fy) /2; };
129 | 
130 |   SE3_T getTcw(){ return SE3_T(_R_cw, _t_w); }
131 | 
132 | 
133 | protected:
134 |   /** The prior or known value for the translation from the world coordinate to the
135 |    * camera coordinate system. Initialized to zero if not provided.
136 |    */
137 | 	Vector3 _t_w;
138 |   /** The prior or known value for the rotation from the WORLD coordinate system to the
139 |    * camera coordinate system . Initialized to identity if not provided.
140 |    */
141 | 	SO3_T _R_cw;
142 | 	/** The known camera internal parameters: focal length and principle points
143 | 	*/
144 | 	Tp _fx, _fy, _cx, _cy;
145 | 
146 | };
147 | 
148 | 
149 | #endif 
150 | 


--------------------------------------------------------------------------------
/pose/Simulator.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef _RAND_GENERATOR_HEADER_
  2 | #define _RAND_GENERATOR_HEADER_
  3 | 
  4 | #include <se3.hpp>
  5 | #include <time.h>
  6 | #include <limits>
  7 | #include <random>
  8 | #include "Utility.hpp"
  9 | 
 10 | using namespace Eigen;
 11 | using namespace std;
 12 | 
 13 | std::default_random_engine generator;
 14 | std::normal_distribution<double> distribution(0., 1.0);
 15 | 
 16 | template< typename T >
 17 | Matrix<T, 3, 1> generate_random_translation_uniform(T size)
 18 | {
 19 | 	Matrix<T, 3, 1>  translation = Matrix< T, Dynamic, Dynamic>::Random(3, 1);
 20 | 	return size * translation;
 21 | }
 22 | 
 23 | template< typename T >
 24 | Sophus::SO3< T > generate_random_rotation(T max_angle_radian_, bool use_guassian_ = true)
 25 | {
 26 | 	Matrix<T, 3, 1> rv;
 27 | 	if (use_guassian_){
 28 | 		rv[0] = distribution(generator); //standard normal distribution
 29 | 		rv[1] = distribution(generator);
 30 | 		rv[2] = distribution(generator);
 31 | 	}
 32 | 	else {
 33 | 		rv = Matrix< T, Dynamic, Dynamic>::Random(3, 1); //uniform -1. to  1.
 34 | 	}
 35 | 
 36 | 	rv[0] = max_angle_radian_*rv[0]; //angle rotation around x
 37 | 	rv[1] = max_angle_radian_*rv[1] * T(.5); // y axis
 38 | 	rv[2] = max_angle_radian_*rv[2]; // z axis
 39 | 
 40 | 	T m_pi_2 = T(M_PI / 2.);
 41 | 	rv[0] = rv[0] > M_PI ? M_PI : rv[0];
 42 | 	rv[0] = rv[0] < -M_PI ? -M_PI : rv[0];
 43 | 	rv[1] = rv[1] > m_pi_2 ? m_pi_2 : rv[1];
 44 | 	rv[1] = rv[1] < -m_pi_2 ? -m_pi_2 : rv[1];
 45 | 	rv[2] = rv[2] > M_PI ? M_PI : rv[2];
 46 | 	rv[2] = rv[2] < -M_PI ? -M_PI : rv[2];
 47 | 
 48 | 	Matrix<T, 3, 3>  R1;
 49 | 	R1(0, 0) = 1.0;
 50 | 	R1(0, 1) = 0.0;
 51 | 	R1(0, 2) = 0.0;
 52 | 	R1(1, 0) = 0.0;
 53 | 	R1(1, 1) = cos(rv[0]);
 54 | 	R1(1, 2) = -sin(rv[0]);
 55 | 	R1(2, 0) = 0.0;
 56 | 	R1(2, 1) = -R1(1, 2); //sin(rpy[0]);
 57 | 	R1(2, 2) = R1(1, 1); //cos(rpy[0])
 58 | 
 59 | 	Matrix<T, 3, 3>  R2;
 60 | 	R2(0, 0) = cos(rv[1]);
 61 | 	R2(0, 1) = 0.0;
 62 | 	R2(0, 2) = sin(rv[1]);
 63 | 	R2(1, 0) = 0.0;
 64 | 	R2(1, 1) = 1.0;
 65 | 	R2(1, 2) = 0.0;
 66 | 	R2(2, 0) = -R2(0, 2);
 67 | 	R2(2, 1) = 0.0;
 68 | 	R2(2, 2) = R2(0, 0);
 69 | 
 70 | 	Matrix<T, 3, 3>  R3;
 71 | 	R3(0, 0) = cos(rv[2]);
 72 | 	R3(0, 1) = -sin(rv[2]);
 73 | 	R3(0, 2) = 0.0;
 74 | 	R3(1, 0) = -R3(0, 1);
 75 | 	R3(1, 1) = R3(0, 0);
 76 | 	R3(1, 2) = 0.0;
 77 | 	R3(2, 0) = 0.0;
 78 | 	R3(2, 1) = 0.0;
 79 | 	R3(2, 2) = 1.0;
 80 | 
 81 | 	Sophus::SO3< T > rotation(R3 * R2 * R1);
 82 | 	return rotation;
 83 | }
 84 | 
 85 | template< typename T >
 86 | void simulate_nl_nl_correspondences(const Sophus::SO3<T>& R_cw_, int number_, T noise_nl_, T outlier_ratio_nl_, bool use_guassian_,
 87 | 	Matrix<T, Dynamic, Dynamic>* pM_, Matrix<T, Dynamic, Dynamic>* pN_, Matrix<T, Dynamic, Dynamic>* pN_gt = NULL,
 88 | 	Matrix<T, Dynamic, Dynamic>* p_all_weights_ = NULL)
 89 | {
 90 | 	typedef Matrix<T, 3, 1> Point3;
 91 | 	typedef Matrix<T, Dynamic, Dynamic> MX;
 92 | 
 93 | 	MX w(number_, 1); //dynamic weights for N-N correspondences
 94 | 	pM_->resize(3, number_); //normal in WRS
 95 | 	pN_->resize(3, number_); //normal in CRS
 96 | 	MX N_gt(3, number_); //ground truth normal in CRS
 97 | 	for (int i = 0; i < number_; i++)	{
 98 | 		do {
 99 | 			N_gt.col(i) = generate_random_rotation<T>( T(M_PI / 2.), false) * Point3(0, 0, -1); //have to be uniform distribution here
100 | 			N_gt.col(i).normalize();
101 | 			pM_->col(i) = R_cw_.inverse() * N_gt.col(i); //transform to world reference system
102 | 			pM_->col(i).normalize(); 
103 | 			//add noise
104 | 			pN_->col(i) = generate_random_rotation<T>(noise_nl_, use_guassian_) * N_gt.col(i);
105 | 			pN_->col(i).normalize();
106 | 		} while (acos(pN_->col(i)(2)) < T(M_PI / 2)); //ensure that the normal is facing the camera
107 | 		//simulate weights
108 | 		w(i) = pN_->col(i).dot(N_gt.col(i));
109 | 	}
110 | 	//add outliers
111 | 	int out = int(outlier_ratio_nl_*number_ + T(.5));
112 | 	RandomElements<int> re(number_);
113 | 	vector<int> vIdx;	re.run(out, &vIdx);
114 | 	for (int i = 0; i < out; i++){
115 | 		Matrix< T, 3, 1> nl_g;
116 | 		do {
117 | 			pN_->col(i) = generate_random_rotation<T>(M_PI / 2, false) * Point3(0, 0, -1);//have to be uniform distribution here
118 | 			pN_->col(i).normalize();
119 | 		} while (acos(pN_->col(i)(2)) < M_PI / 2);
120 | 	}
121 | 
122 | 	if (pN_gt){
123 | 		*pN_gt = N_gt; //note that p_nl_c_gt_ is not polluted by noise or outliers
124 | 	}
125 | 	if (p_all_weights_){
126 | 		assert(p_all_weights_->rows() == number_ && p_all_weights_->cols() == 3);
127 | 		p_all_weights_->col(2) = w;
128 | 	}
129 | 	return;
130 | }
131 | 
132 | //generate random 3-D point within a viewing frutum defined by
133 | //T min_depth_, T max_depth_, 
134 | //T tan_fov_x, T tan_fov_y
135 | template< typename T >
136 | Matrix<T, 3, 1> generate_a_random_point(T min_depth_, T max_depth_, T tan_fov_x, T tan_fov_y)
137 | {
138 | 	T x_range = tan_fov_x * max_depth_;
139 | 	T y_range = tan_fov_y * max_depth_;
140 | 	Matrix<T, 3, 1>  cleanPoint = Matrix<T, 3, 1>::Random(3, 1);
141 | 	cleanPoint[0] *= x_range; //x
142 | 	cleanPoint[1] *= y_range; //y
143 | 	cleanPoint[2] = (cleanPoint[2] + 1.) / 2. *(max_depth_ - min_depth_) + min_depth_; //z
144 | 	return cleanPoint;
145 | }
146 | 
147 | //project a set of 3-D points in camera coordinate system to 2-D points
148 | template< typename T >
149 | Matrix<T, Dynamic, Dynamic> project_point_cloud(const Matrix<T, Dynamic, Dynamic>& pt_c, T f_){
150 | 	Matrix<T, Dynamic, Dynamic> pt_2d(2, pt_c.cols());
151 | 	for (int i = 0; i < pt_c.cols(); i++) {
152 | 		pt_2d.col(i)[0] = f_ * pt_c.col(i)[0] / pt_c.col(i)[2];
153 | 		pt_2d.col(i)[1] = f_ * pt_c.col(i)[1] / pt_c.col(i)[2];
154 | 	}
155 | 	return pt_2d;
156 | }
157 | 
158 | template< typename T >
159 | Matrix<T, Dynamic, Dynamic> simulate_rand_point_cloud_in_frustum(int number_, T f_, T min_depth_, T max_depth_)
160 | {
161 | 	T tan_fov_x = T(320. / f_); //the frame resolution is 640x480 with principle point at the centre of the frame
162 | 	T tan_fov_y = T(240. / f_);
163 | 	Matrix<T, Dynamic, Dynamic> all_P(3, number_);
164 | 	for (int i = 0; i < (int)number_; i++){
165 | 		bool is_outside_frustum = true;
166 | 		while (is_outside_frustum){
167 | 			Matrix<T, 3, 1 > P = generate_a_random_point<T>(min_depth_, max_depth_, tan_fov_x, tan_fov_y); // generate random point in CRS
168 | 			all_P.col(i) = P;
169 | 			is_outside_frustum = !(fabs(P[0] / P[2]) < tan_fov_x && fabs(P[1] / P[2]) < tan_fov_y); //check whether the point is inside the frustum
170 | 		}
171 | 	}
172 | 	return all_P;
173 | }
174 | 
175 | template< typename T >
176 | void simulate_2d_3d_correspondences(const Sophus::SO3<T>& R_cw_, const Matrix<T, 3, 1>& t_w_,
177 | 	int number_, T noise_, T outlier_ratio_, T min_depth_, T max_depth_, T f_, bool use_guassian_,
178 | 	Matrix<T, Dynamic, Dynamic>* pQ_, // *pQ_: points with noise in 3-D world system 
179 | 	Matrix<T, Dynamic, Dynamic>* pU_, // *pU_: 2-D points 
180 | 	Matrix<T, Dynamic, Dynamic>* pP_gt = NULL, // *pP_gt: ground truth of 3-D points in camera system 
181 | 	Matrix<T, Dynamic, Dynamic>* p_all_weights_ = NULL)
182 | {
183 | 	typedef Matrix<T, Dynamic, Dynamic> MX;
184 | 
185 | 	MX w(number_, 1); //dynamic weights for 2-3 correspondences
186 | 	//1. generate 3-D points P in CRS
187 | 	MX P_gt = simulate_rand_point_cloud_in_frustum<T>(number_, f_, min_depth_, max_depth_); //pt cloud in camera system
188 | 	//2. project to 2-D 
189 | 	MX kp_2d = project_point_cloud<T>(P_gt, f_);
190 | 	//3. transform from world to camera coordinate system
191 | 	pQ_->resize(3, number_);
192 | 	for (int i = 0; i < number_; i++) {
193 | 		pQ_->col(i) = R_cw_.inverse() * (P_gt.col(i) - t_w_);
194 | 	}
195 | 	//4. add 2-D noise
196 | 	for (int i = 0; i < number_; i++) {
197 | 		Matrix<T, 2, 1> rv; //random variable
198 | 		if (use_guassian_)
199 | 			rv = Matrix<T, 2, 1>(distribution(generator), distribution(generator));
200 | 		else
201 | 			rv = MX::Random(2, 1);
202 | 		w(i) = T(1.) / rv.norm();
203 | 		kp_2d.col(i) = kp_2d.col(i) + noise_ * rv;
204 | 	}
205 | 
206 | 	//5. add 2-D outliers
207 | 	int out = int(outlier_ratio_*number_ + .5);
208 | 	MX out_points = simulate_rand_point_cloud_in_frustum<T>(out, f_, min_depth_, max_depth_); //outliers remain in CRS
209 | 	MX out_2d_points = project_point_cloud<T>(out_points, f_);
210 | 	RandomElements<int> re(number_);
211 | 	vector<int> vIdx;	re.run(out, &vIdx);
212 | 	for (int i = 0; i < out; i++){
213 | 		kp_2d.col(vIdx[i]) = out_2d_points.col(i);
214 | 	}
215 | 	//6. convert to 2-D key points into unit vectors 
216 | 	pU_->resize(3, number_); 
217 | 	pU_->row(0) = kp_2d.row(0);
218 | 	pU_->row(1) = kp_2d.row(1);
219 | 	pU_->row(2) = f_ * MX::Ones(1,number_);
220 | 
221 | 	for (int c = 0; c < number_; c++) {
222 | 		pU_->col(c).normalize();
223 | 	}
224 | 	if (pP_gt){
225 | 		*pP_gt = P_gt; //note that pt_c was not polluted by outliers
226 | 	}
227 | 
228 | 	if (p_all_weights_){
229 | 		assert(p_all_weights_->rows() == number_ && p_all_weights_->cols() == 3);
230 | 		p_all_weights_->col(0) = w;
231 | 	}
232 | 	return;
233 | }
234 | 
235 | template< typename T >
236 | void simulate_2d_3d_3d_correspondences(const Sophus::SO3<T>& R_cw_, const Matrix<T, 3, 1>& t_w_,
237 | 	int number_, T noise_2d_, T noise_3d_, T outlier_ratio_, T min_depth_, T max_depth_, T f_, bool use_guassian_,
238 | 	Matrix<T, Dynamic, Dynamic>* pQ_, // *pQ_: points with noise in 3-D world system 
239 | 	Matrix<T, Dynamic, Dynamic>* pU_, // *pU_: 2-D points 
240 | 	Matrix<T, Dynamic, Dynamic>* pP_gt = NULL, // *pP_gt: ground truth of 3-D points in camera system 
241 | 	Matrix<T, Dynamic, Dynamic>* p_all_weights_ = NULL)
242 | {
243 | 	typedef Matrix<T, Dynamic, Dynamic> MX;
244 | 	simulate_2d_3d_correspondences<T>(R_cw_, t_w_, number_, noise_2d_, outlier_ratio_, min_depth_, max_depth_, f_, use_guassian_,
245 |                 pQ_, pU_, pP_gt, p_all_weights_);
246 | 
247 | 	MX w(number_, 1); //dynamic weights for 2-3 correspondences
248 | 	//1. generate 3-D points P in CRS
249 | 	//4. add 3-D noise
250 | 	for (int i = 0; i < number_; i++) {
251 | 		Matrix<T, 3, 1> rv; //random variable
252 | 		if (use_guassian_)
253 | 			rv = Matrix<T, 3, 1>(distribution(generator), distribution(generator), distribution(generator));
254 | 		else
255 | 			rv = MX::Random(3, 1);
256 | 		w(i) = T(1.) / rv.norm();
257 | 		pQ_->col(i) = pQ_->col(i) + noise_3d_*rv;
258 | 	}
259 | 
260 | 	if (p_all_weights_){
261 | 		assert(p_all_weights_->rows() == number_ && p_all_weights_->cols() == 3);
262 | 		p_all_weights_->col(1) = w;
263 | 	}
264 | 	return;
265 | }
266 | 
267 | 
268 | template< typename T >
269 | void simulate_3d_3d_correspondences(const Sophus::SO3<T>& R_cw_, const Matrix<T, 3, 1>& t_w_,
270 | 	int number_, T noise_, T outlier_ratio_, T min_depth_, T max_depth_, T f_, bool use_guassian_,
271 | 	Matrix<T, Dynamic, Dynamic>* pQ_, // *pQ_: points with noise in 3-D world system 
272 | 	Matrix<T, Dynamic, Dynamic>* pP_gt = NULL, // *pP_gt: ground truth of 3-D points in camera system 
273 | 	Matrix<T, Dynamic, Dynamic>* p_all_weights_ = NULL)
274 | {
275 | 	typedef Matrix<T, Dynamic, Dynamic> MX;
276 | 
277 | 	MX w(number_, 1); //dynamic weights for 2-3 correspondences
278 | 	//1. generate 3-D points P in CRS
279 | 	MX P_gt = simulate_rand_point_cloud_in_frustum<T>(number_, f_, min_depth_, max_depth_); //pt cloud in camera system
280 | 	//3. transform from camera to world coordinate system
281 | 	pQ_->resize(3, number_);
282 | 	for (int i = 0; i < number_; i++) {
283 | 		pQ_->col(i) = R_cw_.inverse() * (P_gt.col(i) - t_w_);
284 | 	}
285 | 	//4. add 3-D noise
286 | 	for (int i = 0; i < number_; i++) {
287 | 		Matrix<T, 3, 1> rv; //random variable
288 | 		if (use_guassian_)
289 | 			rv = Matrix<T, 3, 1>(distribution(generator), distribution(generator), distribution(generator));
290 | 		else
291 | 			rv = MX::Random(3, 1);
292 | 		w(i) = T(1.) / rv.norm();
293 | 		pQ_->col(i) = pQ_->col(i) + noise_*rv;
294 | 	}
295 | 
296 | 	//5. add 2-D outliers
297 | 	int out = int(outlier_ratio_*number_ + .5);
298 | 	MX out_points = simulate_rand_point_cloud_in_frustum<T>(out, f_, min_depth_, max_depth_); //outliers remain in CRS
299 | 	RandomElements<int> re(number_);
300 | 	vector<int> vIdx;	re.run(out, &vIdx);
301 | 	for (int i = 0; i < out; i++){
302 | 		pQ_->col(vIdx[i]) = out_points.col(i);
303 | 	}
304 | 	
305 | 	if (pP_gt){
306 | 		*pP_gt = P_gt; //note that pt_c was not polluted by outliers
307 | 	}
308 | 
309 | 	if (p_all_weights_){
310 | 		assert(p_all_weights_->rows() == number_ && p_all_weights_->cols() == 3);
311 | 		p_all_weights_->col(1) = w;
312 | 	}
313 | 	return;
314 | }
315 | 
316 | template< typename T >
317 | void simulate_2d_3d_nl_correspondences(const Sophus::SO3<T>& R_cw_, const Matrix<T, 3, 1>& t_w_, //rotation and translation in world reference system
318 | 	int number_,  // total number of correspondences
319 | 	T n2D_, T or_2D_, //noise level and outlier ratio for 2-3 correspondences
320 | 	T n3D_, T or_3D_, //noise level and outlier ratio for 3-3 correspondences
321 | 	T nNl_, T or_Nl_, //noise level and outlier ratio for N-N correspondences
322 | 	T min_depth_, T max_depth_, T f_, bool use_guassian_,
323 | 	//outputs
324 | 	Matrix<T, Dynamic, Dynamic>* pQ_, Matrix<T, Dynamic, Dynamic>* pM_, //Q and M are 3-D points and normal in world 
325 | 	Matrix<T, Dynamic, Dynamic>* pP_, Matrix<T, Dynamic, Dynamic>* pN_, //P and N are 3-D points and normal in camera system
326 | 	Matrix<T, Dynamic, Dynamic>* pU_,//U are the unit vectors pointing from camera centre to 2-D key points
327 | 	Matrix<T, Dynamic, Dynamic>* p_all_weights_ = NULL) //store all weights for 2-3, 3-3 and N-N correspondences
328 | {
329 | 	Matrix<T, Dynamic, Dynamic> all_weights(number_, 3);
330 | 	//generate 2-D to 3-D pairs
331 | 	Matrix<T, Dynamic, Dynamic> P_gt; //ground truth 3-D points in camera reference system
332 | 	simulate_2d_3d_correspondences<T>(R_cw_, t_w_, number_, n2D_, or_2D_, min_depth_, max_depth_, f_, use_guassian_,
333 | 		&*pQ_, &*pU_, &P_gt, &all_weights);
334 | 
335 | 	//generate normal to normal pairs
336 | 	Matrix<T, Dynamic, Dynamic> nl_c_gt;
337 | 	simulate_nl_nl_correspondences<T>(R_cw_, number_, nNl_, or_Nl_, true, &*pM_, &*pN_, &nl_c_gt, &all_weights);
338 | 
339 | 	//add noise to 3-D points in camera reference frame
340 | 	pP_->resize(3, number_);
341 | 	for (int i = 0; i < number_; i++) {
342 | 		Matrix<T, 3, 1> rv; //random variable
343 | 		if (use_guassian_) //use Guassian noise
344 | 			rv = Matrix<T, 3, 1>(distribution(generator), distribution(generator), distribution(generator));
345 | 		else
346 | 			rv = Matrix<T, Dynamic, Dynamic>::Random(3, 1);
347 | 		//all_weights(i, 1) = short(rv.norm() / 1.414 * numeric_limits<short>::max()); //simulate weights
348 | 		all_weights(i, 1) = T(1.) / rv.norm();
349 | 		pP_->col(i) = P_gt.col(i) + n3D_ * rv;
350 | 	}
351 | 
352 | 	//add 3-D outliers
353 | 	int out = int(or_3D_*number_ + .5);
354 | 	RandomElements<int> re(number_);
355 | 	vector<int> vIdx;	re.run(out, &vIdx);
356 | 	Matrix<T, Dynamic, Dynamic> pt_c_out = simulate_rand_point_cloud_in_frustum(out, f_, min_depth_, max_depth_);
357 | 	for (int i = 0; i < out; i++){
358 | 		pP_->col(vIdx[i]) = pt_c_out.col(i);
359 | 	}
360 | 
361 | 	if (p_all_weights_){
362 | 		assert(p_all_weights_->rows() == number_ && p_all_weights_->cols() == 3);
363 | 		*p_all_weights_ = all_weights;
364 | 	}
365 | 
366 | 	return;
367 | }
368 | 
369 | template< typename T >
370 | T lateral_noise_kinect(T theta_, T z_, T f_)
371 | {
372 | 	//Nguyen, C.V., Izadi, S., &Lovell, D. (2012).Modeling kinect sensor noise for improved 3D reconstruction and tracking.In 3DIM / 3DPVT (pp. 524?30).http://doi.org/10.1109/3DIMPVT.2012.84
373 | 	T sigma_l;
374 | 	sigma_l = T(.8) + T(.035)*theta_ / (T(M_PI / 2.) - theta_);
375 | 	sigma_l = sigma_l * z_ / f_;
376 | 	return sigma_l;
377 | }
378 | 
379 | template< typename T >
380 | T axial_noise_kinect(T theta_, T z_){
381 | 	T sigma_a;
382 | 	if (fabs(theta_) <= T(M_PI / 3.))
383 | 		sigma_a = T(.0012) + T(.0019)*(z_ - 0.4)*(z_ - 0.4);
384 | 	else
385 | 		sigma_a = T(.0012) + T(.0019)*(z_ - 0.4)*(z_ - 0.4) + T(.0001) * theta_* theta_ / sqrt(z_) / (M_PI / 2 - theta_) / (M_PI / 2 - theta_);
386 | 	return sigma_a;
387 | }
388 | 
389 | template< typename T >
390 | void simulate_kinect_2d_3d_nl_correspondences(const Sophus::SO3<T>& R_cw_, const Matrix<T, 3, 1>& t_w_, int number_, 
391 | 	T noise_2d_, T outlier_ratio_2d_, T outlier_ratio_3d_, T noise_nl_, T outlier_ratio_nl_, T min_depth_, T max_depth_, T f_,
392 | 	Matrix<T, Dynamic, Dynamic>* p_pt_w_, 
393 | 	Matrix<T, Dynamic, Dynamic>* p_nl_w_,
394 | 	Matrix<T, Dynamic, Dynamic>* p_pt_c_, 
395 | 	Matrix<T, Dynamic, Dynamic>* p_nl_c_, 
396 | 	Matrix<T, Dynamic, Dynamic>* p_bv_,
397 | 	Matrix<T, Dynamic, Dynamic>* p_weights_ = NULL)
398 | {
399 | 	Matrix<T, Dynamic, Dynamic> all_weights(number_, 3); //simulated weights
400 | 	Matrix<T, Dynamic, Dynamic> pt_c_gt; //ground truth 3-D points in camera reference system
401 | 	simulate_2d_3d_correspondences<T>(R_cw_, t_w_, number_, noise_2d_, outlier_ratio_2d_, min_depth_, max_depth_, f_, true,
402 | 							&*p_pt_w_, &*p_bv_, &pt_c_gt, &all_weights);
403 | 
404 | 	//generate normal to normal pairs
405 | 	Matrix<T, Dynamic, Dynamic> nl_c_gt;
406 | 	simulate_nl_nl_correspondences<T>(R_cw_, number_, noise_nl_, outlier_ratio_nl_, true, &*p_nl_w_, &*p_nl_c_, &nl_c_gt, &all_weights);
407 | 
408 | 	T sigma_min = axial_noise_kinect<T>(T(.0), min_depth_);
409 | 
410 | 	//add Gaussian noise to 3-D points in camera reference frame
411 | 	p_pt_c_->resize(3, number_);
412 | 	for (int i = 0; i < number_; i++) {
413 | 		T theta = acos(nl_c_gt.col(i).dot(Matrix<T, 3, 1>(0, 0, -1)));
414 | 		T z = pt_c_gt.col(i)(2);
415 | 		T sigma_l = lateral_noise_kinect<T>(theta, z, f_);
416 | 		T sigma_a = axial_noise_kinect<T>(theta, z);
417 | 		Matrix<T, 3, 1> random_variable(sigma_l*distribution(generator), sigma_l*distribution(generator), sigma_a*distribution(generator));
418 | 		p_pt_c_->col(i) = pt_c_gt.col(i) + random_variable;
419 | 		all_weights(i, 1) = T(sigma_min / sigma_a);
420 | 	}
421 | 
422 | 	//add 3-D outliers
423 | 	int out = int(outlier_ratio_3d_*number_ + .5);
424 | 	RandomElements<int> re(number_);
425 | 	vector<int> vIdx;	re.run(out, &vIdx);
426 | 	Matrix<T, Dynamic, Dynamic> pt_c_out = simulate_rand_point_cloud_in_frustum(out, f_, min_depth_, max_depth_);
427 | 	for (int i = 0; i < out; i++){
428 | 		p_pt_c_->col(vIdx[i]) = pt_c_out.col(i);
429 | 	}
430 | 	if (p_weights_){
431 | 		assert(p_weights_->rows() == number_ && p_weights_->cols() == 3);
432 | 		*p_weights_ = all_weights;
433 | 	}
434 | 
435 | 	return;
436 | }
437 | 
438 | 
439 | #endif
440 | 


--------------------------------------------------------------------------------
/pose/Utility.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef BTL_OTHER_UTILITY_HELPER
  2 | #define BTL_OTHER_UTILITY_HELPER
  3 | 
  4 | //helpers based-on stl and boost
  5 | 
  6 | #include <iostream>
  7 | #include <fstream>
  8 | #include <vector>
  9 | #include <list>
 10 | #include <numeric>
 11 | #include <complex>
 12 | #include <string>
 13 | #include <chrono>
 14 | #include <memory>
 15 | 
 16 | 
 17 | #define SMALL 1e-50 // a small value
 18 | #define BTL_DOUBLE_MAX 10e20
 19 | // for print
 20 | template <class T>
 21 | std::ostream& operator << ( std::ostream& os, const std::vector< T > & v )
 22 | {
 23 |     os << "[";
 24 | 
 25 |     for ( typename std::vector< T >::const_iterator constItr = v.begin(); constItr != v.end(); ++constItr )
 26 |     {
 27 |         os << " " << ( *constItr ) << " ";
 28 |     }
 29 | 
 30 |     os << "]";
 31 |     return os;
 32 | }
 33 | 
 34 | template <class T>
 35 | std::ostream& operator << ( std::ostream& os, const std::list< T >& l_ )
 36 | {
 37 |     os << "[";
 38 |     for ( typename std::list< T >::const_iterator cit_List = l_.begin(); cit_List != l_.end(); cit_List++ )
 39 |     {
 40 |         os << " " << *cit_List << " ";
 41 |     }
 42 |     os << "]";
 43 |     return os;
 44 | }
 45 | 
 46 | template <typename T> int sgn(T val) {
 47 |     return (T(0) < val) - (val < T(0));
 48 | }
 49 | //calculate vector<> difference for testing
 50 | template< class T>
 51 | T matNormL1 ( const std::vector< T >& vMat1_, const std::vector< T >& vMat2_ )
 52 | {
 53 |     T tAccumDiff = 0;
 54 |     for(unsigned int i=0; i < vMat1_.size(); i++ )
 55 |     {
 56 |         T tDiff = vMat1_[i] - vMat2_[i];
 57 |         tDiff = tDiff >= 0? tDiff:-tDiff;
 58 |         tAccumDiff += tDiff;
 59 |     }
 60 |     return tAccumDiff;
 61 | }
 62 | 
 63 | template< class T >
 64 | void getNeighbourIdxCylinder(const unsigned short& usRows, const unsigned short& usCols, const T& i, std::vector< T >* pNeighbours_ )
 65 | {
 66 |     // get the neighbor 1d index in a cylindrical coordinate system
 67 |     int a = usRows*usCols;
 68 | 
 69 |     pNeighbours_->clear();
 70 |     pNeighbours_->push_back(i);
 71 |     T r = i/usCols;
 72 |     T c = i%usCols;
 73 |     T nL= c==0?        i-1 +usCols : i-1;    
 74 |     T nR= c==usCols-1? i+1 -usCols : i+1;
 75 |     pNeighbours_->push_back(nL);
 76 |     pNeighbours_->push_back(nR);
 77 | 
 78 |     if(r>0)//get up
 79 |     {
 80 |         T nU= i-usCols;
 81 |         pNeighbours_->push_back(nU);
 82 |         T nUL= nU%usCols == 0? nU-1 +usCols: nU-1;
 83 |         pNeighbours_->push_back(nUL);
 84 |         T nUR= nU%usCols == usCols-1? nU+1 -usCols : nU+1;
 85 |         pNeighbours_->push_back(nUR);
 86 |     }
 87 |     else if(r==usRows-1)//correspond to polar region
 88 |     {
 89 |         T t = r*usCols;
 90 |         for( T n=0; n<usCols; n++)
 91 |             pNeighbours_->push_back(t+n);
 92 |     }
 93 |     if(r<usRows-1)//get down
 94 |     {
 95 |         T nD= i+usCols;
 96 |         pNeighbours_->push_back(nD);
 97 |         T nDL= nD%usCols == 0? nD-1 +usCols: nD-1;
 98 |         pNeighbours_->push_back(nDL);
 99 |         T nDR= nD%usCols == usCols-1? nD+1 -usCols : nD+1;
100 |         pNeighbours_->push_back(nDR);
101 |     }
102 | 
103 |     return;
104 | }
105 | 
106 | 
107 | template <typename T>
108 | std::vector<int> sortIndexes(const std::vector<T> &v) {
109 | 
110 |   // initialize original index locations
111 |   std::vector<int> idx(v.size());
112 |   std::iota(idx.begin(), idx.end(), 0);
113 | 
114 |   // sort indexes based on comparing values in v
115 |   std::sort(idx.begin(), idx.end(), [&v](int i1, int i2) {return v[i1] > v[i2];});
116 | 
117 |   return idx;
118 | }
119 | 
120 | using namespace std;
121 | // places randomly selected  element at end of array
122 | // then shrinks array by 1 element. Done when only 1 element is left
123 | // m is the # to be selected from
124 | // n is the total # of elements
125 | template< class T >
126 | class  RandomElements
127 | {
128 |     T* _idx;
129 |     int _n;
130 | public:
131 |     RandomElements(int n):_n(n){
132 |         _idx = new T[n];
133 |     }
134 |     ~RandomElements(){
135 |         delete _idx;
136 |     }
137 | 
138 |     void run( int m, vector< T >* p_v_idx_ )
139 |     {
140 |         p_v_idx_->clear();
141 |         for (T i = 0; i < _n; i++) {
142 |             _idx[i] = i;
143 |         }
144 |         int temp = 0;
145 |         int ridx = _n-1;
146 |         for(int j=(_n-1); j>_n-m-1; j--)// one pass through array
147 |         {
148 |             ridx = rand()%(j+1);// index = 0 to j
149 |             temp = _idx[ridx];// value will be moved to end element
150 |             _idx[ridx] = _idx[j];// end element value in random spot
151 |             _idx[j] = temp;// selected element moved to end. This value is final
152 |             p_v_idx_->push_back(temp);
153 |         }
154 |         return;
155 |     }
156 | };//class RandElement
157 | 
158 | 
159 | // Prosac sampler used for PROSAC implemented according to "Matching with PROSAC
160 | // - Progressive Sampling Consensus" by Chum and Matas.
161 | template< class T >
162 | class ProsacSampler {
163 | public:
164 |     ProsacSampler( const int min_num_samples, const int num_datapoints )
165 |     : _m(min_num_samples) 
166 |     {
167 |         _N = num_datapoints;
168 |         _T_N = 20000;
169 |         _t = 1;
170 |     }
171 |     ~ProsacSampler() {}
172 | 
173 |     // Set the sample such that you are sampling the kth prosac sample (Eq. 6).
174 |     void setSampleNumber(int k)
175 |     {
176 |         _t = k;
177 |     }
178 | 
179 |     // Samples the input variable data and fills the vector subset with the prosac
180 |     // samples.
181 |     // NOTE: This assumes that data is in sorted order by quality where data[i] is
182 |     // of higher quality than data[j] for all i < j.
183 |     bool sample(std::vector<int>* subset_indices)
184 |     {
185 |         // Set t_n according to the PROSAC paper's recommendation.
186 |         T t_n = _T_N;
187 |         int n = this->_m;
188 |         // From Equations leading up to Eq 3 in Chum et al.
189 |         for (int i = 0; i < this->_m; i++) {
190 |             t_n *= static_cast<T >(n - i) / (_N - i);
191 |         }
192 | 
193 |         T t_n_prime = 1.0;
194 |         // Choose min n such that T_n_prime >= t (Eq. 5).
195 |         for (int t = 1; t <= _t; t++) {
196 |             if (t > t_n_prime && n < _N) {
197 |                 T t_n_plus1 = (t_n * (n + 1.0)) / (n + 1.0 - this->_m);
198 |                 t_n_prime += ceil(t_n_plus1 - t_n);
199 |                 t_n = t_n_plus1;
200 |                 n++;
201 |             }
202 |         }
203 |         subset_indices->reserve(this->_m);
204 |         if (t_n_prime < _t) {
205 |             // Randomly sample m data points from the top n data points.
206 |             std::vector<int> random_numbers;
207 |             for (int i = 0; i < this->_m; i++) {
208 |                 // Generate a random number that has not already been used.
209 |                 int rand_number;
210 |                 while (std::find(random_numbers.begin(),
211 |                                random_numbers.end(),
212 |                                (rand_number = rand()%(n) )) !=
213 |                      random_numbers.end()) {
214 |                 }
215 | 
216 |                 random_numbers.push_back(rand_number);
217 | 
218 |                 // Push the *unique* random index back.
219 |                 subset_indices->push_back(rand_number);
220 |             }
221 |         } else {
222 |             std::vector<int> random_numbers;
223 |             // Randomly sample m-1 data points from the top n-1 data points.
224 |             for (int i = 0; i < this->_m - 1; i++) {
225 |               // Generate a random number that has not already been used.
226 |               int rand_number;
227 |               while (std::find(random_numbers.begin(),
228 |                                random_numbers.end(),
229 |                                (rand_number = rand()%(n-1) )) !=
230 |                      random_numbers.end()) {
231 |               }
232 |               random_numbers.push_back(rand_number);
233 | 
234 |               // Push the *unique* random index back.
235 |               subset_indices->push_back(rand_number);
236 |             }
237 |             // Make the last point from the nth position.
238 |             subset_indices->push_back(n);
239 |         }
240 |         assert((int)subset_indices->size() == this->_m);
241 |         _t++;
242 |         return true;
243 |     }
244 | 
245 |     private:
246 |     int _N; // total number of data point
247 |     int _T_N; // Number of iterations of PROSAC before it just acts like ransac.
248 |     int _t; // The kth sample of prosac sampling.
249 |     int _m; //minum number of samples
250 | };
251 | 
252 | 
253 | #endif
254 | 


--------------------------------------------------------------------------------
/sophus/average.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef SOPHUS_AVERAGE_HPP
  2 | #define SOPHUS_AVERAGE_HPP
  3 | 
  4 | #include "common.hpp"
  5 | #include "rxso2.hpp"
  6 | #include "rxso3.hpp"
  7 | #include "se2.hpp"
  8 | #include "se3.hpp"
  9 | #include "sim2.hpp"
 10 | #include "sim3.hpp"
 11 | #include "so2.hpp"
 12 | #include "so3.hpp"
 13 | 
 14 | namespace Sophus {
 15 | 
 16 | template <class SequenceContainer>
 17 | optional<typename SequenceContainer::value_type> iterativeMean(
 18 |     SequenceContainer const& foo_Ts_bar, int max_num_iterations) {
 19 |   size_t N = foo_Ts_bar.size();
 20 |   SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
 21 | 
 22 |   using Group = typename SequenceContainer::value_type;
 23 |   using Scalar = typename Group::Scalar;
 24 |   using Tangent = typename Group::Tangent;
 25 | 
 26 |   // This implements the algorithm in the beginning of Sec. 4.2 in
 27 |   // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.
 28 |   Group foo_T_average = foo_Ts_bar.front();
 29 |   Scalar w = Scalar(1. / N);
 30 |   for (int i = 0; i < max_num_iterations; ++i) {
 31 |     Tangent average;
 32 |     setToZero<Tangent>(average);
 33 |     for (Group const& foo_T_bar : foo_Ts_bar) {
 34 |       average += w * (foo_T_average.inverse() * foo_T_bar).log();
 35 |     }
 36 |     Group foo_T_newaverage = foo_T_average * Group::exp(average);
 37 |     if (squaredNorm<Tangent>(
 38 |             (foo_T_newaverage.inverse() * foo_T_average).log()) <
 39 |         Constants<Scalar>::epsilon()) {
 40 |       return foo_T_newaverage;
 41 |     }
 42 | 
 43 |     foo_T_average = foo_T_newaverage;
 44 |   }
 45 |   return nullopt;
 46 | }
 47 | 
 48 | // Mean implementation for SO(2).
 49 | template <class SequenceContainer,
 50 |           class Scalar = typename SequenceContainer::value_type::Scalar>
 51 | enable_if_t<
 52 |     std::is_same<typename SequenceContainer::value_type, SO2<Scalar>>::value,
 53 |     optional<typename SequenceContainer::value_type>>
 54 | average(SequenceContainer const& foo_Ts_bar) {
 55 |   // This implements rotational part of Proposition 12 from Sec. 6.2 of
 56 |   // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.
 57 |   size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
 58 |   SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
 59 |   SO2<Scalar> foo_T_average = foo_Ts_bar.front();
 60 |   Scalar w = Scalar(1. / N);
 61 | 
 62 |   Scalar average(0);
 63 |   for (SO2<Scalar> const& foo_T_bar : foo_Ts_bar) {
 64 |     average += w * (foo_T_average.inverse() * foo_T_bar).log();
 65 |   }
 66 |   return foo_T_average * SO2<Scalar>::exp(average);
 67 | }
 68 | 
 69 | // Mean implementation for RxSO(2).
 70 | template <class SequenceContainer,
 71 |           class Scalar = typename SequenceContainer::value_type::Scalar>
 72 | enable_if_t<
 73 |     std::is_same<typename SequenceContainer::value_type, RxSO2<Scalar>>::value,
 74 |     optional<typename SequenceContainer::value_type>>
 75 | average(SequenceContainer const& foo_Ts_bar) {
 76 |   size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
 77 |   SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
 78 |   RxSO2<Scalar> foo_T_average = foo_Ts_bar.front();
 79 |   Scalar w = Scalar(1. / N);
 80 | 
 81 |   Vector2<Scalar> average(Scalar(0), Scalar(0));
 82 |   for (RxSO2<Scalar> const& foo_T_bar : foo_Ts_bar) {
 83 |     average += w * (foo_T_average.inverse() * foo_T_bar).log();
 84 |   }
 85 |   return foo_T_average * RxSO2<Scalar>::exp(average);
 86 | }
 87 | 
 88 | namespace details {
 89 | template <class T>
 90 | void getQuaternion(T const&);
 91 | 
 92 | template <class Scalar>
 93 | Eigen::Quaternion<Scalar> getUnitQuaternion(SO3<Scalar> const& R) {
 94 |   return R.unit_quaternion();
 95 | }
 96 | 
 97 | template <class Scalar>
 98 | Eigen::Quaternion<Scalar> getUnitQuaternion(RxSO3<Scalar> const& sR) {
 99 |   return sR.so3().unit_quaternion();
100 | }
101 | 
102 | template <class SequenceContainer,
103 |           class Scalar = typename SequenceContainer::value_type::Scalar>
104 | Eigen::Quaternion<Scalar> averageUnitQuaternion(
105 |     SequenceContainer const& foo_Ts_bar) {
106 |   // This:  http://stackoverflow.com/a/27410865/1221742
107 |   size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
108 |   SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
109 |   Eigen::Matrix<Scalar, 4, Eigen::Dynamic> Q(4, N);
110 |   int i = 0;
111 |   Scalar w = Scalar(1. / N);
112 |   for (auto const& foo_T_bar : foo_Ts_bar) {
113 |     Q.col(i) = w * details::getUnitQuaternion(foo_T_bar).coeffs();
114 |     ++i;
115 |   }
116 | 
117 |   Eigen::Matrix<Scalar, 4, 4> QQt = Q * Q.transpose();
118 |   // TODO: Figure out why we can't use SelfAdjointEigenSolver here.
119 |   Eigen::EigenSolver<Eigen::Matrix<Scalar, 4, 4>> es(QQt);
120 | 
121 |   std::complex<Scalar> max_eigenvalue = es.eigenvalues()[0];
122 |   Eigen::Matrix<std::complex<Scalar>, 4, 1> max_eigenvector =
123 |       es.eigenvectors().col(0);
124 | 
125 |   for (int i = 1; i < 4; i++) {
126 |     if (std::norm(es.eigenvalues()[i]) > std::norm(max_eigenvalue)) {
127 |       max_eigenvalue = es.eigenvalues()[i];
128 |       max_eigenvector = es.eigenvectors().col(i);
129 |     }
130 |   }
131 |   Eigen::Quaternion<Scalar> quat;
132 |   quat.coeffs() <<                //
133 |       max_eigenvector[0].real(),  //
134 |       max_eigenvector[1].real(),  //
135 |       max_eigenvector[2].real(),  //
136 |       max_eigenvector[3].real();
137 |   return quat;
138 | }
139 | }  // namespace details
140 | 
141 | // Mean implementation for SO(3).
142 | //
143 | // TODO: Detect degenerated cases and return nullopt.
144 | template <class SequenceContainer,
145 |           class Scalar = typename SequenceContainer::value_type::Scalar>
146 | enable_if_t<
147 |     std::is_same<typename SequenceContainer::value_type, SO3<Scalar>>::value,
148 |     optional<typename SequenceContainer::value_type>>
149 | average(SequenceContainer const& foo_Ts_bar) {
150 |   return SO3<Scalar>(details::averageUnitQuaternion(foo_Ts_bar));
151 | }
152 | 
153 | // Mean implementation for R x SO(3).
154 | template <class SequenceContainer,
155 |           class Scalar = typename SequenceContainer::value_type::Scalar>
156 | enable_if_t<
157 |     std::is_same<typename SequenceContainer::value_type, RxSO3<Scalar>>::value,
158 |     optional<typename SequenceContainer::value_type>>
159 | average(SequenceContainer const& foo_Ts_bar) {
160 |   size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
161 | 
162 |   SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
163 |   Scalar scale_sum = Scalar(0);
164 |   using std::log;
165 |   using std::exp;
166 |   for (RxSO3<Scalar> const& foo_T_bar : foo_Ts_bar) {
167 |     scale_sum += log(foo_T_bar.scale());
168 |   }
169 |   return RxSO3<Scalar>(exp(scale_sum / N),
170 |                        SO3<Scalar>(details::averageUnitQuaternion(foo_Ts_bar)));
171 | }
172 | 
173 | template <class SequenceContainer,
174 |           class Scalar = typename SequenceContainer::value_type::Scalar>
175 | enable_if_t<
176 |     std::is_same<typename SequenceContainer::value_type, SE2<Scalar>>::value,
177 |     optional<typename SequenceContainer::value_type>>
178 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
179 |   // TODO: Implement Proposition 12 from Sec. 6.2 of
180 |   // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.
181 |   return iterativeMean(foo_Ts_bar, max_num_iterations);
182 | }
183 | 
184 | template <class SequenceContainer,
185 |           class Scalar = typename SequenceContainer::value_type::Scalar>
186 | enable_if_t<
187 |     std::is_same<typename SequenceContainer::value_type, Sim2<Scalar>>::value,
188 |     optional<typename SequenceContainer::value_type>>
189 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
190 |   return iterativeMean(foo_Ts_bar, max_num_iterations);
191 | }
192 | 
193 | template <class SequenceContainer,
194 |           class Scalar = typename SequenceContainer::value_type::Scalar>
195 | enable_if_t<
196 |     std::is_same<typename SequenceContainer::value_type, SE3<Scalar>>::value,
197 |     optional<typename SequenceContainer::value_type>>
198 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
199 |   return iterativeMean(foo_Ts_bar, max_num_iterations);
200 | }
201 | 
202 | template <class SequenceContainer,
203 |           class Scalar = typename SequenceContainer::value_type::Scalar>
204 | enable_if_t<
205 |     std::is_same<typename SequenceContainer::value_type, Sim3<Scalar>>::value,
206 |     optional<typename SequenceContainer::value_type>>
207 | average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
208 |   return iterativeMean(foo_Ts_bar, max_num_iterations);
209 | }
210 | 
211 | }  // namespace Sophus
212 | 
213 | #endif  // SOPHUS_AVERAGE_HPP
214 | 


--------------------------------------------------------------------------------
/sophus/common.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef SOPHUS_COMMON_HPP
  2 | #define SOPHUS_COMMON_HPP
  3 | 
  4 | #include <cmath>
  5 | #include <cstdio>
  6 | #include <cstdlib>
  7 | #include <iostream>
  8 | #include <random>
  9 | #include <type_traits>
 10 | 
 11 | #include <Eigen/Core>
 12 | 
 13 | // following boost's assert.hpp
 14 | #undef SOPHUS_ENSURE
 15 | 
 16 | // ENSURES are similar to ASSERTS, but they are always checked for (including in
 17 | // release builds). At the moment there are no ASSERTS in Sophus which should
 18 | // only be used for checks which are performance critical.
 19 | 
 20 | #ifdef __GNUC__
 21 | #define SOPHUS_FUNCTION __PRETTY_FUNCTION__
 22 | #elif (_MSC_VER >= 1310)
 23 | #define SOPHUS_FUNCTION __FUNCTION__
 24 | #else
 25 | #define SOPHUS_FUNCTION "unknown"
 26 | #endif
 27 | 
 28 | // Make sure this compiles with older versions of Eigen which do not have
 29 | // EIGEN_DEVICE_FUNC defined.
 30 | #ifndef EIGEN_DEVICE_FUNC
 31 | #define EIGEN_DEVICE_FUNC
 32 | #endif
 33 | 
 34 | #define SOPHUS_FUNC EIGEN_DEVICE_FUNC
 35 | 
 36 | namespace Sophus {
 37 | namespace details {
 38 | 
 39 | // Following: http://stackoverflow.com/a/22759544
 40 | template <class T>
 41 | class IsStreamable {
 42 |  private:
 43 |   template <class TT>
 44 |   static auto test(int)
 45 |       -> decltype(std::declval<std::stringstream&>() << std::declval<TT>(),
 46 |                   std::true_type());
 47 | 
 48 |   template <class>
 49 |   static auto test(...) -> std::false_type;
 50 | 
 51 |  public:
 52 |   static bool const value = decltype(test<T>(0))::value;
 53 | };
 54 | 
 55 | template <class T>
 56 | class ArgToStream {
 57 |  public:
 58 |   static void impl(std::stringstream& stream, T&& arg) {
 59 |     stream << std::forward<T>(arg);
 60 |   }
 61 | };
 62 | 
 63 | inline void FormatStream(std::stringstream& stream, char const* text) {
 64 |   stream << text;
 65 |   return;
 66 | }
 67 | 
 68 | // Following: http://en.cppreference.com/w/cpp/language/parameter_pack
 69 | template <class T, typename... Args>
 70 | void FormatStream(std::stringstream& stream, char const* text, T&& arg,
 71 |                   Args&&... args) {
 72 |   static_assert(IsStreamable<T>::value,
 73 |                 "One of the args has no ostream overload!");
 74 |   for (; *text != '\0'; ++text) {
 75 |     if (*text == '%') {
 76 |       ArgToStream<T&&>::impl(stream, std::forward<T>(arg));
 77 |       FormatStream(stream, text + 1, std::forward<Args>(args)...);
 78 |       return;
 79 |     }
 80 |     stream << *text;
 81 |   }
 82 |   stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1
 83 |          << " args unused.";
 84 |   return;
 85 | }
 86 | 
 87 | template <class... Args>
 88 | std::string FormatString(char const* text, Args&&... args) {
 89 |   std::stringstream stream;
 90 |   FormatStream(stream, text, std::forward<Args>(args)...);
 91 |   return stream.str();
 92 | }
 93 | 
 94 | inline std::string FormatString() { return std::string(); }
 95 | }  // namespace details
 96 | }  // namespace Sophus
 97 | 
 98 | #if defined(SOPHUS_DISABLE_ENSURES)
 99 | 
100 | #define SOPHUS_ENSURE(expr, ...) ((void)0)
101 | 
102 | #elif defined(SOPHUS_ENABLE_ENSURE_HANDLER)
103 | 
104 | namespace Sophus {
105 | void ensureFailed(char const* function, char const* file, int line,
106 |                   char const* description);
107 | }
108 | 
109 | #define SOPHUS_ENSURE(expr, ...)                     \
110 |   ((expr) ? ((void)0)                                \
111 |           : ::Sophus::ensureFailed(                  \
112 |                 SOPHUS_FUNCTION, __FILE__, __LINE__, \
113 |                 Sophus::details::FormatString(##__VA_ARGS__).c_str()))
114 | #else
115 | namespace Sophus {
116 | template <class... Args>
117 | SOPHUS_FUNC void defaultEnsure(char const* function, char const* file, int line,
118 |                                char const* description, Args&&... args) {
119 |   std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n",
120 |               function, file, line);
121 | #ifdef __CUDACC__
122 |   std::printf("%s", description);
123 | #else
124 |   std::cout << details::FormatString(description, std::forward<Args>(args)...)
125 |             << std::endl;
126 |   std::abort();
127 | #endif
128 | }
129 | }  // namespace Sophus
130 | #define SOPHUS_ENSURE(expr, ...)                                         \
131 |   ((expr) ? ((void)0) : Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, \
132 |                                               __LINE__, ##__VA_ARGS__))
133 | #endif
134 | 
135 | namespace Sophus {
136 | 
137 | template <class Scalar>
138 | struct Constants {
139 |   SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
140 | 
141 |   SOPHUS_FUNC static Scalar pi() { return Scalar(M_PI); }
142 | };
143 | 
144 | template <>
145 | struct Constants<float> {
146 |   SOPHUS_FUNC static float constexpr epsilon() {
147 |     return static_cast<float>(1e-5);
148 |   }
149 | 
150 |   SOPHUS_FUNC static float constexpr pi() { return static_cast<float>(M_PI); }
151 | };
152 | 
153 | // Leightweight optional implementation which require ``T`` to have a
154 | // default constructor.
155 | //
156 | // TODO: Replace with std::optional once Sophus moves to c++17.
157 | //
158 | struct nullopt_t {
159 |   explicit constexpr nullopt_t() {}
160 | };
161 | 
162 | constexpr nullopt_t nullopt{};
163 | template <class T>
164 | 
165 | class optional {
166 |  public:
167 |   optional() : is_valid_(false) {}
168 | 
169 |   optional(nullopt_t) : is_valid_(false) {}
170 | 
171 |   optional(T const& type) : type_(type), is_valid_(true) {}
172 | 
173 |   explicit operator bool() const { return is_valid_; }
174 | 
175 |   T const* operator->() const {
176 |     SOPHUS_ENSURE(is_valid_, "must be valid");
177 |     return &type_;
178 |   }
179 | 
180 |   T* operator->() {
181 |     SOPHUS_ENSURE(is_valid_, "must be valid");
182 |     return &type_;
183 |   }
184 | 
185 |   T const& operator*() const {
186 |     SOPHUS_ENSURE(is_valid_, "must be valid");
187 |     return type_;
188 |   }
189 | 
190 |   T& operator*() {
191 |     SOPHUS_ENSURE(is_valid_, "must be valid");
192 |     return type_;
193 |   }
194 | 
195 |  private:
196 |   T type_;
197 |   bool is_valid_;
198 | };
199 | 
200 | template <bool B, class T = void>
201 | using enable_if_t = typename std::enable_if<B, T>::type;
202 | 
203 | template <class G>
204 | struct IsUniformRandomBitGenerator {
205 |   static const bool value = std::is_unsigned<typename G::result_type>::value &&
206 |                             std::is_unsigned<decltype(G::min())>::value &&
207 |                             std::is_unsigned<decltype(G::max())>::value;
208 | };
209 | }  // namespace Sophus
210 | 
211 | #endif  // SOPHUS_COMMON_HPP
212 | 


--------------------------------------------------------------------------------
/sophus/geometry.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef GEOMETRY_HPP
  2 | #define GEOMETRY_HPP
  3 | 
  4 | #include "se2.hpp"
  5 | #include "se3.hpp"
  6 | #include "so2.hpp"
  7 | #include "so3.hpp"
  8 | #include "types.hpp"
  9 | 
 10 | namespace Sophus {
 11 | 
 12 | // Takes in a rotation ``R_foo_plane`` and returns the corresponding line
 13 | // normal along the y-axis (in reference frame ``foo``).
 14 | //
 15 | template <class T>
 16 | Vector2<T> normalFromSO2(SO2<T> const& R_foo_line) {
 17 |   return R_foo_line.matrix().col(1);
 18 | }
 19 | 
 20 | // Takes in line normal in reference frame foo and constructs a corresponding
 21 | // rotation matrix ``R_foo_line``.
 22 | //
 23 | // Precondition: ``normal_foo`` must not be close to zero.
 24 | //
 25 | template <class T>
 26 | SO2<T> SO2FromNormal(Vector2<T> normal_foo) {
 27 |   SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants<T>::epsilon(), "%",
 28 |                 normal_foo.transpose());
 29 |   normal_foo.normalize();
 30 |   return SO2<T>(normal_foo.y(), -normal_foo.x());
 31 | }
 32 | 
 33 | // Takes in a rotation ``R_foo_plane`` and returns the corresponding plane
 34 | // normal along the z-axis
 35 | // (in reference frame ``foo``).
 36 | //
 37 | template <class T>
 38 | Vector3<T> normalFromSO3(SO3<T> const& R_foo_plane) {
 39 |   return R_foo_plane.matrix().col(2);
 40 | }
 41 | 
 42 | // Takes in plane normal in reference frame foo and constructs a corresponding
 43 | // rotation matrix ``R_foo_plane``.
 44 | //
 45 | // Note: The ``plane`` frame is defined as such that the normal points along the
 46 | //       positive z-axis. One can specify hints for the x-axis and y-axis of the
 47 | //       ``plane`` frame.
 48 | //
 49 | // Preconditions:
 50 | // - ``normal_foo``, ``xDirHint_foo``, ``yDirHint_foo`` must not be close to
 51 | //   zero.
 52 | // - ``xDirHint_foo`` and ``yDirHint_foo`` must be approx. perpendicular.
 53 | //
 54 | template <class T>
 55 | Matrix3<T> rotationFromNormal(Vector3<T> const& normal_foo,
 56 |                               Vector3<T> xDirHint_foo = Vector3<T>(T(1), T(0),
 57 |                                                                    T(0)),
 58 |                               Vector3<T> yDirHint_foo = Vector3<T>(T(0), T(1),
 59 |                                                                    T(0))) {
 60 |   SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants<T>::epsilon(),
 61 |                 "xDirHint (%) and yDirHint (%) must be perpendicular.",
 62 |                 xDirHint_foo.transpose(), yDirHint_foo.transpose());
 63 |   using std::abs;
 64 |   using std::sqrt;
 65 |   T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm();
 66 |   T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm();
 67 |   T const normal_foo_sqr_length = normal_foo.squaredNorm();
 68 |   SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants<T>::epsilon(), "%",
 69 |                 xDirHint_foo.transpose());
 70 |   SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants<T>::epsilon(), "%",
 71 |                 yDirHint_foo.transpose());
 72 |   SOPHUS_ENSURE(normal_foo_sqr_length > Constants<T>::epsilon(), "%",
 73 |                 normal_foo.transpose());
 74 | 
 75 |   Matrix3<T> basis_foo;
 76 |   basis_foo.col(2) = normal_foo;
 77 | 
 78 |   if (abs(xDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
 79 |     xDirHint_foo.normalize();
 80 |   }
 81 |   if (abs(yDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
 82 |     yDirHint_foo.normalize();
 83 |   }
 84 |   if (abs(normal_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
 85 |     basis_foo.col(2).normalize();
 86 |   }
 87 | 
 88 |   T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo));
 89 |   T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo));
 90 |   if (abs_x_dot_z < abs_y_dot_z) {
 91 |     // basis_foo.z and xDirHint are far from parallel.
 92 |     basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized();
 93 |     basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2));
 94 |   } else {
 95 |     // basis_foo.z and yDirHint are far from parallel.
 96 |     basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized();
 97 |     basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0));
 98 |   }
 99 |   T det = basis_foo.determinant();
100 |   // sanity check
101 |   SOPHUS_ENSURE(abs(det - T(1)) < Constants<T>::epsilon(),
102 |                 "Determinant of basis is not 1, but %. Basis is \n%\n", det,
103 |                 basis_foo);
104 |   return basis_foo;
105 | }
106 | 
107 | // Takes in plane normal in reference frame foo and constructs a corresponding
108 | // rotation matrix ``R_foo_plane``.
109 | //
110 | // See ``rotationFromNormal`` for details.
111 | //
112 | template <class T>
113 | SO3<T> SO3FromNormal(Vector3<T> const& normal_foo) {
114 |   return SO3<T>(rotationFromNormal(normal_foo));
115 | }
116 | 
117 | // Returns a line (wrt. to frame ``foo``), given a pose of the ``line`` in
118 | // reference frame ``foo``.
119 | //
120 | // Note: The plane is defined by X-axis of the ``line`` frame.
121 | //
122 | template <class T>
123 | Line2<T> lineFromSE2(SE2<T> const& T_foo_line) {
124 |   return Line2<T>(normalFromSO2(T_foo_line.so2()), T_foo_line.translation());
125 | }
126 | 
127 | // Returns the pose ``T_foo_line``, given a line in reference frame ``foo``.
128 | //
129 | // Note: The line is defined by X-axis of the frame ``line``.
130 | //
131 | template <class T>
132 | SE2<T> SE2FromLine(Line2<T> const& line_foo) {
133 |   T const d = line_foo.offset();
134 |   Vector2<T> const n = line_foo.normal();
135 |   SO2<T> const R_foo_plane = SO2FromNormal(n);
136 |   return SE2<T>(R_foo_plane, -d * n);
137 | }
138 | 
139 | // Returns a plane (wrt. to frame ``foo``), given a pose of the ``plane`` in
140 | // reference frame ``foo``.
141 | //
142 | // Note: The plane is defined by XY-plane of the frame ``plane``.
143 | //
144 | template <class T>
145 | Plane3<T> planeFromSE3(SE3<T> const& T_foo_plane) {
146 |   return Plane3<T>(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation());
147 | }
148 | 
149 | // Returns the pose ``T_foo_plane``, given a plane in reference frame ``foo``.
150 | //
151 | // Note: The plane is defined by XY-plane of the frame ``plane``.
152 | //
153 | template <class T>
154 | SE3<T> SE3FromPlane(Plane3<T> const& plane_foo) {
155 |   T const d = plane_foo.offset();
156 |   Vector3<T> const n = plane_foo.normal();
157 |   SO3<T> const R_foo_plane = SO3FromNormal(n);
158 |   return SE3<T>(R_foo_plane, -d * n);
159 | }
160 | 
161 | // Takes in a hyperplane and returns unique representation by ensuring that the
162 | // ``offset`` is not negative.
163 | //
164 | template <class T, int N>
165 | Eigen::Hyperplane<T, N> makeHyperplaneUnique(
166 |     const Eigen::Hyperplane<T, N>& plane) {
167 |   if (plane.offset() >= 0) {
168 |     return plane;
169 |   }
170 | 
171 |   return Eigen::Hyperplane<T, N>(-plane.normal(), -plane.offset());
172 | }
173 | 
174 | }  // namespace Sophus
175 | 
176 | #endif  // GEOMETRY_HPP
177 | 


--------------------------------------------------------------------------------
/sophus/interpolate.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef SOPHUS_INTERPOLATE_HPP
 2 | #define SOPHUS_INTERPOLATE_HPP
 3 | 
 4 | #include <Eigen/Eigenvalues>
 5 | 
 6 | #include "interpolate_details.hpp"
 7 | 
 8 | namespace Sophus {
 9 | 
10 | // This function interpolates between two Lie group elements ``foo_T_bar``
11 | // and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1].
12 | //
13 | // It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar``
14 | // and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns
15 | // ``foo_T_bar``.
16 | //
17 | // (Since interpolation on Lie groups is inverse-invariant, we can equivalently
18 | // think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the
19 | // return value being ``quiz_T_foo``.)
20 | //
21 | // Precondition: ``p`` must be in [0, 1].
22 | //
23 | template <class G, class Scalar2 = typename G::Scalar>
24 | enable_if_t<interp_details::Traits<G>::supported, G> interpolate(
25 |     G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) {
26 |   using Scalar = typename G::Scalar;
27 |   Scalar inter_p(p);
28 |   SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1),
29 |                 "p (%) must in [0, 1].");
30 |   return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log());
31 | }
32 | 
33 | }  // namespace Sophus
34 | 
35 | #endif  // SOPHUS_INTERPOLATE_HPP
36 | 


--------------------------------------------------------------------------------
/sophus/interpolate_details.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef SOPHUS_INTERPOLATE_DETAILS_HPP
  2 | #define SOPHUS_INTERPOLATE_DETAILS_HPP
  3 | 
  4 | #include "rxso2.hpp"
  5 | #include "rxso3.hpp"
  6 | #include "se2.hpp"
  7 | #include "se3.hpp"
  8 | #include "sim2.hpp"
  9 | #include "sim3.hpp"
 10 | #include "so2.hpp"
 11 | #include "so3.hpp"
 12 | 
 13 | namespace Sophus {
 14 | namespace interp_details {
 15 | 
 16 | template <class Group>
 17 | struct Traits;
 18 | 
 19 | template <class Scalar>
 20 | struct Traits<SO2<Scalar>> {
 21 |   static bool constexpr supported = true;
 22 | 
 23 |   static bool hasShortestPathAmbiguity(SO2<Scalar> const& foo_T_bar) {
 24 |     using std::abs;
 25 |     Scalar angle = SO2<Scalar>::log(foo_T_bar);
 26 |     return abs(abs(angle) - Constants<Scalar>::pi()) <
 27 |            Constants<Scalar>::epsilon();
 28 |   }
 29 | };
 30 | 
 31 | template <class Scalar>
 32 | struct Traits<RxSO2<Scalar>> {
 33 |   static bool constexpr supported = true;
 34 | 
 35 |   static bool hasShortestPathAmbiguity(RxSO2<Scalar> const& foo_T_bar) {
 36 |     return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());
 37 |   }
 38 | };
 39 | 
 40 | template <class Scalar>
 41 | struct Traits<SO3<Scalar>> {
 42 |   static bool constexpr supported = true;
 43 | 
 44 |   static bool hasShortestPathAmbiguity(SO3<Scalar> const& foo_T_bar) {
 45 |     using std::abs;
 46 |     Scalar angle;
 47 |     SO3<Scalar>::logAndTheta(foo_T_bar, &angle);
 48 |     return abs(abs(angle) - Constants<Scalar>::pi()) <
 49 |            Constants<Scalar>::epsilon();
 50 |   }
 51 | };
 52 | 
 53 | template <class Scalar>
 54 | struct Traits<RxSO3<Scalar>> {
 55 |   static bool constexpr supported = true;
 56 | 
 57 |   static bool hasShortestPathAmbiguity(RxSO3<Scalar> const& foo_T_bar) {
 58 |     return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());
 59 |   }
 60 | };
 61 | 
 62 | template <class Scalar>
 63 | struct Traits<SE2<Scalar>> {
 64 |   static bool constexpr supported = true;
 65 | 
 66 |   static bool hasShortestPathAmbiguity(SE2<Scalar> const& foo_T_bar) {
 67 |     return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());
 68 |   }
 69 | };
 70 | 
 71 | template <class Scalar>
 72 | struct Traits<SE3<Scalar>> {
 73 |   static bool constexpr supported = true;
 74 | 
 75 |   static bool hasShortestPathAmbiguity(SE3<Scalar> const& foo_T_bar) {
 76 |     return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());
 77 |   }
 78 | };
 79 | 
 80 | template <class Scalar>
 81 | struct Traits<Sim2<Scalar>> {
 82 |   static bool constexpr supported = true;
 83 | 
 84 |   static bool hasShortestPathAmbiguity(Sim2<Scalar> const& foo_T_bar) {
 85 |     return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(
 86 |         foo_T_bar.rxso2().so2());
 87 |     ;
 88 |   }
 89 | };
 90 | 
 91 | template <class Scalar>
 92 | struct Traits<Sim3<Scalar>> {
 93 |   static bool constexpr supported = true;
 94 | 
 95 |   static bool hasShortestPathAmbiguity(Sim3<Scalar> const& foo_T_bar) {
 96 |     return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(
 97 |         foo_T_bar.rxso3().so3());
 98 |     ;
 99 |   }
100 | };
101 | 
102 | }  // namespace details
103 | }  // namespace Sophus
104 | 
105 | #endif  // SOPHUS_INTERPOLATE_DETAILS_HPP
106 | 


--------------------------------------------------------------------------------
/sophus/num_diff.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef SOPHUS_NUM_DIFF_HPP
 2 | #define SOPHUS_NUM_DIFF_HPP
 3 | 
 4 | #include <type_traits>
 5 | #include <utility>
 6 | 
 7 | #include "types.hpp"
 8 | 
 9 | namespace Sophus {
10 | 
11 | // Numerical differentiation using finite differences
12 | template <class Scalar>
13 | class NumDiff {
14 |  public:
15 |   // Calculates the derivative of a curve at a point ``t``.
16 |   //
17 |   // Here, a curve is a function from a Scalar to a Euclidean space. Thus, it
18 |   // returns either a Scalar, a vector or a matrix.
19 |   //
20 |   template <class Fn>
21 |   static auto curve(Fn const& curve, Scalar t,
22 |                     Scalar h = Constants<Scalar>::epsilon)
23 |       -> decltype(curve(t)) {
24 |     using ReturnType = decltype(curve(t));
25 |     static_assert(IsFloatingPoint<ReturnType>::value,
26 |                   "ReturnType must be either a floating point scalar, "
27 |                   "vector or matrix.");
28 |     static_assert(
29 |         std::is_same<typename GetScalar<ReturnType>::Scalar, Scalar>::value,
30 |         "Input and output scalar must be same type (e.g. both float, or both "
31 |         "double).");
32 |     return (curve(t + h) - curve(t - h)) / (Scalar(2) * h);
33 |   }
34 | };
35 | }  // namespace Sophus
36 | 
37 | #endif  // SOPHUS_NUM_DIFF_HPP
38 | 


--------------------------------------------------------------------------------
/sophus/rotation_matrix.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef SOPHUS_ROTATION_MATRIX_HPP
 2 | #define SOPHUS_ROTATION_MATRIX_HPP
 3 | 
 4 | #include <Eigen/Dense>
 5 | #include <Eigen/SVD>
 6 | 
 7 | #include "types.hpp"
 8 | 
 9 | namespace Sophus {
10 | 
11 | // Takes in arbiray square matrix and returns true if it is
12 | // orthogonal.
13 | template <class D>
14 | SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<D> const& R) {
15 |   using Scalar = typename D::Scalar;
16 |   static int const N = D::RowsAtCompileTime;
17 |   static int const M = D::ColsAtCompileTime;
18 | 
19 |   static_assert(N == M, "must be a square matrix");
20 |   static_assert(N >= 2, "must have compile time dimension >= 2");
21 | 
22 |   return (R * R.transpose() - Matrix<Scalar, N, N>::Identity()).norm() <
23 |          Constants<Scalar>::epsilon();
24 | }
25 | 
26 | // Takes in arbiray square matrix and returns true if it is
27 | // "scaled-orthogonal" with positive determinant.
28 | //
29 | template <class D>
30 | SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> const& sR) {
31 |   using Scalar = typename D::Scalar;
32 |   static int const N = D::RowsAtCompileTime;
33 |   static int const M = D::ColsAtCompileTime;
34 |   using std::pow;
35 |   using std::sqrt;
36 | 
37 |   Scalar det = sR.determinant();
38 | 
39 |   if (det <= 0) {
40 |     return false;
41 |   }
42 | 
43 |   Scalar scale_sqr = pow(det, Scalar(2. / N));
44 | 
45 |   static_assert(N == M, "must be a square matrix");
46 |   static_assert(N >= 2, "must have compile time dimension >= 2");
47 | 
48 |   return (sR * sR.transpose() - scale_sqr * Matrix<Scalar, N, N>::Identity())
49 |              .template lpNorm<Eigen::Infinity>() <
50 |          sqrt(Constants<Scalar>::epsilon());
51 | }
52 | 
53 | // Takes in arbiray square matrix (2x2 or larger) and returns closest
54 | // orthogonal matrix with positive determinant.
55 | template <class D>
56 | SOPHUS_FUNC
57 |     Matrix<typename D::Scalar, D::RowsAtCompileTime, D::RowsAtCompileTime>
58 |     makeRotationMatrix(Eigen::MatrixBase<D> const& R) {
59 |   using Scalar = typename D::Scalar;
60 |   static int const N = D::RowsAtCompileTime;
61 |   static int const M = D::ColsAtCompileTime;
62 | 
63 |   static_assert(N == M, "must be a square matrix");
64 |   static_assert(N >= 2, "must have compile time dimension >= 2");
65 | 
66 |   Eigen::JacobiSVD<Matrix<Scalar, N, N>> svd(
67 |       R, Eigen::ComputeFullU | Eigen::ComputeFullV);
68 | 
69 |   // Determine determinant of orthogonal matrix U*V'.
70 |   Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant();
71 |   // Starting from the identity matrix D, set the last entry to d (+1 or
72 |   // -1),  so that det(U*D*V') = 1.
73 |   Matrix<Scalar, N, N> Diag = Matrix<Scalar, N, N>::Identity();
74 |   Diag(N - 1, N - 1) = d;
75 |   return svd.matrixU() * Diag * svd.matrixV().transpose();
76 | }
77 | 
78 | }  // namespace Sophus
79 | 
80 | #endif  // SOPHUS_ROTATION_MATRIX_HPP
81 | 


--------------------------------------------------------------------------------
/sophus/sim_details.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef SOPHUS_SIM_DETAILS_HPP
  2 | #define SOPHUS_SIM_DETAILS_HPP
  3 | 
  4 | #include "types.hpp"
  5 | 
  6 | namespace Sophus {
  7 | namespace details {
  8 | 
  9 | template <class Scalar, int N>
 10 | Matrix<Scalar, N, N> calcW(Matrix<Scalar, N, N> const& Omega,
 11 |                            Scalar const theta, Scalar const sigma) {
 12 |   using std::abs;
 13 |   using std::exp;
 14 |   using std::sin;
 15 |   using std::cos;
 16 |   static Matrix<Scalar, N, N> const I = Matrix<Scalar, N, N>::Identity();
 17 |   static Scalar const one(1);
 18 |   static Scalar const half(0.5);
 19 |   Matrix<Scalar, N, N> const Omega2 = Omega * Omega;
 20 |   Scalar const scale = exp(sigma);
 21 |   Scalar A, B, C;
 22 |   if (abs(sigma) < Constants<Scalar>::epsilon()) {
 23 |     C = one;
 24 |     if (abs(theta) < Constants<Scalar>::epsilon()) {
 25 |       A = half;
 26 |       B = Scalar(1. / 6.);
 27 |     } else {
 28 |       Scalar theta_sq = theta * theta;
 29 |       A = (one - cos(theta)) / theta_sq;
 30 |       B = (theta - sin(theta)) / (theta_sq * theta);
 31 |     }
 32 |   } else {
 33 |     C = (scale - one) / sigma;
 34 |     if (abs(theta) < Constants<Scalar>::epsilon()) {
 35 |       Scalar sigma_sq = sigma * sigma;
 36 |       A = ((sigma - one) * scale + one) / sigma_sq;
 37 |       B = (scale * half * sigma_sq + scale - one - sigma * scale) /
 38 |           (sigma_sq * sigma);
 39 |     } else {
 40 |       Scalar theta_sq = theta * theta;
 41 |       Scalar a = scale * sin(theta);
 42 |       Scalar b = scale * cos(theta);
 43 |       Scalar c = theta_sq + sigma * sigma;
 44 |       A = (a * sigma + (one - b) * theta) / (theta * c);
 45 |       B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq);
 46 |     }
 47 |   }
 48 |   return A * Omega + B * Omega2 + C * I;
 49 | }
 50 | 
 51 | template <class Scalar, int N>
 52 | Matrix<Scalar, N, N> calcWInv(Matrix<Scalar, N, N> const& Omega,
 53 |                               Scalar const theta, Scalar const sigma,
 54 |                               Scalar const scale) {
 55 |   using std::abs;
 56 |   using std::sin;
 57 |   using std::cos;
 58 |   static Matrix<Scalar, N, N> const I = Matrix<Scalar, N, N>::Identity();
 59 |   static Scalar const half(0.5);
 60 |   static Scalar const one(1);
 61 |   static Scalar const two(2);
 62 |   Matrix<Scalar, N, N> const Omega2 = Omega * Omega;
 63 |   Scalar const scale_sq = scale * scale;
 64 |   Scalar const theta_sq = theta * theta;
 65 |   Scalar const sin_theta = sin(theta);
 66 |   Scalar const cos_theta = cos(theta);
 67 | 
 68 |   Scalar a, b, c;
 69 |   if (abs(sigma * sigma) < Constants<Scalar>::epsilon()) {
 70 |     c = one - half * sigma;
 71 |     a = -half;
 72 |     if (abs(theta_sq) < Constants<Scalar>::epsilon()) {
 73 |       b = Scalar(1. / 12.);
 74 |     } else {
 75 |       b = (theta * sin_theta + two * cos_theta - two) /
 76 |           (two * theta_sq * (cos_theta - one));
 77 |     }
 78 |   } else {
 79 |     Scalar const scale_cu = scale_sq * scale;
 80 |     c = sigma / (scale - one);
 81 |     if (abs(theta_sq) < Constants<Scalar>::epsilon()) {
 82 |       a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one));
 83 |       b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) /
 84 |           (two * scale_cu - Scalar(6) * scale_sq + Scalar(6) * scale - two);
 85 |     } else {
 86 |       Scalar const s_sin_theta = scale * sin_theta;
 87 |       Scalar const s_cos_theta = scale * cos_theta;
 88 |       a = (theta * s_cos_theta - theta - sigma * s_sin_theta) /
 89 |           (theta * (scale_sq - two * s_cos_theta + one));
 90 |       b = -scale *
 91 |           (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta -
 92 |            scale * sigma + sigma * cos_theta - sigma) /
 93 |           (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq +
 94 |                        two * s_cos_theta + scale - one));
 95 |     }
 96 |   }
 97 |   return a * Omega + b * Omega2 + c * I;
 98 | }
 99 | 
100 | }  // details
101 | }  // Sophus
102 | 
103 | #endif
104 | 


--------------------------------------------------------------------------------
/sophus/so2.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef SOPHUS_SO2_HPP
  2 | #define SOPHUS_SO2_HPP
  3 | 
  4 | #include <complex>
  5 | 
  6 | // Include only the selective set of Eigen headers that we need.
  7 | // This helps when using Sophus with unusual compilers, like nvcc.
  8 | #include <Eigen/LU>
  9 | 
 10 | #include "rotation_matrix.hpp"
 11 | #include "types.hpp"
 12 | 
 13 | namespace Sophus {
 14 | template <class Scalar_, int Options = 0>
 15 | class SO2;
 16 | using SO2d = SO2<double>;
 17 | using SO2f = SO2<float>;
 18 | }  // namespace Sophus
 19 | 
 20 | namespace Eigen {
 21 | namespace internal {
 22 | 
 23 | template <class Scalar_, int Options>
 24 | struct traits<Sophus::SO2<Scalar_, Options>> {
 25 |   using Scalar = Scalar_;
 26 |   using ComplexType = Sophus::Vector2<Scalar, Options>;
 27 | };
 28 | 
 29 | template <class Scalar_, int Options>
 30 | struct traits<Map<Sophus::SO2<Scalar_>, Options>>
 31 |     : traits<Sophus::SO2<Scalar_, Options>> {
 32 |   using Scalar = Scalar_;
 33 |   using ComplexType = Map<Sophus::Vector2<Scalar>, Options>;
 34 | };
 35 | 
 36 | template <class Scalar_, int Options>
 37 | struct traits<Map<Sophus::SO2<Scalar_> const, Options>>
 38 |     : traits<Sophus::SO2<Scalar_, Options> const> {
 39 |   using Scalar = Scalar_;
 40 |   using ComplexType = Map<Sophus::Vector2<Scalar> const, Options>;
 41 | };
 42 | }  // namespace internal
 43 | }  // namespace Eigen
 44 | 
 45 | namespace Sophus {
 46 | 
 47 | // SO2 base type - implements SO2 class but is storage agnostic.
 48 | //
 49 | // SO(2) is the group of rotations in 2d. As a matrix group, it is the set of
 50 | // matrices which are orthogonal such that ``R * R' = I`` (with ``R'`` being the
 51 | // transpose of ``R``) and have a positive determinant. In particular, the
 52 | // determinant is 1. Let ``theta`` be the rotation angle, the rotation matrix
 53 | // can be written in close form:
 54 | //
 55 | //  | cos(theta) -sin(theta) |
 56 | //  | sin(theta)  cos(theta) |
 57 | //
 58 | // As a matter of fact, the first column of those matrices is isomorph to the
 59 | // set of unit complex numbers U(1). Thus, internally, SO2 is represented as
 60 | // complex number with length 1.
 61 | //
 62 | // SO(2) is a compact and commutative group. First it is compact since the set
 63 | // of rotation matrices is a closed and bounded set. Second it is commutative
 64 | // since ``R(alpha) * R(beta) = R(beta) * R(alpha)``,  simply because ``alpha +
 65 | // beta = beta + alpha`` with ``alpha`` and ``beta`` being rotation angles
 66 | // (about the same axis).
 67 | //
 68 | // Class invairant: The 2-norm of ``unit_complex`` must be close to 1.
 69 | // Technically speaking, it must hold that:
 70 | //
 71 | //   ``|unit_complex().squaredNorm() - 1| <= Constants<Scalar>::epsilon()``.
 72 | template <class Derived>
 73 | class SO2Base {
 74 |  public:
 75 |   using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
 76 |   using ComplexT = typename Eigen::internal::traits<Derived>::ComplexType;
 77 | 
 78 |   // Degrees of freedom of manifold, number of dimensions in tangent space (one
 79 |   // since we only have in-plane rotations).
 80 |   static int constexpr DoF = 1;
 81 |   // Number of internal parameters used (complex numbers are a tuples).
 82 |   static int constexpr num_parameters = 2;
 83 |   // Group transformations are 2x2 matrices.
 84 |   static int constexpr N = 2;
 85 |   using Transformation = Matrix<Scalar, N, N>;
 86 |   using Point = Vector2<Scalar>;
 87 |   using Line = ParametrizedLine2<Scalar>;
 88 |   using Tangent = Scalar;
 89 |   using Adjoint = Scalar;
 90 | 
 91 |   // Adjoint transformation
 92 |   //
 93 |   // This function return the adjoint transformation ``Ad`` of the group
 94 |   // element ``A`` such that for all ``x`` it holds that
 95 |   // ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
 96 |   //
 97 |   // It simply ``1``, since ``SO(2)`` is a commutative group.
 98 |   //
 99 |   SOPHUS_FUNC Adjoint Adj() const { return 1; }
100 | 
101 |   // Returns copy of instance casted to NewScalarType.
102 |   //
103 |   template <class NewScalarType>
104 |   SOPHUS_FUNC SO2<NewScalarType> cast() const {
105 |     return SO2<NewScalarType>(unit_complex().template cast<NewScalarType>());
106 |   }
107 | 
108 |   // This provides unsafe read/write access to internal data. SO(2) is
109 |   // represented by a unit complex number (two parameters). When using direct
110 |   // write access, the user needs to take care of that the complex number stays
111 |   // normalized.
112 |   //
113 |   SOPHUS_FUNC Scalar* data() { return unit_complex_nonconst().data(); }
114 | 
115 |   // Const version of data() above.
116 |   //
117 |   SOPHUS_FUNC Scalar const* data() const { return unit_complex().data(); }
118 | 
119 |   // Returns group inverse.
120 |   //
121 |   SOPHUS_FUNC SO2<Scalar> inverse() const {
122 |     return SO2<Scalar>(unit_complex().x(), -unit_complex().y());
123 |   }
124 | 
125 |   // Logarithmic map
126 |   //
127 |   // Returns tangent space representation (= rotation angle) of the instance.
128 |   //
129 |   SOPHUS_FUNC Scalar log() const { return SO2<Scalar>::log(*this); }
130 | 
131 |   // It re-normalizes ``unit_complex`` to unit length.
132 |   //
133 |   // Note: Because of the class invariant, there is typically no need to call
134 |   // this function directly.
135 |   //
136 |   SOPHUS_FUNC void normalize() {
137 |     Scalar length = std::sqrt(unit_complex().x() * unit_complex().x() +
138 |                               unit_complex().y() * unit_complex().y());
139 |     SOPHUS_ENSURE(length >= Constants<Scalar>::epsilon(),
140 |                   "Complex number should not be close to zero!");
141 |     unit_complex_nonconst().x() /= length;
142 |     unit_complex_nonconst().y() /= length;
143 |   }
144 | 
145 |   // Returns 2x2 matrix representation of the instance.
146 |   //
147 |   // For SO(2), the matrix representation is an orthogonal matrix ``R`` with
148 |   // ``det(R)=1``, thus the so-called "rotation matrix".
149 |   //
150 |   SOPHUS_FUNC Transformation matrix() const {
151 |     Scalar const& real = unit_complex().x();
152 |     Scalar const& imag = unit_complex().y();
153 |     Transformation R;
154 |     // clang-format off
155 |     R <<
156 |       real, -imag,
157 |       imag,  real;
158 |     // clang-format on
159 |     return R;
160 |   }
161 | 
162 |   // Assignment operator
163 |   //
164 |   template <class OtherDerived>
165 |   SOPHUS_FUNC SO2Base<Derived>& operator=(SO2Base<OtherDerived> const& other) {
166 |     unit_complex_nonconst() = other.unit_complex();
167 |     return *this;
168 |   }
169 | 
170 |   // Group multiplication, which is rotation concatenation.
171 |   //
172 |   SOPHUS_FUNC SO2<Scalar> operator*(SO2<Scalar> const& other) const {
173 |     SO2<Scalar> result(*this);
174 |     result *= other;
175 |     return result;
176 |   }
177 | 
178 |   // Group action on 3-points.
179 |   //
180 |   // This function rotates a 3 dimensional point ``p`` by the SO3 element
181 |   //  ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``.
182 |   //
183 |   SOPHUS_FUNC Point operator*(Point const& p) const {
184 |     Scalar const& real = unit_complex().x();
185 |     Scalar const& imag = unit_complex().y();
186 |     return Point(real * p[0] - imag * p[1], imag * p[0] + real * p[1]);
187 |   }
188 | 
189 |   // Group action on lines.
190 |   //
191 |   // This function rotates a parametrized line ``l(t) = o + t * d`` by the SO3
192 |   // element:
193 |   //
194 |   // Both direction ``d`` and origin ``o`` are rotated as a 3 dimensional point
195 |   //
196 |   SOPHUS_FUNC Line operator*(Line const& l) const {
197 |     return Line((*this) * l.origin(), (*this) * l.direction());
198 |   }
199 | 
200 |   // In-place group multiplication.
201 |   //
202 |   SOPHUS_FUNC SO2Base<Derived> operator*=(SO2<Scalar> const& other) {
203 |     Scalar lhs_real = unit_complex().x();
204 |     Scalar lhs_imag = unit_complex().y();
205 |     Scalar const& rhs_real = other.unit_complex().x();
206 |     Scalar const& rhs_imag = other.unit_complex().y();
207 |     // complex multiplication
208 |     unit_complex_nonconst().x() = lhs_real * rhs_real - lhs_imag * rhs_imag;
209 |     unit_complex_nonconst().y() = lhs_real * rhs_imag + lhs_imag * rhs_real;
210 | 
211 |     Scalar squared_norm = unit_complex_nonconst().squaredNorm();
212 |     // We can assume that the squared-norm is close to 1 since we deal with a
213 |     // unit complex number. Due to numerical precision issues, there might
214 |     // be a small drift after pose concatenation. Hence, we need to renormalizes
215 |     // the complex number here.
216 |     // Since squared-norm is close to 1, we do not need to calculate the costly
217 |     // square-root, but can use an approximation around 1 (see
218 |     // http://stackoverflow.com/a/12934750 for details).
219 |     if (squared_norm != Scalar(1.0)) {
220 |       unit_complex_nonconst() *= Scalar(2.0) / (Scalar(1.0) + squared_norm);
221 |     }
222 |     return *this;
223 |   }
224 | 
225 |   // Takes in complex number / tuple and normalizes it.
226 |   //
227 |   // Precondition: The complex number must not be close to zero.
228 |   //
229 |   SOPHUS_FUNC void setComplex(Point const& complex) {
230 |     unit_complex_nonconst() = complex;
231 |     normalize();
232 |   }
233 | 
234 |   // Accessor of unit quaternion.
235 |   //
236 |   SOPHUS_FUNC
237 |   ComplexT const& unit_complex() const {
238 |     return static_cast<Derived const*>(this)->unit_complex();
239 |   }
240 | 
241 |   ////////////////////////////////////////////////////////////////////////////
242 |   // public static functions
243 |   ////////////////////////////////////////////////////////////////////////////
244 | 
245 |   // Group exponential
246 |   //
247 |   // This functions takes in an element of tangent space (= rotation angle
248 |   // ``theta``) and returns the corresponding element of the group SO(2).
249 |   //
250 |   // To be more specific, this function computes ``expmat(hat(omega))``
251 |   // with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the
252 |   // hat()-operator of SO(2).
253 |   //
254 |   SOPHUS_FUNC static SO2<Scalar> exp(Tangent const& theta) {
255 |     return SO2<Scalar>(std::cos(theta), std::sin(theta));
256 |   }
257 | 
258 |   // Returns the infinitesimal generators of SO3.
259 |   //
260 |   // The infinitesimal generators of SO(2) is:
261 |   //
262 |   //   |  0  1 |
263 |   //   | -1  0 |
264 |   //
265 |   SOPHUS_FUNC static Transformation generator() { return hat(1); }
266 | 
267 |   // hat-operator
268 |   //
269 |   // It takes in the scalar representation ``theta`` (= rotation angle) and
270 |   // returns the corresponding matrix representation of Lie algebra element.
271 |   //
272 |   // Formally, the ``hat()`` operator of SO(2) is defined as
273 |   //
274 |   //   ``hat(.): R^2 -> R^{2x2},  hat(theta) = theta * G``
275 |   //
276 |   // with ``G`` being the infinitesimal generator of SO(2).
277 |   //
278 |   // The corresponding inverse is the ``vee``-operator, see below.
279 |   //
280 |   SOPHUS_FUNC static Transformation hat(Tangent const& theta) {
281 |     Transformation Omega;
282 |     // clang-format off
283 |     Omega <<
284 |         Scalar(0),   -theta,
285 |             theta, Scalar(0);
286 |     // clang-format on
287 |     return Omega;
288 |   }
289 | 
290 |   // Lie bracket
291 |   //
292 |   // It returns the Lie bracket of SO(2). Since SO(2) is a commutative group,
293 |   // the Lie bracket is simple ``0``.
294 |   //
295 |   SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
296 |     return Scalar(0);
297 |   }
298 | 
299 |   // Logarithmic map
300 |   //
301 |   // Computes the logarithm, the inverse of the group exponential which maps
302 |   // element of the group (rotation matrices) to elements of the tangent space
303 |   // (rotation angles).
304 |   //
305 |   // To be specific, this function computes ``vee(logmat(.))`` with
306 |   // ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
307 |   // of SO(2).
308 |   //
309 |   SOPHUS_FUNC static Tangent log(SO2<Scalar> const& other) {
310 |     using std::atan2;
311 |     return atan2(other.unit_complex_.y(), other.unit_complex().x());
312 |   }
313 | 
314 |   // vee-operator
315 |   //
316 |   // It takes the 2x2-matrix representation ``Omega`` and maps it to the
317 |   // corresponding scalar representation of Lie algebra.
318 |   //
319 |   // This is the inverse of the hat-operator, see above.
320 |   //
321 |   // Precondition: ``Omega`` must have the following structure:
322 |   //
323 |   //                |  0 -a |
324 |   //                |  a  0 |
325 |   //
326 |   SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
327 |     using std::abs;
328 |     return Omega(1, 0);
329 |   }
330 | 
331 |  private:
332 |   // Mutator of unit_complex is private to ensure class invariant. That is
333 |   // the complex number must stay close to unit length.
334 |   //
335 |   SOPHUS_FUNC
336 |   ComplexT& unit_complex_nonconst() {
337 |     return static_cast<Derived*>(this)->unit_complex_nonconst();
338 |   }
339 | };
340 | 
341 | // SO2 default type - Constructors and default storage for SO2 Type
342 | template <class Scalar_, int Options>
343 | class SO2 : public SO2Base<SO2<Scalar_, Options>> {
344 |   using Base = SO2Base<SO2<Scalar_, Options>>;
345 | 
346 |  public:
347 |   using Scalar = Scalar_;
348 |   using Transformation = typename Base::Transformation;
349 |   using Point = typename Base::Point;
350 |   using Tangent = typename Base::Tangent;
351 |   using Adjoint = typename Base::Adjoint;
352 |   using ComplexMember = Vector2<Scalar, Options>;
353 | 
354 |   // ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``.
355 |   friend class SO2Base<SO2<Scalar, Options>>;
356 | 
357 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
358 | 
359 |   // Default constructor initialize unit complex number to identity rotation.
360 |   //
361 |   SOPHUS_FUNC SO2() : unit_complex_(Scalar(1), Scalar(0)) {}
362 | 
363 |   // Copy constructor
364 |   //
365 |   template <class OtherDerived>
366 |   SOPHUS_FUNC SO2(SO2Base<OtherDerived> const& other)
367 |       : unit_complex_(other.unit_complex()) {}
368 | 
369 |   // Constructor from rotation matrix
370 |   //
371 |   // Precondition: rotation matrix need to be orthogonal with determinant of 1.
372 |   //
373 |   SOPHUS_FUNC explicit SO2(Transformation const& R)
374 |       : unit_complex_(Scalar(0.5) * (R(0, 0) + R(1, 1)),
375 |                       Scalar(0.5) * (R(1, 0) - R(0, 1))) {
376 |     SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R);
377 |     SOPHUS_ENSURE(R.determinant() > 0, "det(R) is not positive: %",
378 |                   R.determinant());
379 |   }
380 | 
381 |   // Returns closed SO2 given arbirary 2x2 matrix.
382 |   //
383 |   static SO2 fitToSO2(Transformation const& R) {
384 |     return SO2(makeRotationMatrix(R));
385 |   }
386 | 
387 |   // Constructor from pair of real and imaginary number.
388 |   //
389 |   // Precondition: The pair must not be close to zero.
390 |   //
391 |   SOPHUS_FUNC SO2(Scalar const& real, Scalar const& imag)
392 |       : unit_complex_(real, imag) {
393 |     Base::normalize();
394 |   }
395 | 
396 |   // Constructor from 2-vector.
397 |   //
398 |   // Precondition: The vector must not be close to zero.
399 |   //
400 |   template <class D>
401 |   SOPHUS_FUNC explicit SO2(Eigen::MatrixBase<D> const& complex)
402 |       : unit_complex_(complex) {
403 |     static_assert(std::is_same<typename D::Scalar, Scalar>::value,
404 |                   "must be same Scalar type");
405 |     Base::normalize();
406 |   }
407 | 
408 |   // Draw uniform sample from SO(2) manifold.
409 |   //
410 |   template <class UniformRandomBitGenerator>
411 |   static SO2 sampleUniform(UniformRandomBitGenerator& generator) {
412 |     static_assert(IsUniformRandomBitGenerator<UniformRandomBitGenerator>::value,
413 |                   "generator must meet the UniformRandomBitGenerator concept");
414 |     std::uniform_real_distribution<Scalar> uniform(-Constants<Scalar>::pi(),
415 |                                                    Constants<Scalar>::pi());
416 |     return SO2(uniform(generator));
417 |   }
418 | 
419 |   // Constructor from an rotation angle.
420 |   //
421 |   SOPHUS_FUNC explicit SO2(Scalar theta) {
422 |     unit_complex_nonconst() = SO2<Scalar>::exp(theta).unit_complex();
423 |   }
424 | 
425 |   // Accessor of unit complex number
426 |   //
427 |   SOPHUS_FUNC ComplexMember const& unit_complex() const {
428 |     return unit_complex_;
429 |   }
430 | 
431 |  protected:
432 |   // Mutator of complex number is protected to ensure class invariant.
433 |   //
434 |   SOPHUS_FUNC ComplexMember& unit_complex_nonconst() { return unit_complex_; }
435 | 
436 |   ComplexMember unit_complex_;
437 | };
438 | 
439 | }  // namespace Sophus
440 | 
441 | namespace Eigen {
442 | 
443 | // Specialization of Eigen::Map for ``SO2``.
444 | //
445 | // Allows us to wrap SO2 objects around POD array (e.g. external c style
446 | // complex number / tuple).
447 | template <class Scalar_, int Options>
448 | class Map<Sophus::SO2<Scalar_>, Options>
449 |     : public Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>> {
450 |   using Base = Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>>;
451 | 
452 |  public:
453 |   using Scalar = Scalar_;
454 | 
455 |   using Transformation = typename Base::Transformation;
456 |   using Point = typename Base::Point;
457 |   using Tangent = typename Base::Tangent;
458 |   using Adjoint = typename Base::Adjoint;
459 | 
460 |   // ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``.
461 |   friend class Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>>;
462 | 
463 |   EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
464 |   using Base::operator*=;
465 |   using Base::operator*;
466 | 
467 |   SOPHUS_FUNC
468 |   Map(Scalar* coeffs) : unit_complex_(coeffs) {}
469 | 
470 |   // Accessor of unit complex number.
471 |   //
472 |   SOPHUS_FUNC
473 |   Map<Sophus::Vector2<Scalar>, Options> const& unit_complex() const {
474 |     return unit_complex_;
475 |   }
476 | 
477 |  protected:
478 |   // Mutator of unit_complex is protected to ensure class invariant.
479 |   //
480 |   SOPHUS_FUNC
481 |   Map<Sophus::Vector2<Scalar>, Options>& unit_complex_nonconst() {
482 |     return unit_complex_;
483 |   }
484 | 
485 |   Map<Matrix<Scalar, 2, 1>, Options> unit_complex_;
486 | };
487 | 
488 | // Specialization of Eigen::Map for ``SO2 const``.
489 | //
490 | // Allows us to wrap SO2 objects around POD array (e.g. external c style
491 | // complex number / tuple).
492 | template <class Scalar_, int Options>
493 | class Map<Sophus::SO2<Scalar_> const, Options>
494 |     : public Sophus::SO2Base<Map<Sophus::SO2<Scalar_> const, Options>> {
495 |   using Base = Sophus::SO2Base<Map<Sophus::SO2<Scalar_> const, Options>>;
496 | 
497 |  public:
498 |   using Scalar = Scalar_;
499 |   using Transformation = typename Base::Transformation;
500 |   using Point = typename Base::Point;
501 |   using Tangent = typename Base::Tangent;
502 |   using Adjoint = typename Base::Adjoint;
503 | 
504 |   EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
505 |   using Base::operator*=;
506 |   using Base::operator*;
507 | 
508 |   SOPHUS_FUNC Map(Scalar const* coeffs) : unit_complex_(coeffs) {}
509 | 
510 |   // Accessor of unit complex number.
511 |   //
512 |   SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& unit_complex()
513 |       const {
514 |     return unit_complex_;
515 |   }
516 | 
517 |  protected:
518 |   // Mutator of unit_complex is protected to ensure class invariant.
519 |   //
520 |   Map<Matrix<Scalar, 2, 1> const, Options> const unit_complex_;
521 | };
522 | }
523 | 
524 | #endif  // SOPHUS_SO2_HPP
525 | 


--------------------------------------------------------------------------------
/sophus/sophus.hpp:
--------------------------------------------------------------------------------
 1 | // This file is part of Sophus.
 2 | //
 3 | // Copyright 2013 Hauke Strasdat
 4 | //
 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | // of this software and associated documentation files (the "Software"), to
 7 | // deal in the Software without restriction, including without limitation the
 8 | // rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
 9 | // sell copies of the Software, and to permit persons to whom the Software is
10 | // furnished to do so, subject to the following conditions:
11 | //
12 | // The above copyright notice and this permission notice shall be included in
13 | // all copies or substantial portions of the Software.
14 | //
15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
21 | // IN THE SOFTWARE.
22 | 
23 | #ifndef SOPHUS_HPP
24 | #define SOPHUS_HPP
25 | 
26 | #include <stdexcept>
27 | 
28 | #include <Eigen/Eigen>
29 | #include <Eigen/Geometry>
30 | 
31 | namespace Sophus {
32 | using namespace Eigen;
33 | 
34 | template<typename Scalar>
35 | struct SophusConstants {
36 |   EIGEN_ALWAYS_INLINE static
37 |   const Scalar epsilon() {
38 |     return static_cast<Scalar>(1e-10);
39 |   }
40 | 
41 |   EIGEN_ALWAYS_INLINE static
42 |   const Scalar pi() {
43 |     return static_cast<Scalar>(M_PI);
44 |   }
45 | };
46 | 
47 | template<>
48 | struct SophusConstants<float> {
49 |   EIGEN_ALWAYS_INLINE static
50 |   const float epsilon() {
51 |     return static_cast<float>(1e-5);
52 |   }
53 | 
54 |   EIGEN_ALWAYS_INLINE static
55 |   const float pi() {
56 |     return static_cast<float>(M_PI);
57 |   }
58 | };
59 | 
60 | class SophusException : public std::runtime_error {
61 | public:
62 |   SophusException (const std::string& str)
63 |     : runtime_error("Sophus exception: " + str) {
64 |   }
65 | };
66 | 
67 | }
68 | 
69 | #endif
70 | 


--------------------------------------------------------------------------------
/sophus/test_macros.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef SOPUHS_TESTS_MACROS_HPP
  2 | #define SOPUHS_TESTS_MACROS_HPP
  3 | 
  4 | #include <sophus/types.hpp>
  5 | 
  6 | namespace Sophus {
  7 | namespace details {
  8 | 
  9 | template <class Scalar>
 10 | class Pretty {
 11 |  public:
 12 |   static std::string impl(Scalar s) { return FormatString("%", s); }
 13 | };
 14 | 
 15 | template <class Scalar, int M, int N>
 16 | class Pretty<Eigen::Matrix<Scalar, M, N>> {
 17 |  public:
 18 |   static std::string impl(Matrix<Scalar, M, N> const& v) {
 19 |     return FormatString("\n%\n", v);
 20 |   }
 21 | };
 22 | 
 23 | template <class T>
 24 | std::string pretty(T const& v) {
 25 |   return Pretty<T>::impl(v);
 26 | }
 27 | 
 28 | template <class... Args>
 29 | void testFailed(bool& passed, char const* func, char const* file, int line,
 30 |                 std::string const& msg) {
 31 |   std::cerr << FormatString("Test failed in function %, file %, line %\n", func,
 32 |                             file, line);
 33 |   std::cerr << msg << "\n\n";
 34 |   passed = false;
 35 | }
 36 | }  // namespace details
 37 | 
 38 | void processTestResult(bool passed) {
 39 |   if (!passed) {
 40 |     std::cerr << "failed!" << std::endl << std::endl;
 41 |     exit(-1);
 42 |   }
 43 |   std::cerr << "passed." << std::endl << std::endl;
 44 | }
 45 | }  // namespace Sophus
 46 | 
 47 | #define SOPHUS_STRINGIFY(x) #x
 48 | 
 49 | // GenericTests whether condition is true.
 50 | // The in-out parameter passed will be set to false if test fails.
 51 | #define SOPHUS_TEST(passed, condition, ...)                                    \
 52 |   do {                                                                         \
 53 |     if (!(condition)) {                                                        \
 54 |       std::string msg = Sophus::details::FormatString(                         \
 55 |           "condition ``%`` is false\n", SOPHUS_STRINGIFY(condition));          \
 56 |       msg += Sophus::details::FormatString(__VA_ARGS__);                       \
 57 |       Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
 58 |                                   msg);                                        \
 59 |     }                                                                          \
 60 |   } while (false)
 61 | 
 62 | // GenericTests whether left is equal to right given a threshold.
 63 | // The in-out parameter passed will be set to false if test fails.
 64 | #define SOPHUS_TEST_EQUAL(passed, left, right, ...)                            \
 65 |   do {                                                                         \
 66 |     if (left != right) {                                                       \
 67 |       std::string msg = Sophus::details::FormatString(                         \
 68 |           "% (=%) is not equal to % (=%)\n", SOPHUS_STRINGIFY(left),           \
 69 |           Sophus::details::pretty(left), SOPHUS_STRINGIFY(right),              \
 70 |           Sophus::details::pretty(right));                                     \
 71 |       msg += Sophus::details::FormatString(__VA_ARGS__);                       \
 72 |       Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
 73 |                                   msg);                                        \
 74 |     }                                                                          \
 75 |   } while (false)
 76 | 
 77 | // GenericTests whether left is equal to right given a threshold.
 78 | // The in-out parameter passed will be set to false if test fails.
 79 | #define SOPHUS_TEST_NEQ(passed, left, right, ...)                              \
 80 |   do {                                                                         \
 81 |     if (left == right) {                                                       \
 82 |       std::string msg = Sophus::details::FormatString(                         \
 83 |           "% (=%) shoudl not be equal to % (=%)\n", SOPHUS_STRINGIFY(left),    \
 84 |           Sophus::details::pretty(left), SOPHUS_STRINGIFY(right),              \
 85 |           Sophus::details::pretty(right));                                     \
 86 |       msg += Sophus::details::FormatString(__VA_ARGS__);                       \
 87 |       Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
 88 |                                   msg);                                        \
 89 |     }                                                                          \
 90 |   } while (false)
 91 | 
 92 | // GenericTests whether left is approximatly equal to right given a threshold.
 93 | // The in-out parameter passed will be set to false if test fails.
 94 | #define SOPHUS_TEST_APPROX(passed, left, right, thr, ...)                      \
 95 |   do {                                                                         \
 96 |     auto nrm = Sophus::metric((left), (right));                                \
 97 |     if (!(nrm < (thr))) {                                                      \
 98 |       std::string msg = Sophus::details::FormatString(                         \
 99 |           "% (=%) is not approx % (=%); % is %; nrm is %\n",                   \
100 |           SOPHUS_STRINGIFY(left), Sophus::details::pretty(left),               \
101 |           SOPHUS_STRINGIFY(right), Sophus::details::pretty(right),             \
102 |           SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm);           \
103 |       msg += Sophus::details::FormatString(__VA_ARGS__);                       \
104 |       Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
105 |                                   msg);                                        \
106 |     }                                                                          \
107 |   } while (false)
108 | 
109 | // GenericTests whether left is NOT approximatly equal to right given a
110 | // threshold.
111 | // The in-out parameter passed will be set to false if test fails.
112 | #define SOPHUS_TEST_NOT_APPROX(passed, left, right, thr, ...)                  \
113 |   do {                                                                         \
114 |     auto nrm = Sophus::metric((left), (right));                                \
115 |     if (nrm < (thr)) {                                                         \
116 |       std::string msg = Sophus::details::FormatString(                         \
117 |           "% (=%) is approx % (=%), but it should not!\n % is %; nrm is %\n",  \
118 |           SOPHUS_STRINGIFY(left), Sophus::details::pretty(left),               \
119 |           SOPHUS_STRINGIFY(right), Sophus::details::pretty(right),             \
120 |           SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm);           \
121 |       msg += Sophus::details::FormatString(__VA_ARGS__);                       \
122 |       Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
123 |                                   msg);                                        \
124 |     }                                                                          \
125 |   } while (false)
126 | 
127 | #endif  // SOPUHS_TESTS_MACROS_HPP
128 | 


--------------------------------------------------------------------------------
/sophus/tests.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef SOPUHS_TESTS_HPP
  2 | #define SOPUHS_TESTS_HPP
  3 | 
  4 | #include <vector>
  5 | #include <unsupported/Eigen/MatrixFunctions>
  6 | 
  7 | #include "sophus.hpp"
  8 | 
  9 | namespace Sophus {
 10 | 
 11 | using namespace std;
 12 | using namespace Eigen;
 13 | 
 14 | template <class LieGroup>
 15 | class Tests {
 16 | 
 17 | public:
 18 |   typedef typename LieGroup::Scalar Scalar;
 19 |   typedef typename LieGroup::Transformation Transformation;
 20 |   typedef typename LieGroup::Tangent Tangent;
 21 |   typedef typename LieGroup::Point Point;
 22 |   typedef typename LieGroup::Adjoint Adjoint;
 23 |   static const int N = LieGroup::N;
 24 |   static const int DoF = LieGroup::DoF;
 25 | 
 26 |   const Scalar SMALL_EPS;
 27 | 
 28 |   Tests() : SMALL_EPS(SophusConstants<Scalar>::epsilon()) {
 29 |   }
 30 | 
 31 |   void setGroupElements(const vector<LieGroup> & group_vec) {
 32 |     group_vec_  = group_vec;
 33 |   }
 34 | 
 35 |   void setTangentVectors(const vector<Tangent> & tangent_vec) {
 36 |     tangent_vec_  = tangent_vec;
 37 |   }
 38 | 
 39 |   void setPoints(const vector<Point> & point_vec) {
 40 |     point_vec_  = point_vec;
 41 |   }
 42 | 
 43 |   bool adjointTest() {
 44 |     bool passed = true;
 45 |     for (size_t i=0; i<group_vec_.size(); ++i) {
 46 |       Transformation T = group_vec_[i].matrix();
 47 |       Adjoint Ad = group_vec_[i].Adj();
 48 |       for (size_t j=0; j<tangent_vec_.size(); ++j) {
 49 |         Tangent x = tangent_vec_[j];
 50 | 
 51 |         Transformation I;
 52 |         I.setIdentity();
 53 |         Tangent ad1 = Ad*x;
 54 |         Tangent ad2 = LieGroup::vee(T*LieGroup::hat(x)
 55 |                                     *group_vec_[i].inverse().matrix());
 56 |         Scalar nrm = norm(ad1-ad2);
 57 | 
 58 |         if (isnan(nrm) || nrm>20.*SMALL_EPS) {
 59 |           cerr << "Adjoint" << endl;
 60 |           cerr  << "Test case: " << i << "," << j <<endl;
 61 |           cerr << (ad1-ad2) <<endl;
 62 |           cerr << endl;
 63 |           passed = false;
 64 |         }
 65 |       }
 66 |     }
 67 |     return passed;
 68 |   }
 69 | 
 70 |   bool expLogTest() {
 71 |     bool passed = true;
 72 | 
 73 |     for (size_t i=0; i<group_vec_.size(); ++i) {
 74 |       Transformation T1 = group_vec_[i].matrix();
 75 |       Transformation T2 = LieGroup::exp(group_vec_[i].log()).matrix();
 76 |       Transformation DiffT = T1-T2;
 77 |       Scalar nrm = DiffT.norm();
 78 | 
 79 |       if (isnan(nrm) || nrm>SMALL_EPS) {
 80 |         cerr << "G - exp(log(G))" << endl;
 81 |         cerr  << "Test case: " << i << endl;
 82 |         cerr << DiffT <<endl;
 83 |         cerr << endl;
 84 |         passed = false;
 85 |       }
 86 |     }
 87 |     return passed;
 88 |   }
 89 | 
 90 |   bool expMapTest() {
 91 |     bool passed = true;
 92 |     for (size_t i=0; i<tangent_vec_.size(); ++i) {
 93 | 
 94 |       Tangent omega = tangent_vec_[i];
 95 |       Transformation exp_x = LieGroup::exp(omega).matrix();
 96 |       Transformation expmap_hat_x = (LieGroup::hat(omega)).exp();
 97 |       Transformation DiffR = exp_x-expmap_hat_x;
 98 |       Scalar nrm = DiffR.norm();
 99 | 
100 |       if (isnan(nrm) || nrm>10.*SMALL_EPS) {
101 |         cerr << "expmap(hat(x)) - exp(x)" << endl;
102 |         cerr  << "Test case: " << i << endl;
103 |         cerr << exp_x <<endl;
104 |         cerr << expmap_hat_x <<endl;
105 |         cerr << DiffR <<endl;
106 |         cerr << endl;
107 |         passed = false;
108 |       }
109 |     }
110 |     return passed;
111 |   }
112 | 
113 |   bool groupActionTest() {
114 |     bool passed = true;
115 | 
116 |     for (size_t i=0; i<group_vec_.size(); ++i) {
117 |       for (size_t j=0; j<point_vec_.size(); ++j) {
118 |         const Point & p = point_vec_[j];
119 |         Transformation T = group_vec_[i].matrix();
120 |         Point res1 = group_vec_[i]*p;
121 |         Point res2 = map(T, p);
122 |         Scalar nrm = (res1-res2).norm();
123 |         if (isnan(nrm) || nrm>SMALL_EPS) {
124 |           cerr << "Transform vector" << endl;
125 |           cerr  << "Test case: " << i << endl;
126 |           cerr << (res1-res2) <<endl;
127 |           cerr << endl;
128 |           passed = false;
129 |         }
130 |       }
131 |     }
132 |     return passed;
133 |   }
134 | 
135 | 
136 |   bool lieBracketTest() {
137 |     bool passed = true;
138 |     for (size_t i=0; i<tangent_vec_.size(); ++i) {
139 |       for (size_t j=0; j<tangent_vec_.size(); ++j) {
140 |         Tangent res1 = LieGroup::lieBracket(tangent_vec_[i],tangent_vec_[j]);
141 |         Transformation hati = LieGroup::hat(tangent_vec_[i]);
142 |         Transformation hatj = LieGroup::hat(tangent_vec_[j]);
143 | 
144 |         Tangent res2 = LieGroup::vee(hati*hatj-hatj*hati);
145 |         Tangent resDiff = res1-res2;
146 |         if (norm(resDiff)>SMALL_EPS) {
147 |           cerr << "Lie Bracket Test" << endl;
148 |           cerr  << "Test case: " << i << ", " <<j<< endl;
149 |           cerr << resDiff << endl;
150 |           cerr << endl;
151 |           passed = false;
152 |         }
153 |       }
154 |     }
155 |     return passed;
156 |   }
157 | 
158 |   bool mapAndMultTest() {
159 |     bool passed = true;
160 |     for (size_t i=0; i<group_vec_.size(); ++i) {
161 |       for (size_t j=0; j<group_vec_.size(); ++j) {
162 |         Transformation mul_resmat = (group_vec_[i]*group_vec_[j]).matrix();
163 |         Scalar fastmul_res_raw[LieGroup::num_parameters];
164 |         Eigen::Map<LieGroup> fastmul_res(fastmul_res_raw);
165 |         Eigen::Map<const LieGroup> group_j_constmap(group_vec_[j].data());
166 |         fastmul_res = group_vec_[i];
167 |         fastmul_res.fastMultiply(group_j_constmap);
168 |         Transformation diff =  mul_resmat-fastmul_res.matrix();
169 |         Scalar nrm = diff.norm();
170 |         if (isnan(nrm) || nrm>SMALL_EPS) {
171 |           cerr << "Map & Multiply" << endl;
172 |           cerr  << "Test case: " << i  << "," << j << endl;
173 |           cerr << diff <<endl;
174 |           cerr << endl;
175 |           passed = false;
176 |         }
177 |       }
178 |     }
179 |     return passed;
180 |   }
181 | 
182 |   bool veeHatTest() {
183 |     bool passed = true;
184 |     for (size_t i=0; i<tangent_vec_.size(); ++i) {
185 |       Tangent resDiff
186 |           = tangent_vec_[i] - LieGroup::vee(LieGroup::hat(tangent_vec_[i]));
187 |       if (norm(resDiff)>SMALL_EPS) {
188 |         cerr << "Hat-vee Test" << endl;
189 |         cerr  << "Test case: " << i <<  endl;
190 |         cerr << resDiff << endl;
191 |         cerr << endl;
192 |         passed = false;
193 |       }
194 |     }
195 |     return passed;
196 |   }
197 | 
198 | 
199 | 
200 |   void runAllTests() {
201 |     bool passed = adjointTest();
202 |     if (!passed) {
203 |       cerr << "failed!" << endl << endl;
204 |       exit(-1);
205 |     }
206 |     passed = expLogTest();
207 |     if (!passed) {
208 |       cerr << "failed!" << endl << endl;
209 |       exit(-1);
210 |     }
211 |     passed = expMapTest();
212 |     if (!passed) {
213 |       cerr << "failed!" << endl << endl;
214 |       exit(-1);
215 |     }
216 |     passed = groupActionTest();
217 |     if (!passed) {
218 |       cerr << "failed!" << endl << endl;
219 |       exit(-1);
220 |     }
221 |     passed = lieBracketTest();
222 |     if (!passed) {
223 |       cerr << "failed!" << endl << endl;
224 |       exit(-1);
225 |     }
226 |     passed = mapAndMultTest();
227 |     if (!passed) {
228 |       cerr << "failed!" << endl << endl;
229 |       exit(-1);
230 |     }
231 |     passed = veeHatTest();
232 |     if (!passed) {
233 |       cerr << "failed!" << endl << endl;
234 |       exit(-1);
235 |     }
236 |     cerr << "passed." << endl << endl;
237 |   }
238 | 
239 | private:
240 |   Matrix<Scalar,N-1,1> map(const Matrix<Scalar,N,N> & T,
241 |                            const Matrix<Scalar,N-1,1> & p) {
242 |     return T.template topLeftCorner<N-1,N-1>()*p
243 |         + T.template topRightCorner<N-1,1>();
244 |   }
245 | 
246 |   Matrix<Scalar,N,1> map(const Matrix<Scalar,N,N> & T,
247 |                          const Matrix<Scalar,N,1> & p) {
248 |     return T*p;
249 |   }
250 | 
251 |   Scalar norm(const Scalar & v) {
252 |     return std::abs(v);
253 |   }
254 | 
255 |   Scalar norm(const Matrix<Scalar,DoF,1> & T) {
256 |     return T.norm();
257 |   }
258 | 
259 |   std::vector<LieGroup> group_vec_;
260 |   std::vector<Tangent> tangent_vec_;
261 |   std::vector<Point> point_vec_;
262 | };
263 | }
264 | #endif // TESTS_HPP
265 | 


--------------------------------------------------------------------------------
/sophus/types.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef SOPHUS_TYEPES_HPP
  2 | #define SOPHUS_TYEPES_HPP
  3 | 
  4 | #include "common.hpp"
  5 | 
  6 | namespace Sophus {
  7 | 
  8 | template <class Scalar, int M, int Options = 0>
  9 | using Vector = Eigen::Matrix<Scalar, M, 1, Options>;
 10 | 
 11 | template <class Scalar, int Options = 0>
 12 | using Vector2 = Vector<Scalar, 2, Options>;
 13 | using Vector2f = Vector2<float>;
 14 | using Vector2d = Vector2<double>;
 15 | 
 16 | template <class Scalar, int Options = 0>
 17 | using Vector3 = Vector<Scalar, 3, Options>;
 18 | using Vector3f = Vector3<float>;
 19 | using Vector3d = Vector3<double>;
 20 | 
 21 | template <class Scalar>
 22 | using Vector4 = Vector<Scalar, 4>;
 23 | using Vector4f = Vector4<float>;
 24 | using Vector4d = Vector4<double>;
 25 | 
 26 | template <class Scalar>
 27 | using Vector6 = Vector<Scalar, 6>;
 28 | using Vector6f = Vector6<float>;
 29 | using Vector6d = Vector6<double>;
 30 | 
 31 | template <class Scalar>
 32 | using Vector7 = Vector<Scalar, 7>;
 33 | using Vector7f = Vector7<float>;
 34 | using Vector7d = Vector7<double>;
 35 | 
 36 | template <class Scalar, int M, int N>
 37 | using Matrix = Eigen::Matrix<Scalar, M, N>;
 38 | 
 39 | template <class Scalar>
 40 | using Matrix2 = Matrix<Scalar, 2, 2>;
 41 | using Matrix2f = Matrix2<float>;
 42 | using Matrix2d = Matrix2<double>;
 43 | 
 44 | template <class Scalar>
 45 | using Matrix3 = Matrix<Scalar, 3, 3>;
 46 | using Matrix3f = Matrix3<float>;
 47 | using Matrix3d = Matrix3<double>;
 48 | 
 49 | template <class Scalar>
 50 | using Matrix4 = Matrix<Scalar, 4, 4>;
 51 | using Matrix4f = Matrix4<float>;
 52 | using Matrix4d = Matrix4<double>;
 53 | 
 54 | template <class Scalar>
 55 | using Matrix6 = Matrix<Scalar, 6, 6>;
 56 | using Matrix6f = Matrix6<float>;
 57 | using Matrix6d = Matrix6<double>;
 58 | 
 59 | template <class Scalar>
 60 | using Matrix7 = Matrix<Scalar, 7, 7>;
 61 | using Matrix7f = Matrix7<float>;
 62 | using Matrix7d = Matrix7<double>;
 63 | 
 64 | template <class Scalar, int N, int Options = 0>
 65 | using ParametrizedLine = Eigen::ParametrizedLine<Scalar, N, Options>;
 66 | 
 67 | template <class Scalar, int Options = 0>
 68 | using ParametrizedLine3 = ParametrizedLine<Scalar, 3, Options>;
 69 | using ParametrizedLine3f = ParametrizedLine3<float>;
 70 | using ParametrizedLine3d = ParametrizedLine3<double>;
 71 | 
 72 | template <class Scalar, int Options = 0>
 73 | using ParametrizedLine2 = ParametrizedLine<Scalar, 2, Options>;
 74 | using ParametrizedLine2f = ParametrizedLine2<float>;
 75 | using ParametrizedLine2d = ParametrizedLine2<double>;
 76 | 
 77 | namespace details {
 78 | template <class Scalar>
 79 | class Metric {
 80 |  public:
 81 |   static Scalar impl(Scalar s0, Scalar s1) {
 82 |     using std::abs;
 83 |     return abs(s0 - s1);
 84 |   }
 85 | };
 86 | 
 87 | template <class Scalar, int M, int N>
 88 | class Metric<Matrix<Scalar, M, N>> {
 89 |  public:
 90 |   static Scalar impl(Matrix<Scalar, M, N> const& p0,
 91 |                      Matrix<Scalar, M, N> const& p1) {
 92 |     return (p0 - p1).norm();
 93 |   }
 94 | };
 95 | 
 96 | template <typename Scalar>
 97 | class SetToZero {
 98 |  public:
 99 |   static void impl(Scalar& s) { s = Scalar(0); }
100 | };
101 | 
102 | template <typename Scalar, int M, int N>
103 | class SetToZero<Matrix<Scalar, M, N>> {
104 |  public:
105 |   static void impl(Matrix<Scalar, M, N>& v) { v.setZero(); }
106 | };
107 | 
108 | template <typename Scalar>
109 | class SquaredNorm {
110 |  public:
111 |   static Scalar impl(Scalar const& s) { return s * s; }
112 | };
113 | 
114 | template <typename Scalar, int N>
115 | class SquaredNorm<Matrix<Scalar, N, 1>> {
116 |  public:
117 |   static Scalar impl(Matrix<Scalar, N, 1> const& s) { return s.squaredNorm(); }
118 | };
119 | 
120 | template <typename Scalar>
121 | class Transpose {
122 |  public:
123 |   static Scalar impl(Scalar const& s) { return s; }
124 | };
125 | 
126 | template <typename Scalar, int M, int N>
127 | class Transpose<Matrix<Scalar, M, N>> {
128 |  public:
129 |   static Matrix<Scalar, M, N> impl(Matrix<Scalar, M, N> const& s) {
130 |     return s.transpose();
131 |   }
132 | };
133 | }  // namespace details
134 | 
135 | // Returns Euclidiean metric between two points ``p0`` and ``p1``, with ``p``
136 | // being a matrix or a scalar.
137 | //
138 | template <class T>
139 | auto metric(T const& p0, T const& p1)
140 |     -> decltype(details::Metric<T>::impl(p0, p1)) {
141 |   return details::Metric<T>::impl(p0, p1);
142 | }
143 | 
144 | // Sets point ``p`` to zero, with ``p`` being a matrix or a scalar.
145 | //
146 | template <class T>
147 | auto setToZero(T& p) -> decltype(details::SetToZero<T>::impl(p)) {
148 |   return details::SetToZero<T>::impl(p);
149 | }
150 | 
151 | // Returns the squared 2-norm of ``p``, with ``p`` being a vector or a scalar.
152 | //
153 | template <class T>
154 | auto squaredNorm(T const& p) -> decltype(details::SquaredNorm<T>::impl(p)) {
155 |   return details::SquaredNorm<T>::impl(p);
156 | }
157 | 
158 | // Returns ``p.transpose()`` if ``p`` is a matrix, and simply ``p`` if m is a
159 | // scalar.
160 | //
161 | template <typename T>
162 | auto transpose(T const& p) -> decltype(details::Transpose<T>::impl(T())) {
163 |   return details::Transpose<T>::impl(p);
164 | }
165 | 
166 | template <class Scalar>
167 | struct IsFloatingPoint {
168 |   static bool const value = std::is_floating_point<Scalar>::value;
169 | };
170 | 
171 | template <class Scalar, int M, int N>
172 | struct IsFloatingPoint<Matrix<Scalar, M, N>> {
173 |   static bool const value = std::is_floating_point<Scalar>::value;
174 | };
175 | 
176 | template <class Scalar_>
177 | struct GetScalar {
178 |   using Scalar = Scalar_;
179 | };
180 | 
181 | template <class Scalar_, int M, int N>
182 | struct GetScalar<Matrix<Scalar_, M, N>> {
183 |   using Scalar = Scalar_;
184 | };
185 | 
186 | // Planes in 3d are hyperplanes.
187 | template <class T>
188 | using Plane3 = Eigen::Hyperplane<T, 3>;
189 | using Plane3d = Plane3<double>;
190 | using Plane3f = Plane3<float>;
191 | 
192 | // Lines in 2d are hyperplanes.
193 | template <class T>
194 | using Line2 = Eigen::Hyperplane<T, 2>;
195 | using Line2d = Line2<double>;
196 | using Line2f = Line2<float>;
197 | 
198 | }  // namespace Sophus
199 | 
200 | #endif  // SOPHUS_TYEPES_HPP
201 | 


--------------------------------------------------------------------------------
/sophus/velocities.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef SOPHUS_VELOCITIES_HPP
 2 | #define SOPHUS_VELOCITIES_HPP
 3 | 
 4 | #include <functional>
 5 | 
 6 | #include "num_diff.hpp"
 7 | #include "se3.hpp"
 8 | 
 9 | namespace Sophus {
10 | namespace experimental {
11 | // Experimental since the API will certainly change drastically in the future.
12 | 
13 | // Transforms velocity vector by rotation ``foo_R_bar``.
14 | //
15 | // Note: vel_bar can be either a linear or a rotational velocity vector.
16 | //
17 | template <class Scalar>
18 | Vector3<Scalar> transformVelocity(SO3<Scalar> const& foo_R_bar,
19 |                                   Vector3<Scalar> const& vel_bar) {
20 |   // For rotational velocities note that:
21 |   //
22 |   //   vel_bar = vee(foo_R_bar * hat(vel_bar) * foo_R_bar^T)
23 |   //           = vee(hat(Adj(foo_R_bar) * vel_bar))
24 |   //           = Adj(foo_R_bar) * vel_bar
25 |   //           = foo_R_bar * vel_bar.
26 |   //
27 |   return foo_R_bar * vel_bar;
28 | }
29 | 
30 | // Transforms velocity vector by pose ``foo_T_bar``.
31 | //
32 | // Note: vel_bar can be either a linear or a rotational velocity vector.
33 | //
34 | template <class Scalar>
35 | Vector3<Scalar> transformVelocity(SE3<Scalar> const& foo_T_bar,
36 |                                   Vector3<Scalar> const& vel_bar) {
37 |   return transformVelocity(foo_T_bar.so3(), vel_bar);
38 | }
39 | 
40 | // finite difference approximation of instantanious velocity in frame foo
41 | //
42 | template <class Scalar>
43 | Vector3<Scalar> finiteDifferenceRotationalVelocity(
44 |     std::function<SO3<Scalar>(Scalar)> const& foo_R_bar, Scalar t,
45 |     Scalar h = Constants<Scalar>::epsilon()) {
46 |   // https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor
47 |   //
48 |   // W = dR(t)/dt * R^{-1}(t)
49 |   Matrix3<Scalar> dR_dt_in_frame_foo = NumDiff<Scalar>::curve(
50 |       [&foo_R_bar](Scalar t0) -> Matrix3<Scalar> {
51 |         return foo_R_bar(t0).matrix();
52 |       },
53 |       t, h);
54 |   // velocity tensor
55 |   Matrix3<Scalar> W_in_frame_foo =
56 |       dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix();
57 |   return SO3<Scalar>::vee(W_in_frame_foo);
58 | }
59 | 
60 | // finite difference approximation of instantanious velocity in frame foo
61 | //
62 | template <class Scalar>
63 | Vector3<Scalar> finiteDifferenceRotationalVelocity(
64 |     std::function<SE3<Scalar>(Scalar)> const& foo_T_bar, Scalar t,
65 |     Scalar h = Constants<Scalar>::epsilon()) {
66 |   return finiteDifferenceRotationalVelocity<Scalar>(
67 |       [&foo_T_bar](Scalar t) -> SO3<Scalar> { return foo_T_bar(t).so3(); }, t,
68 |       h);
69 | }
70 | 
71 | }  // namespace experimental
72 | }  // namespace Sophus
73 | 
74 | #endif  // SOPHUS_VELOCITIES_HPP
75 | 


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