Hide keyboard shortcuts

Hot-keys on this page

r m x p   toggle line displays

j k   next/prev highlighted chunk

0   (zero) top of page

1   (one) first highlighted chunk

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

66

67

68

69

70

71

72

73

74

75

76

77

78

79

80

81

82

83

84

85

86

87

88

89

90

91

92

93

94

95

96

97

98

99

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

""" 

Function for camera calibrations 

""" 

import pandas as pd 

import numpy as np 

import cv2 

import os 

from navipy.io import ivfile 

import matplotlib.pyplot as plt 

from mpl_toolkits.mplot3d import Axes3D # noqa F401 

 

 

def concatenate_manhattan_2d3d(manhattan_3d, manhattan_2d): 

""" Join 3D and 2D points for pose estimation """ 

if not isinstance(manhattan_2d, pd.DataFrame): 

raise TypeError('manhattan_2d should be a DataFrame') 

if not isinstance(manhattan_3d, pd.DataFrame): 

raise TypeError('manhattan_3d should be a DataFrame') 

d = {'three_d': manhattan_3d, 'two_d': manhattan_2d} 

manhattan_3d2d = pd.concat(d.values(), axis=1, keys=d.keys()) 

manhattan_3d2d = manhattan_3d2d.astype(float) 

return manhattan_3d2d 

 

 

def objects_points_from_manhattan(manhattan_3d2d): 

"""return objects points from a join manhatta 

 

.. seealso: concatenate_manhattan_2d3d 

""" 

if not isinstance(manhattan_3d2d, pd.DataFrame): 

raise TypeError('manhattan_3d2d should be a DataFrame') 

return manhattan_3d2d.dropna().loc[:, 'three_d']\ 

.loc[:, ['x', 'y', 'z']].values 

 

 

def image_points_from_manhattan(manhattan_3d2d): 

"""return image points from a joined manhattan 

 

.. seelso: concatenate_manhattan_2d3d 

""" 

if not isinstance(manhattan_3d2d, pd.DataFrame): 

raise TypeError('manhattan_3d2d should be a DataFrame') 

return manhattan_3d2d.dropna().loc[:, 'two_d'].loc[:, ['x', 'y']].values 

 

 

def compose_from_vectors(rvec, tvec): 

"""return camera pose from rotation and translation vectors 

""" 

r = cv2.Rodrigues(rvec)[0] 

camera_pose = np.hstack((r, tvec)) 

camera_pose = np.vstack((camera_pose, [0, 0, 0, 1])) 

return camera_pose 

 

 

def camera_pose(manhattan_2d, manhattan_3d, camera_intrinsics): 

"""solvePnP with 3D-2D points correspondance. 

 

The camera intrinsics have to be known appriori 

""" 

camera_matrix = camera_intrinsics['intrinsic_matrix'] 

distcoeffs = camera_intrinsics['distortion'] 

 

manhattan_2d3d = concatenate_manhattan_2d3d(manhattan_3d, manhattan_2d) 

objects_points = objects_points_from_manhattan(manhattan_2d3d) 

image_points = image_points_from_manhattan(manhattan_2d3d) 

 

retval, rvec, tvec = cv2.solvePnP( 

objects_points, image_points, camera_matrix, distcoeffs) 

 

return compose_from_vectors(rvec, tvec) 

 

 

def calibrates_extrinsic(filenames, 

manhattan_3d, 

cameras_intrinsics, 

corner_th=-1, 

manhattan_3d_key='manhattan'): 

"""Calibrate from files. 

 

:param filenames: a list of ivfile name with manhattan markers 

:param file_manhattan_3d: hdf file with manhattan_3d_key to load \ 

containing a pandas dataframe with x,y,z coordinates of each points. 

:param cameras_intrinsics: a list of camera intrinsics 

:param corner_th: a threshold for points to be ignored on the top\ 

right image corner (useful when certain points are not visible) 

:param manhattan_3d_key: the key to load the manhattan_3d \ 

(default: manhattan) 

""" 

cameras_extrinsics = dict() 

for cam_i, cfile in filenames.items(): 

_, ext = os.path.splitext(cfile) 

if ext == '.tra': 

manhattan_2d = ivfile.manhattan(cfile, corner_th) 

elif ext == '.csv': 

manhattan_2d = pd.read_csv(cfile, 

names=['x', 'y'], 

header=0) 

idx = (manhattan_2d.x < corner_th) & (manhattan_2d.y < corner_th) 

manhattan_2d.x[idx] = np.nan 

manhattan_2d.y[idx] = np.nan 

else: 

msg = 'FileType not understood (only .tra, .csv are supported)' 

raise ValueError(msg) 

cameras_extrinsics[cam_i] = dict() 

cameras_extrinsics[cam_i]['pose'] = camera_pose( 

manhattan_2d, manhattan_3d, cameras_intrinsics[cam_i]) 

return cameras_extrinsics 

 

 

def plot_manhattan(manhattan_3d, figsize=(15, 15), unit=None): 

"""Plot a manhattan dataframe 

 

:param manhattan_3d: x,y,z coordinates of each tower 

:param figsize: figure size 

:returns: figure and axis handles 

 

""" 

if unit is None: 

try: 

unit = manhattan.unit 

except AttributeError as e: 

pass 

if unit is None: 

unit = 'no-unit' 

 

fig = plt.figure(figsize=figsize) 

ax0 = fig.add_subplot(221) 

ax1 = fig.add_subplot(223) 

ax3 = fig.add_subplot(224) 

for ax, col_i, col_j in zip([ax1, ax0, ax3], 

['x', 'x', 'z'], 

['y', 'z', 'y']): 

ax.plot(manhattan_3d.loc[:, col_i], manhattan_3d.loc[:, col_j], 'ko') 

ax.set_xlabel('{} [{}]'.format(col_i, unit)) 

ax.set_ylabel('{} [{}]'.format(col_j, unit)) 

ax.axis('equal') 

 

# Annotate 

for row_i, row_val in manhattan_3d.iterrows(): 

shift = [10, 10] 

xy = row_val.loc[['x', 'y']] 

xytext = xy - 2 * ((xy > 0) - 0.5) * shift 

ax1.annotate('{}'.format(row_i), xy, xytext) 

 

ax4 = fig.add_subplot(222, projection='3d') 

for i, row in manhattan_3d.iterrows(): 

ax4.plot([row.x, row.x], 

[row.y, row.y], 

[0, row.z], 'k-.') 

ax4.set_xlabel('{} [{}]'.format('x', unit)) 

ax4.set_ylabel('{} [{}]'.format('y', unit)) 

ax4.set_zlabel('{} [{}]'.format('z', unit)) 

return fig, [ax0, ax1, ax3, ax4]