├── __init__.py ├── LICENSE ├── .gitignore ├── Readme.md ├── optimizer.py └── calibration.py /__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Photogrammetry & Robotics Bonn, University of Bonn 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 deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | 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 all 13 | 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 FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | env/ 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | downloads/ 16 | eggs/ 17 | .eggs/ 18 | lib/ 19 | lib64/ 20 | parts/ 21 | sdist/ 22 | var/ 23 | wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | 49 | # Translations 50 | *.mo 51 | *.pot 52 | 53 | # Django stuff: 54 | *.log 55 | local_settings.py 56 | 57 | # Flask stuff: 58 | instance/ 59 | .webassets-cache 60 | 61 | # Scrapy stuff: 62 | .scrapy 63 | 64 | # Sphinx documentation 65 | docs/_build/ 66 | 67 | # PyBuilder 68 | target/ 69 | 70 | # Jupyter Notebook 71 | .ipynb_checkpoints 72 | 73 | # pyenv 74 | .python-version 75 | 76 | # celery beat schedule file 77 | celerybeat-schedule 78 | 79 | # SageMath parsed files 80 | *.sage.py 81 | 82 | # dotenv 83 | .env 84 | 85 | # virtualenv 86 | .venv 87 | venv/ 88 | ENV/ 89 | 90 | # Spyder project settings 91 | .spyderproject 92 | .spyproject 93 | 94 | # Rope project settings 95 | .ropeproject 96 | 97 | # mkdocs documentation 98 | /site 99 | 100 | # mypy 101 | .mypy_cache/ 102 | -------------------------------------------------------------------------------- /Readme.md: -------------------------------------------------------------------------------- 1 | > [!IMPORTANT] 2 | > **Repository Archived** 3 | > This repository has been archived and is no longer actively maintained. You are welcome to explore the code, but please note that no further updates, issues, or pull requests will be accepted. 4 | > 5 | > Thank you for your interest and contributions. 6 | 7 | Motion Based Multi-Sensor Extrinsic Calibration 8 | ------------ 9 | These python scripts support multi-sensor extrinsic calibration using only odmetry/motion infomation. 10 | The main feature is a non-linear least squares optimization based on Gauss-Helmert framework, as described in the following papers: 11 | 12 | Kaihong Huang and Cyrill Stachniss, 13 | Extrinsic Multi-Sensor Calibration For Mobile Robots Using the Gauss-Helmert Model, 14 | In Proc. of the IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), 2017 15 | 16 | 17 | To run the script, you need to install *cppad* (and its python wrapper *pycppad* ) for automatic differentiation. 18 | ### Installing cppad and pycppad### 19 | First install cppad. For Ubuntu 16.04 users, 20 | ``` 21 | sudo apt-get install cppad 22 | ``` 23 | For Ubuntu 14.04, complie and install it from source code 24 | ``` 25 | git clone https://github.com/coin-or/CppAD.git 26 | cd CppAD/ 27 | mkdir build && cd build 28 | cmake .. 29 | sudo make install 30 | ``` 31 | Next is pycppad 32 | ``` 33 | git clone https://github.com/b45ch1/pycppad.git 34 | cd pycppad/ 35 | gedit setup.py 36 | ``` 37 | change *cppad_include_dir* to *['/usr/include']* in the file setup.py then 38 | ``` 39 | python setup.py build_ext --inplace --debug --undef NDEBUG 40 | pip install . 41 | python ./test_example.py 42 | ``` 43 | ### How to use ### 44 | Please check *demo_and_test* function in calibration.py file for example usage. 45 | -------------------------------------------------------------------------------- /optimizer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Tue Nov 28 16:53:59 2017 5 | 6 | @author: kaihong 7 | """ 8 | import numpy as np 9 | 10 | def huber_w(y): 11 | return np.fmin(1, 1/np.abs(y)) 12 | 13 | def huber_w_smooth(y): 14 | return 1/np.sqrt(1 + y**2) 15 | 16 | def exp_w(c=2): 17 | def _exp_w(y): 18 | return np.exp(-0.5*y**2/c**2) 19 | return _exp_w 20 | 21 | def SolveGaussHelmertProblem(g, x, lm, Cov_ll, maxiter=100, thres=1e-5): 22 | """ Solve Gauss-Helmert Problem: 23 | argmin ||dl||^2, subject to g(x+dx, l+dl)=0, where x is unknown parameters 24 | and l are observations. 25 | 26 | Input 27 | -------- 28 | g: callable object 29 | Represent constraint function g(x,l)=0. 30 | f should take two arguments (x and l) and return a tuple (error, Jx, Jl), 31 | where error is g(x,l), Jx is the Jacobian dg/dx and Jl is the Jacobian dg/dl. 32 | 33 | x: 1xP array 34 | Parameter initial guess x0 of size P 35 | 36 | lm: NxQ array 37 | Observation matrix, each row represent one observation l instance of size Q 38 | 39 | Cov_ll: NxQxQ array 40 | Covariance matrice for each observation instance l1 ... lN 41 | 42 | maxiter: int 43 | Terminate when maximum number of iteration reached. 44 | 45 | thres: float 46 | Terminate when max(|dx|) < thres 47 | 48 | Return 49 | -------- 50 | xnu: 1xP array 51 | Estimated parameter vector x 52 | 53 | Cov_xx: PxP array 54 | Covariance matrix of estimated x 55 | 56 | sigma_0: float 57 | factor, should close 1 58 | 59 | vv: NxQ array 60 | Observation corrections dl 61 | 62 | w: 1xN array 63 | Weights for each observation l, lower weight means an outliar 64 | """ 65 | 66 | N, Q = lm.shape[:2] 67 | err_i, A_i, B_i = g(x, lm[0,:]) 68 | M = len(err_i) # residual size 69 | P = len(x) # parameter size 70 | 71 | R = N * Q - P # estimation redundancy 72 | if R<0: 73 | raise RuntimeError('Not enough observations') 74 | 75 | ''' init ''' 76 | xnu = x.copy() 77 | lnu = lm.copy() # current updated observations 78 | vv = np.zeros_like(lm) # observations correction 79 | 80 | Am = np.empty( (N,) + A_i.shape ) 81 | Bm = np.empty( (N,) + B_i.shape ) 82 | W_gg = np.empty( (N,) + (M, M) ) 83 | Cg = np.empty( (N, M) ) 84 | Nm = np.empty( (P, P) ) 85 | nv = np.empty( P ) 86 | X_gg = np.empty( N ) 87 | 88 | W_ll = np.empty_like(Cov_ll) 89 | for i in range(N): 90 | W_ll[i] = np.linalg.pinv(Cov_ll[i]) 91 | 92 | for it in range(maxiter): 93 | Nm[:,:] = 0 94 | nv[:] = 0 95 | for i in range(N): 96 | # update Jacobian and residual 97 | err_i, Am[i], Bm[i] = g(xnu, lnu[i,:]) 98 | A_i, B_i = Am[i], Bm[i] 99 | Cg[i] = B_i.dot(vv[i]) - err_i 100 | 101 | # weights of constraints 102 | W_gg[i] = np.linalg.pinv( B_i.dot(Cov_ll[i]).dot(B_i.T) ) 103 | 104 | X_gg[i] = np.sqrt( Cg[i].T.dot(W_gg[i]).dot(Cg[i]) ) 105 | sigma_gg = np.median(X_gg)/0.6745 106 | w = huber_w_smooth( X_gg/sigma_gg ) 107 | 108 | for i in range(N): 109 | # normal equation 110 | ATBWB = Am[i].T.dot(w[i]*W_gg[i]) 111 | Nm += ATBWB.dot(Am[i]) 112 | nv += ATBWB.dot(Cg[i]) 113 | 114 | 115 | lambda_N = np.sort( np.linalg.eigvalsh(Nm) ) 116 | if np.abs(np.log( lambda_N[-1]/lambda_N[0] ) ) > 10: 117 | print( 'normal equation matrix nearly singular') 118 | 119 | # solve 120 | dx = np.linalg.solve(Nm, nv) 121 | 122 | # update 123 | xnu = xnu + dx 124 | 125 | omega = 0 126 | for i in range(N): 127 | lamba = W_gg[i].dot( Am[i].dot(dx) - Cg[i] ) 128 | dl = -Cov_ll[i].dot( Bm[i].T.dot(lamba) ) - vv[i] 129 | 130 | lnu[i,:] = lnu[i,:] + dl 131 | 132 | # calculate observation-corrections 133 | vv[i] = lnu[i,:] - lm[i,:] 134 | 135 | Cov_xx = np.linalg.pinv(Nm) 136 | omega = np.sum([vv[i].dot(W_ll[i]).dot(vv[i]) for i in range(N)]) 137 | sigma_0 = omega/R 138 | print('GH Iter %d: %f' % (it, sigma_0)) 139 | 140 | if np.abs(dx).max() < thres: 141 | break 142 | return xnu, Cov_xx, sigma_0, vv, w 143 | 144 | def test_SolveGaussHelmertProblem(): 145 | '''' implicit test model g(x,l) = x*l = 10 ''' 146 | def g(x,l): 147 | return np.atleast_1d(np.dot(x,l)-10), np.atleast_2d(l), np.atleast_2d(x) 148 | x_true = np.array([1.,1.,1.]) 149 | 150 | # data 151 | sigma_l = 1e-3 152 | sigma_x = 1e-2 153 | T = 500 154 | 155 | lm = np.empty((T,3)) 156 | x0 = x_true + sigma_x*np.random.randn(3) 157 | l_true = np.empty_like(x0) 158 | for t in range(T): 159 | l_true[:2] = 3*np.random.randn(2) 160 | l_true[2] = 10 - np.sum(l_true[:2]) 161 | lm[t,:] = l_true + sigma_l*np.random.randn(3) 162 | Cov_ll = np.tile( sigma_l**2*np.eye(3), (T,1,1) ) 163 | # solve 164 | xe, Cov_xx, sigma_0, vv, w = SolveGaussHelmertProblem(g, x0, lm, Cov_ll) 165 | np.testing.assert_array_almost_equal(x_true, xe, 4) 166 | print('test: solving implicit test model passed') 167 | 168 | 169 | '''explict test model f(x,l): A*x=l''' 170 | A = np.diag([3.,2.,1.]) 171 | x_true = np.array([1.,2.,3.]) 172 | l_true = A.dot(x_true) 173 | def f(x, l): 174 | return (A.dot(x)-l, A, -np.eye(len(l))) 175 | 176 | 177 | for t in range(T): 178 | lm[t,:] = l_true + sigma_l*np.random.randn(3) 179 | Cov_ll = np.tile( sigma_l**2*np.eye(3), (T,1,1) ) 180 | 181 | x0 = x_true + sigma_x*np.random.randn(3) 182 | 183 | # solve 184 | xe, Cov_xx, sigma_0, vv, w = SolveGaussHelmertProblem(f, x0, lm, Cov_ll) 185 | np.testing.assert_array_almost_equal(x_true, xe, 4) 186 | print('test: solving explicit test model passed') 187 | 188 | # batch test 189 | ntest = 100 190 | sigmas = np.empty(ntest) 191 | for i in range(ntest): 192 | x0 = x_true + sigma_x*np.random.randn(3) 193 | for t in range(T): 194 | lm[t,:] = l_true + sigma_l*np.random.randn(3) 195 | xe, Cov_xx, sigmas[i], vv, w = SolveGaussHelmertProblem(f, x0, lm, Cov_ll) 196 | print("Mean sigma_0: %f (which should be close to 1)" % np.mean(sigmas)) 197 | 198 | 199 | if __name__ == "__main__": 200 | test_SolveGaussHelmertProblem() 201 | 202 | -------------------------------------------------------------------------------- /calibration.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Tue Nov 28 16:58:46 2017 5 | 6 | @author: kaihong 7 | """ 8 | from __future__ import print_function 9 | import numpy as np 10 | import scipy 11 | from optimizer import SolveGaussHelmertProblem 12 | from numpy.random import randn 13 | 14 | from pycppad import independent,adfun 15 | def GenerateAutoDiffFunction(g, x0, l0): 16 | dim_x = x0.shape[0] 17 | a_var = independent(np.hstack([x0, l0])) 18 | jacobian = adfun(a_var, g(a_var[:dim_x], a_var[dim_x:])).jacobian 19 | 20 | def g_autodiff(x, l): 21 | err = g(x, l) 22 | J = jacobian(np.hstack([x, l])) 23 | return err, J[:, :dim_x], J[:, dim_x:] 24 | return g_autodiff 25 | 26 | 27 | def Rot2ax(R): 28 | """Rotation matrix to angle-axis vector""" 29 | tr = np.trace(R) 30 | a = np.array( [R[2,1]-R[1,2], R[0,2]-R[2,0], R[1,0]-R[0,1]] ) 31 | an = np.linalg.norm(a) 32 | phi= np.arctan2(an, tr-1) 33 | if np.abs(phi) < 1e-12: 34 | return np.zeros(3,'d') 35 | else: 36 | return phi/an*a 37 | 38 | def skew(v): 39 | return np.array([[ 0, -v[2], v[1]], 40 | [ v[2], 0, -v[0]], 41 | [-v[1], v[0], 0 ]]) 42 | 43 | def ax2Rot(r): 44 | """Angle-axis vector to rotation matrix""" 45 | p = np.linalg.norm(r) 46 | if np.abs(p) < 1e-12: 47 | return np.eye(3) 48 | else: 49 | S = skew(r/p) 50 | return np.eye(3) + np.sin(p)*S + (1.0-np.cos(p))*S.dot(S) 51 | 52 | def MfromRT(r,t): 53 | T = np.eye(4) 54 | T[:3,:3] = ax2Rot(r) 55 | T[:3, 3] = t 56 | return T 57 | 58 | def RTfromM(mat): 59 | return Rot2ax(mat[:3,:3]), mat[:3,3] 60 | 61 | #%% 62 | def ExtrinsicCalibration3D(trajectories_list, trajectories_cov_list=None, *args): 63 | """ Motion-base sensor calibration 64 | Constrain equations: 65 | ( I3 - R(r_b) )*xi_b = t_b - R(eta_b)*t_a 66 | and 67 | R(eta_b)*r_a = r_b 68 | 69 | Input 70 | ----------- 71 | trajectories_list: list( list( 4x4 pose matrices ) ) 72 | 73 | Output 74 | ----------- 75 | calibration result as a list of matrix 76 | """ 77 | num_pose_list = list(map(len, trajectories_list)) 78 | if len(set(num_pose_list))!=1: 79 | raise ValueError("each trajectory should have the same number of poses") 80 | 81 | num_pose = num_pose_list[0] 82 | num_sensor = len(trajectories_list) 83 | num_solution = num_sensor-1 84 | print("Input: %d sensors, each has %d poses" % (num_sensor, num_pose)) 85 | 86 | '''Assemble observation matrix lm, each row l = [ra,ta, ..., rm, tm]''' 87 | stacked_r_list = [ np.vstack( [ Rot2ax(pose_mat[:3,:3]) for pose_mat in trajectory] ) 88 | for trajectory in trajectories_list ] 89 | stacked_t_list = [ np.vstack( [ pose_mat[:3, 3] for pose_mat in trajectory] ) 90 | for trajectory in trajectories_list ] 91 | r_t_interleaved = map(np.hstack, zip(stacked_r_list, stacked_t_list)) 92 | lm = np.hstack( r_t_interleaved ) # lm.shape = (num_pose, 6*num_sensor) 93 | 94 | '''Assemble covariance matrix ''' 95 | if trajectories_cov_list is None: 96 | Cov_ll = np.tile(np.eye(6*num_sensor), (num_pose, 1, 1)) 97 | else: 98 | Cov_ll = np.zeros((num_pose, 6*num_sensor, 6*num_sensor)) 99 | cov_list_time_majored = list(zip(*trajectories_cov_list)) # list[sensor_idx][pose_idx] -> list[pose_idx][sensor_idx] 100 | for pose_idx in range(num_pose): 101 | Cov_ll[pose_idx, :, :] = scipy.linalg.block_diag(*cov_list_time_majored[pose_idx]) 102 | 103 | '''Calculate close form solution as initial guess''' 104 | x0_list = [] 105 | I3 = np.eye(3) 106 | for s in range(1, num_sensor): 107 | # rotation first 108 | H = stacked_r_list[0].T.dot(stacked_r_list[s]) 109 | U, d, Vt = np.linalg.svd(H) 110 | R = Vt.T.dot(U.T) 111 | 112 | # then translation 113 | A = np.vstack([ I3 - ax2Rot(r_) for r_ in stacked_r_list[s]]) 114 | b = np.hstack( stacked_t_list[s] - ( R.dot(stacked_t_list[0].T) ).T ) 115 | t = np.linalg.lstsq(A, b)[0] 116 | x0_list.append([Rot2ax(R), t]) 117 | x0 = np.array(x0_list).flatten() 118 | print('Initial guess:') 119 | map(lambda rt: print(MfromRT(*rt)), x0_list) 120 | 121 | '''Assemble constraint functions ''' 122 | def g(x, l): 123 | x = np.reshape(x, (num_solution, 6)) 124 | l = np.reshape(l, (num_sensor, 6)) 125 | r,t = np.split(l, 2, axis=1) 126 | e = [] 127 | for x_s, s in zip(x, range(1, num_sensor)): 128 | Rq = ax2Rot(x_s[0:3]) 129 | Rs = ax2Rot(r[s]) 130 | e.append(x_s[3:] - Rs.dot(x_s[3:]) + Rq.dot(t[0]) - t[s]) # g1 constraints 131 | e.append( Rq.dot(r[0]) - r[s] ) # full-constraints 132 | return np.hstack(e) 133 | g_diff = GenerateAutoDiffFunction(g, x0, lm[0,:]) 134 | 135 | '''solve''' 136 | xnu, Cov_xx, sigma_0, vv, w = SolveGaussHelmertProblem(g_diff, x0, lm, Cov_ll, *args) 137 | return [MfromRT(x[:3], x[3:]) for x in np.split(xnu, num_solution) ] 138 | 139 | #%% 140 | def demo_and_test(): 141 | def randsp(n=3): 142 | v = np.random.uniform(-1, 1, size=n) 143 | return v/np.linalg.norm(v) 144 | 145 | ''' ground truth transformation between sensors ''' 146 | num_sensor = 3 147 | num_solution = num_sensor-1 148 | x_true = np.array([randsp() for _ in range(num_solution*2) ]).ravel() # x= [r1,t1,...,rn,tn] 149 | Hm = [MfromRT(x[:3], x[3:]) for x in np.split(x_true, num_solution)] 150 | 151 | ''' generate ground truth trajectories ''' 152 | num_pose = 500 153 | dM = [] 154 | Hm_inv = [np.linalg.inv(h) for h in Hm] 155 | for t in range(num_pose): 156 | dm = [MfromRT(randsp(),randsp())] # base sensor 157 | for h, h_inv in zip(Hm, Hm_inv): # other sensor 158 | dm.append( h.dot(dm[0]).dot(h_inv) ) 159 | dM.append(dm) 160 | trajectories_list = zip(*dM) 161 | 162 | ''' add measurement noise''' 163 | sigma_r = 1e-3 164 | sigma_t = 1e-2 165 | 166 | noisy_trajectories = [] 167 | for trajectory in trajectories_list: 168 | one_trajectory = [] 169 | for pose in trajectory: 170 | r,t = RTfromM(pose) 171 | new_pose = MfromRT( r+sigma_r*randn(3), t+sigma_t*randn(3)) 172 | one_trajectory.append(new_pose) 173 | noisy_trajectories.append(one_trajectory) 174 | trajectory_covs = [ [np.diag([sigma_r]*3 + [sigma_t]*3)**2] * num_pose ] * num_sensor 175 | 176 | H_est = ExtrinsicCalibration3D(noisy_trajectories, trajectory_covs) 177 | print("After refinement:") 178 | list(map(print, H_est)) 179 | print("Ground truth:") 180 | list(map(print, Hm)) 181 | 182 | if __name__ =='__main__': 183 | demo_and_test() --------------------------------------------------------------------------------