├── .gitignore ├── GoogLeNet ├── __init__.py ├── base.py ├── googlenet.py ├── googlenet_fc_big.py └── weights │ └── posenet.npy ├── README.md ├── abstract_network ├── __init__.py ├── abstract.py └── setting.py ├── data ├── __init__.py ├── absolute_cambridge.py ├── abstract.py ├── relative_cambridge.py └── rpnet.zip ├── posenet ├── __init__.py ├── posenet.py └── posenet_for_relative_pose.py ├── rpnet ├── __init__.py ├── data_preprocessing.py ├── rpnet.py ├── rpnetfc.py └── rpnetplus.py ├── surf ├── __init__.py ├── relative_cambridge.py └── relative_cambridge_full_size.py ├── toy_example ├── __init__.py ├── camera.py ├── main_demo.py └── toy_example.png └── utility ├── __init__.py ├── correct_checkpoint.py ├── delete_old_checkpoint.py └── printing_function.py /.gitignore: -------------------------------------------------------------------------------- 1 | ########################## 2 | #Linux Specific GitIgnore# 3 | ########################## 4 | .* 5 | !.gitignore 6 | *~ 7 | *.sw[a-p] 8 | 9 | # KDE 10 | .directory 11 | 12 | ############################ 13 | #Eclipse Specific GitIgnore# 14 | ############################ 15 | *.pydevproject 16 | .project 17 | .metadata 18 | bin/** 19 | tmp/** 20 | tmp/**/* 21 | *.tmp 22 | *.bak 23 | *.swp 24 | *~.nib 25 | local.properties 26 | .classpath 27 | .settings/ 28 | .loadpath 29 | 30 | # CDT-specific 31 | .cproject 32 | 33 | # python files 34 | *.pyc 35 | *nohup* 36 | __pycache__ 37 | 38 | ########################### 39 | # Latex 40 | ########################### 41 | ## Core latex/pdflatex auxiliary files: 42 | *.aux 43 | *.lof 44 | *.log 45 | *.lot 46 | *.fls 47 | *.out 48 | *.toc 49 | *.fmt 50 | *.fot 51 | *.cb 52 | *.cb2 53 | .*.lb 54 | 55 | ## Intermediate documents: 56 | *.dvi 57 | *.xdv 58 | *-converted-to.* 59 | # these rules might exclude image files for figures etc. 60 | # *.ps 61 | # *.eps 62 | # *.pdf 63 | 64 | ## Generated if empty string is given at "Please type another file name for output:" 65 | .pdf 66 | 67 | ## Bibliography auxiliary files (bibtex/biblatex/biber): 68 | *.bbl 69 | *.bcf 70 | *.blg 71 | *-blx.aux 72 | *-blx.bib 73 | *.run.xml 74 | 75 | ## Build tool auxiliary files: 76 | *.fdb_latexmk 77 | *.synctex 78 | *.synctex(busy) 79 | *.synctex.gz 80 | *.synctex.gz(busy) 81 | *.pdfsync 82 | 83 | ## Build tool directories for auxiliary files 84 | # latexrun 85 | latex.out/ 86 | 87 | ## Auxiliary and intermediate files from other packages: 88 | # algorithms 89 | *.alg 90 | *.loa 91 | 92 | # achemso 93 | acs-*.bib 94 | 95 | # amsthm 96 | *.thm 97 | 98 | # beamer 99 | *.nav 100 | *.pre 101 | *.snm 102 | *.vrb 103 | 104 | # changes 105 | *.soc 106 | 107 | # cprotect 108 | *.cpt 109 | 110 | # elsarticle (documentclass of Elsevier journals) 111 | *.spl 112 | 113 | # endnotes 114 | *.ent 115 | 116 | # fixme 117 | *.lox 118 | 119 | # feynmf/feynmp 120 | *.mf 121 | *.mp 122 | *.t[1-9] 123 | *.t[1-9][0-9] 124 | *.tfm 125 | 126 | #(r)(e)ledmac/(r)(e)ledpar 127 | *.end 128 | *.?end 129 | *.[1-9] 130 | *.[1-9][0-9] 131 | *.[1-9][0-9][0-9] 132 | *.[1-9]R 133 | *.[1-9][0-9]R 134 | *.[1-9][0-9][0-9]R 135 | *.eledsec[1-9] 136 | *.eledsec[1-9]R 137 | *.eledsec[1-9][0-9] 138 | *.eledsec[1-9][0-9]R 139 | *.eledsec[1-9][0-9][0-9] 140 | *.eledsec[1-9][0-9][0-9]R 141 | 142 | # glossaries 143 | *.acn 144 | *.acr 145 | *.glg 146 | *.glo 147 | *.gls 148 | *.glsdefs 149 | 150 | # gnuplottex 151 | *-gnuplottex-* 152 | 153 | # gregoriotex 154 | *.gaux 155 | *.gtex 156 | 157 | # htlatex 158 | *.4ct 159 | *.4tc 160 | *.idv 161 | *.lg 162 | *.trc 163 | *.xref 164 | 165 | # hyperref 166 | *.brf 167 | 168 | # knitr 169 | *-concordance.tex 170 | # TODO Comment the next line if you want to keep your tikz graphics files 171 | *.tikz 172 | *-tikzDictionary 173 | 174 | # listings 175 | *.lol 176 | 177 | # makeidx 178 | *.idx 179 | *.ilg 180 | *.ind 181 | *.ist 182 | 183 | # minitoc 184 | *.maf 185 | *.mlf 186 | *.mlt 187 | *.mtc[0-9]* 188 | *.slf[0-9]* 189 | *.slt[0-9]* 190 | *.stc[0-9]* 191 | 192 | # minted 193 | _minted* 194 | *.pyg 195 | 196 | # morewrites 197 | *.mw 198 | 199 | # nomencl 200 | *.nlg 201 | *.nlo 202 | *.nls 203 | 204 | # pax 205 | *.pax 206 | 207 | # pdfpcnotes 208 | *.pdfpc 209 | 210 | # sagetex 211 | *.sagetex.sage 212 | *.sagetex.py 213 | *.sagetex.scmd 214 | 215 | # scrwfile 216 | *.wrt 217 | 218 | # sympy 219 | *.sout 220 | *.sympy 221 | sympy-plots-for-*.tex/ 222 | 223 | # pdfcomment 224 | *.upa 225 | *.upb 226 | 227 | # pythontex 228 | *.pytxcode 229 | pythontex-files-*/ 230 | 231 | # tcolorbox 232 | *.listing 233 | 234 | # thmtools 235 | *.loe 236 | 237 | # TikZ & PGF 238 | *.dpth 239 | *.md5 240 | *.auxlock 241 | 242 | # todonotes 243 | *.tdo 244 | 245 | # easy-todo 246 | *.lod 247 | 248 | # xmpincl 249 | *.xmpi 250 | 251 | # xindy 252 | *.xdy 253 | 254 | # xypic precompiled matrices 255 | *.xyc 256 | 257 | # endfloat 258 | *.ttt 259 | *.fff 260 | 261 | # Latexian 262 | TSWLatexianTemp* 263 | 264 | ## Editors: 265 | # WinEdt 266 | *.bak 267 | *.sav 268 | 269 | # Texpad 270 | .texpadtmp 271 | 272 | # LyX 273 | *.lyx~ 274 | 275 | # Kile 276 | *.backup 277 | 278 | # KBibTeX 279 | *~[0-9]* 280 | 281 | # auto folder when using emacs and auctex 282 | ./auto/* 283 | *.el 284 | 285 | # expex forward references with \gathertags 286 | *-tags.tex 287 | 288 | # standalone packages 289 | *.sta 290 | -------------------------------------------------------------------------------- /GoogLeNet/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ensv/RPNet/f4b3fbf510d47801e5b4a1e8b9001a5bb3038a1b/GoogLeNet/__init__.py -------------------------------------------------------------------------------- /GoogLeNet/base.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import tensorflow as tf 3 | from six import string_types 4 | DEFAULT_PADDING = 'SAME' 5 | 6 | 7 | def layer(op): 8 | '''Decorator for composable abstract_network layers.''' 9 | 10 | def layer_decorated(self, *args, **kwargs): 11 | # Automatically set a name if not provided. 12 | name = kwargs.setdefault('name', self.get_unique_name(op.__name__)) 13 | # Figure out the layer inputs. 14 | if len(self.terminals) == 0: 15 | raise RuntimeError('No input variables found for layer %s.' % name) 16 | elif len(self.terminals) == 1: 17 | layer_input = self.terminals[0] 18 | else: 19 | layer_input = list(self.terminals) 20 | # Perform the operation and get the output. 21 | layer_output = op(self, layer_input, *args, **kwargs) 22 | # Add to layer LUT. 23 | self.layers[name] = layer_output 24 | # This output is now the input for the next layer. 25 | self.feed(layer_output) 26 | # Return self for chained calls. 27 | return self 28 | 29 | return layer_decorated 30 | 31 | 32 | class Network(object): 33 | 34 | def __init__(self, inputs, trainable=True): 35 | # The input nodes for this abstract_network 36 | self.inputs = inputs 37 | # The current list of terminal nodes 38 | self.terminals = [] 39 | # Mapping from layer names to layers 40 | self.layers = dict(inputs) 41 | # If true, the resulting variables are set as trainable 42 | self.trainable = trainable 43 | # Switch variable for dropout 44 | self.use_dropout = tf.placeholder_with_default(tf.constant(1.0), 45 | shape=[], 46 | name='use_dropout') 47 | self.setup() 48 | 49 | def setup(self): 50 | '''Construct the abstract_network. ''' 51 | raise NotImplementedError('Must be implemented by the subclass.') 52 | 53 | def load(self, data_path, session, ignore_missing=False, prefix = ''): 54 | '''Load abstract_network weights. 55 | data_path: The path to the numpy-serialized abstract_network weights 56 | session: The current TensorFlow session 57 | ignore_missing: If true, serialized weights for missing layers are ignored. 58 | ''' 59 | data_dict = np.load(data_path, encoding='latin1').item() 60 | for op_name in data_dict: 61 | with tf.variable_scope(op_name, reuse=True): 62 | for param_name, data in data_dict[op_name].items(): 63 | try: 64 | if prefix == '': 65 | var = tf.get_variable(param_name) 66 | else: 67 | var = tf.get_variable('%s' + param_name) 68 | session.run(var.assign(data)) 69 | except ValueError: 70 | if not ignore_missing: 71 | raise 72 | 73 | def feed(self, *args): 74 | '''Set the input(s) for the next operation by replacing the terminal nodes. 75 | The arguments can be either layer names or the actual layers. 76 | ''' 77 | assert len(args) != 0 78 | self.terminals = [] 79 | for fed_layer in args: 80 | if isinstance(fed_layer, string_types): # @UndefinedVariable 81 | try: 82 | fed_layer = self.layers[fed_layer] 83 | except KeyError: 84 | raise KeyError('Unknown layer name fed: %s' % fed_layer) 85 | self.terminals.append(fed_layer) 86 | return self 87 | 88 | def get_output(self): 89 | '''Returns the current abstract_network output.''' 90 | return self.terminals[-1] 91 | 92 | def get_unique_name(self, prefix): 93 | '''Returns an index-suffixed unique name for the given prefix. 94 | This is used for auto-generating layer names based on the type-prefix. 95 | ''' 96 | ident = sum(t.startswith(prefix) for t, _ in self.layers.items()) + 1 97 | return '%s_%d' % (prefix, ident) 98 | 99 | def make_var(self, name, shape): 100 | '''Creates a new TensorFlow variable.''' 101 | return tf.get_variable(name, shape, trainable=self.trainable) 102 | 103 | def validate_padding(self, padding): 104 | '''Verifies that the padding is one of the supported ones.''' 105 | assert padding in ('SAME', 'VALID') 106 | 107 | @layer 108 | def conv(self, 109 | input, 110 | k_h, 111 | k_w, 112 | c_o, 113 | s_h, 114 | s_w, 115 | name, 116 | relu=True, 117 | padding=DEFAULT_PADDING, 118 | group=1, 119 | biased=True): 120 | # Verify that the padding is acceptable 121 | self.validate_padding(padding) 122 | # Get the number of channels in the input 123 | c_i = input.get_shape()[-1] 124 | 125 | # print(c_i, 'c_i', type(c_i), type(group)) 126 | # Verify that the grouping parameter is valid 127 | assert c_i % group == 0 128 | assert c_o % group == 0 129 | # Convolution for a given input and kernel 130 | convolve = lambda i, k: tf.nn.conv2d(i, k, [1, s_h, s_w, 1], padding=padding) 131 | with tf.variable_scope(name) as scope: 132 | 133 | 134 | kernel = self.make_var('weights', shape=[k_h, k_w, int(c_i) / group, c_o]) 135 | 136 | if group == 1: 137 | # This is the common-case. Convolve the input without any further complications. 138 | output = convolve(input, kernel) 139 | else: 140 | # Split the input into groups and then convolve each of them independently 141 | input_groups = tf.split(3, group, input) 142 | kernel_groups = tf.split(3, group, kernel) 143 | output_groups = [convolve(i, k) for i, k in zip(input_groups, kernel_groups)] 144 | # Concatenate the groups 145 | output = tf.concat(3, output_groups) 146 | # Add the biases 147 | if biased: 148 | biases = self.make_var('biases', [c_o]) 149 | output = tf.nn.bias_add(output, biases) 150 | if relu: 151 | # ReLU non-linearity 152 | output = tf.nn.relu(output, name=scope.name) 153 | return output 154 | 155 | @layer 156 | def relu(self, input, name): # @ReservedAssignment 157 | return tf.nn.relu(input, name=name) 158 | 159 | @layer 160 | def max_pool(self, input, k_h, k_w, s_h, s_w, name, padding=DEFAULT_PADDING): # @ReservedAssignment 161 | self.validate_padding(padding) 162 | return tf.nn.max_pool(input, 163 | ksize=[1, k_h, k_w, 1], 164 | strides=[1, s_h, s_w, 1], 165 | padding=padding, 166 | name=name) 167 | 168 | @layer 169 | def avg_pool(self, input, k_h, k_w, s_h, s_w, name, padding=DEFAULT_PADDING): # @ReservedAssignment 170 | self.validate_padding(padding) 171 | return tf.nn.avg_pool(input, 172 | ksize=[1, k_h, k_w, 1], 173 | strides=[1, s_h, s_w, 1], 174 | padding=padding, 175 | name=name) 176 | 177 | @layer 178 | def lrn(self, input, radius, alpha, beta, name, bias=1.0): # @ReservedAssignment 179 | return tf.nn.local_response_normalization(input, 180 | depth_radius=radius, 181 | alpha=alpha, 182 | beta=beta, 183 | bias=bias, 184 | name=name) 185 | 186 | @layer 187 | def concat(self, inputs, axis, name): 188 | return tf.concat(values=inputs, axis=axis, name=name) 189 | 190 | @layer 191 | def add(self, inputs, name): 192 | return tf.add_n(inputs, name=name) 193 | 194 | @layer 195 | def fc(self, input, num_out, name, relu=True): # @ReservedAssignment 196 | with tf.variable_scope(name) as scope: 197 | input_shape = input.get_shape() 198 | if input_shape.ndims == 4: 199 | # The input is spatial. Vectorize it first. 200 | dim = 1 201 | for d in input_shape[1:].as_list(): 202 | dim *= d 203 | feed_in = tf.reshape(input, [-1, dim]) 204 | else: 205 | feed_in, dim = (input, input_shape[-1].value) 206 | weights = self.make_var('weights', shape=[dim, num_out]) 207 | biases = self.make_var('biases', [num_out]) 208 | op = tf.nn.relu_layer if relu else tf.nn.xw_plus_b 209 | fc = op(feed_in, weights, biases, name=scope.name) 210 | return fc 211 | 212 | @layer 213 | def softmax(self, input, name): # @ReservedAssignment 214 | input_shape = map(lambda v: v.value, input.get_shape()) 215 | if len(input_shape) > 2: 216 | # For certain models (like NiN), the singleton spatial dimensions 217 | # need to be explicitly squeezed, since they're not broadcast-able 218 | # in TensorFlow's NHWC ordering (unlike Caffe's NCHW). 219 | if input_shape[1] == 1 and input_shape[2] == 1: 220 | input = tf.squeeze(input, squeeze_dims=[1, 2]) # @ReservedAssignment 221 | else: 222 | raise ValueError('Rank 2 tensor input expected for softmax!') 223 | return tf.nn.softmax(input, name) 224 | 225 | @layer 226 | def batch_normalization(self, input, name, scale_offset=True, relu=False): # @ReservedAssignment 227 | # NOTE: Currently, only inference is supported 228 | with tf.variable_scope(name) as scope: # @UnusedVariable 229 | shape = [input.get_shape()[-1]] 230 | if scale_offset: 231 | scale = self.make_var('scale', shape=shape) 232 | offset = self.make_var('offset', shape=shape) 233 | else: 234 | scale, offset = (None, None) 235 | output = tf.nn.batch_normalization( 236 | input, 237 | mean=self.make_var('mean', shape=shape), 238 | variance=self.make_var('variance', shape=shape), 239 | offset=offset, 240 | scale=scale, 241 | # TODO: This is the default Caffe batch norm eps 242 | # Get the actual eps from parameters 243 | variance_epsilon=1e-5, 244 | name=name) 245 | if relu: 246 | output = tf.nn.relu(output) 247 | return output 248 | 249 | @layer 250 | def dropout(self, input, keep_prob, name): # @ReservedAssignment 251 | keep = 1 - self.use_dropout + (self.use_dropout * keep_prob) 252 | return tf.nn.dropout(input, keep, name=name) -------------------------------------------------------------------------------- /GoogLeNet/googlenet.py: -------------------------------------------------------------------------------- 1 | from GoogLeNet.base import Network 2 | 3 | class GoogLeNet(Network): 4 | def setup(self): 5 | (self.feed('data') 6 | .conv(7, 7, 64, 2, 2, name='conv1') 7 | .max_pool(3, 3, 2, 2, name='pool1') 8 | .lrn(2, 2e-05, 0.75, name='norm1') 9 | .conv(1, 1, 64, 1, 1, name='reduction2') 10 | .conv(3, 3, 192, 1, 1, name='conv2') 11 | .lrn(2, 2e-05, 0.75, name='norm2') 12 | .max_pool(3, 3, 2, 2, name='pool2') 13 | .conv(1, 1, 96, 1, 1, name='icp1_reduction1') 14 | .conv(3, 3, 128, 1, 1, name='icp1_out1')) 15 | 16 | (self.feed('pool2') 17 | .conv(1, 1, 16, 1, 1, name='icp1_reduction2') 18 | .conv(5, 5, 32, 1, 1, name='icp1_out2')) 19 | 20 | (self.feed('pool2') 21 | .max_pool(3, 3, 1, 1, name='icp1_pool') 22 | .conv(1, 1, 32, 1, 1, name='icp1_out3')) 23 | 24 | (self.feed('pool2') 25 | .conv(1, 1, 64, 1, 1, name='icp1_out0')) 26 | 27 | (self.feed('icp1_out0', 28 | 'icp1_out1', 29 | 'icp1_out2', 30 | 'icp1_out3') 31 | .concat(3, name='icp2_in') 32 | .conv(1, 1, 128, 1, 1, name='icp2_reduction1') 33 | .conv(3, 3, 192, 1, 1, name='icp2_out1')) 34 | 35 | (self.feed('icp2_in') 36 | .conv(1, 1, 32, 1, 1, name='icp2_reduction2') 37 | .conv(5, 5, 96, 1, 1, name='icp2_out2')) 38 | 39 | (self.feed('icp2_in') 40 | .max_pool(3, 3, 1, 1, name='icp2_pool') 41 | .conv(1, 1, 64, 1, 1, name='icp2_out3')) 42 | 43 | (self.feed('icp2_in') 44 | .conv(1, 1, 128, 1, 1, name='icp2_out0')) 45 | 46 | (self.feed('icp2_out0', 47 | 'icp2_out1', 48 | 'icp2_out2', 49 | 'icp2_out3') 50 | .concat(3, name='icp2_out') 51 | .max_pool(3, 3, 2, 2, name='icp3_in') 52 | .conv(1, 1, 96, 1, 1, name='icp3_reduction1') 53 | .conv(3, 3, 208, 1, 1, name='icp3_out1')) 54 | 55 | (self.feed('icp3_in') 56 | .conv(1, 1, 16, 1, 1, name='icp3_reduction2') 57 | .conv(5, 5, 48, 1, 1, name='icp3_out2')) 58 | 59 | (self.feed('icp3_in') 60 | .max_pool(3, 3, 1, 1, name='icp3_pool') 61 | .conv(1, 1, 64, 1, 1, name='icp3_out3')) 62 | 63 | (self.feed('icp3_in') 64 | .conv(1, 1, 192, 1, 1, name='icp3_out0')) 65 | 66 | (self.feed('icp3_out0', 67 | 'icp3_out1', 68 | 'icp3_out2', 69 | 'icp3_out3') 70 | .concat(3, name='icp3_out') 71 | .avg_pool(5, 5, 3, 3, padding='VALID', name='cls1_pool') 72 | .conv(1, 1, 128, 1, 1, name='cls1_reduction_pose') 73 | .fc(1024, name='cls1_fc1_pose') 74 | .fc(3, relu=False, name='cls1_fc_pose_xyz')) 75 | 76 | (self.feed('cls1_fc1_pose') 77 | .fc(4, relu=False, name='cls1_fc_pose_wpqr')) 78 | 79 | (self.feed('icp3_out') 80 | .conv(1, 1, 112, 1, 1, name='icp4_reduction1') 81 | .conv(3, 3, 224, 1, 1, name='icp4_out1')) 82 | 83 | (self.feed('icp3_out') 84 | .conv(1, 1, 24, 1, 1, name='icp4_reduction2') 85 | .conv(5, 5, 64, 1, 1, name='icp4_out2')) 86 | 87 | (self.feed('icp3_out') 88 | .max_pool(3, 3, 1, 1, name='icp4_pool') 89 | .conv(1, 1, 64, 1, 1, name='icp4_out3')) 90 | 91 | (self.feed('icp3_out') 92 | .conv(1, 1, 160, 1, 1, name='icp4_out0')) 93 | 94 | (self.feed('icp4_out0', 95 | 'icp4_out1', 96 | 'icp4_out2', 97 | 'icp4_out3') 98 | .concat(3, name='icp4_out') 99 | .conv(1, 1, 128, 1, 1, name='icp5_reduction1') 100 | .conv(3, 3, 256, 1, 1, name='icp5_out1')) 101 | 102 | (self.feed('icp4_out') 103 | .conv(1, 1, 24, 1, 1, name='icp5_reduction2') 104 | .conv(5, 5, 64, 1, 1, name='icp5_out2')) 105 | 106 | (self.feed('icp4_out') 107 | .max_pool(3, 3, 1, 1, name='icp5_pool') 108 | .conv(1, 1, 64, 1, 1, name='icp5_out3')) 109 | 110 | (self.feed('icp4_out') 111 | .conv(1, 1, 128, 1, 1, name='icp5_out0')) 112 | 113 | (self.feed('icp5_out0', 114 | 'icp5_out1', 115 | 'icp5_out2', 116 | 'icp5_out3') 117 | .concat(3, name='icp5_out') 118 | .conv(1, 1, 144, 1, 1, name='icp6_reduction1') 119 | .conv(3, 3, 288, 1, 1, name='icp6_out1')) 120 | 121 | (self.feed('icp5_out') 122 | .conv(1, 1, 32, 1, 1, name='icp6_reduction2') 123 | .conv(5, 5, 64, 1, 1, name='icp6_out2')) 124 | 125 | (self.feed('icp5_out') 126 | .max_pool(3, 3, 1, 1, name='icp6_pool') 127 | .conv(1, 1, 64, 1, 1, name='icp6_out3')) 128 | 129 | (self.feed('icp5_out') 130 | .conv(1, 1, 112, 1, 1, name='icp6_out0')) 131 | 132 | (self.feed('icp6_out0', 133 | 'icp6_out1', 134 | 'icp6_out2', 135 | 'icp6_out3') 136 | .concat(3, name='icp6_out') 137 | .avg_pool(5, 5, 3, 3, padding='VALID', name='cls2_pool') 138 | .conv(1, 1, 128, 1, 1, name='cls2_reduction_pose') 139 | .fc(1024, name='cls2_fc1') 140 | .fc(3, relu=False, name='cls2_fc_pose_xyz')) 141 | 142 | (self.feed('cls2_fc1') 143 | .fc(4, relu=False, name='cls2_fc_pose_wpqr')) 144 | 145 | (self.feed('icp6_out') 146 | .conv(1, 1, 160, 1, 1, name='icp7_reduction1') 147 | .conv(3, 3, 320, 1, 1, name='icp7_out1')) 148 | 149 | (self.feed('icp6_out') 150 | .conv(1, 1, 32, 1, 1, name='icp7_reduction2') 151 | .conv(5, 5, 128, 1, 1, name='icp7_out2')) 152 | 153 | (self.feed('icp6_out') 154 | .max_pool(3, 3, 1, 1, name='icp7_pool') 155 | .conv(1, 1, 128, 1, 1, name='icp7_out3')) 156 | 157 | (self.feed('icp6_out') 158 | .conv(1, 1, 256, 1, 1, name='icp7_out0')) 159 | 160 | (self.feed('icp7_out0', 161 | 'icp7_out1', 162 | 'icp7_out2', 163 | 'icp7_out3') 164 | .concat(3, name='icp7_out') 165 | .max_pool(3, 3, 2, 2, name='icp8_in') 166 | .conv(1, 1, 160, 1, 1, name='icp8_reduction1') 167 | .conv(3, 3, 320, 1, 1, name='icp8_out1')) 168 | 169 | (self.feed('icp8_in') 170 | .conv(1, 1, 32, 1, 1, name='icp8_reduction2') 171 | .conv(5, 5, 128, 1, 1, name='icp8_out2')) 172 | 173 | (self.feed('icp8_in') 174 | .max_pool(3, 3, 1, 1, name='icp8_pool') 175 | .conv(1, 1, 128, 1, 1, name='icp8_out3')) 176 | 177 | (self.feed('icp8_in') 178 | .conv(1, 1, 256, 1, 1, name='icp8_out0')) 179 | 180 | (self.feed('icp8_out0', 181 | 'icp8_out1', 182 | 'icp8_out2', 183 | 'icp8_out3') 184 | .concat(3, name='icp8_out') 185 | .conv(1, 1, 192, 1, 1, name='icp9_reduction1') 186 | .conv(3, 3, 384, 1, 1, name='icp9_out1')) 187 | 188 | (self.feed('icp8_out') 189 | .conv(1, 1, 48, 1, 1, name='icp9_reduction2') 190 | .conv(5, 5, 128, 1, 1, name='icp9_out2')) 191 | 192 | (self.feed('icp8_out') 193 | .max_pool(3, 3, 1, 1, name='icp9_pool') 194 | .conv(1, 1, 128, 1, 1, name='icp9_out3')) 195 | 196 | (self.feed('icp8_out') 197 | .conv(1, 1, 384, 1, 1, name='icp9_out0')) 198 | 199 | (self.feed('icp9_out0', 200 | 'icp9_out1', 201 | 'icp9_out2', 202 | 'icp9_out3') 203 | .concat(3, name='icp9_out') 204 | .avg_pool(7, 7, 1, 1, padding='VALID', name='cls3_pool') 205 | .fc(2048, name='cls3_fc1_pose') 206 | .fc(3, relu=False, name='cls3_fc_pose_xyz')) 207 | 208 | (self.feed('cls3_fc1_pose') 209 | .fc(4, relu=False, name='cls3_fc_pose_wpqr')) -------------------------------------------------------------------------------- /GoogLeNet/googlenet_fc_big.py: -------------------------------------------------------------------------------- 1 | from GoogLeNet.base import Network 2 | 3 | class GoogLeNetFCB(Network): 4 | def setup(self): 5 | (self.feed('data') 6 | .conv(7, 7, 64, 2, 2, name='conv1') 7 | .max_pool(3, 3, 2, 2, name='pool1') 8 | .lrn(2, 2e-05, 0.75, name='norm1') 9 | .conv(1, 1, 64, 1, 1, name='reduction2') 10 | .conv(3, 3, 192, 1, 1, name='conv2') 11 | .lrn(2, 2e-05, 0.75, name='norm2') 12 | .max_pool(3, 3, 2, 2, name='pool2') 13 | .conv(1, 1, 96, 1, 1, name='icp1_reduction1') 14 | .conv(3, 3, 128, 1, 1, name='icp1_out1')) 15 | 16 | (self.feed('pool2') 17 | .conv(1, 1, 16, 1, 1, name='icp1_reduction2') 18 | .conv(5, 5, 32, 1, 1, name='icp1_out2')) 19 | 20 | (self.feed('pool2') 21 | .max_pool(3, 3, 1, 1, name='icp1_pool') 22 | .conv(1, 1, 32, 1, 1, name='icp1_out3')) 23 | 24 | (self.feed('pool2') 25 | .conv(1, 1, 64, 1, 1, name='icp1_out0')) 26 | 27 | (self.feed('icp1_out0', 28 | 'icp1_out1', 29 | 'icp1_out2', 30 | 'icp1_out3') 31 | .concat(3, name='icp2_in') 32 | .conv(1, 1, 128, 1, 1, name='icp2_reduction1') 33 | .conv(3, 3, 192, 1, 1, name='icp2_out1')) 34 | 35 | (self.feed('icp2_in') 36 | .conv(1, 1, 32, 1, 1, name='icp2_reduction2') 37 | .conv(5, 5, 96, 1, 1, name='icp2_out2')) 38 | 39 | (self.feed('icp2_in') 40 | .max_pool(3, 3, 1, 1, name='icp2_pool') 41 | .conv(1, 1, 64, 1, 1, name='icp2_out3')) 42 | 43 | (self.feed('icp2_in') 44 | .conv(1, 1, 128, 1, 1, name='icp2_out0')) 45 | 46 | (self.feed('icp2_out0', 47 | 'icp2_out1', 48 | 'icp2_out2', 49 | 'icp2_out3') 50 | .concat(3, name='icp2_out') 51 | .max_pool(3, 3, 2, 2, name='icp3_in') 52 | .conv(1, 1, 96, 1, 1, name='icp3_reduction1') 53 | .conv(3, 3, 208, 1, 1, name='icp3_out1')) 54 | 55 | (self.feed('icp3_in') 56 | .conv(1, 1, 16, 1, 1, name='icp3_reduction2') 57 | .conv(5, 5, 48, 1, 1, name='icp3_out2')) 58 | 59 | (self.feed('icp3_in') 60 | .max_pool(3, 3, 1, 1, name='icp3_pool') 61 | .conv(1, 1, 64, 1, 1, name='icp3_out3')) 62 | 63 | (self.feed('icp3_in') 64 | .conv(1, 1, 192, 1, 1, name='icp3_out0')) 65 | 66 | (self.feed('icp3_out0', 67 | 'icp3_out1', 68 | 'icp3_out2', 69 | 'icp3_out3') 70 | .concat(3, name='icp3_out') 71 | .avg_pool(5, 5, 3, 3, padding='VALID', name='cls1_pool') 72 | .conv(1, 1, 128, 1, 1, name='cls1_reduction_pose') 73 | .fc(1024, name='cls1_fc1')) 74 | 75 | (self.feed('icp3_out') 76 | .conv(1, 1, 112, 1, 1, name='icp4_reduction1') 77 | .conv(3, 3, 224, 1, 1, name='icp4_out1')) 78 | 79 | (self.feed('icp3_out') 80 | .conv(1, 1, 24, 1, 1, name='icp4_reduction2') 81 | .conv(5, 5, 64, 1, 1, name='icp4_out2')) 82 | 83 | (self.feed('icp3_out') 84 | .max_pool(3, 3, 1, 1, name='icp4_pool') 85 | .conv(1, 1, 64, 1, 1, name='icp4_out3')) 86 | 87 | (self.feed('icp3_out') 88 | .conv(1, 1, 160, 1, 1, name='icp4_out0')) 89 | 90 | (self.feed('icp4_out0', 91 | 'icp4_out1', 92 | 'icp4_out2', 93 | 'icp4_out3') 94 | .concat(3, name='icp4_out') 95 | .conv(1, 1, 128, 1, 1, name='icp5_reduction1') 96 | .conv(3, 3, 256, 1, 1, name='icp5_out1')) 97 | 98 | (self.feed('icp4_out') 99 | .conv(1, 1, 24, 1, 1, name='icp5_reduction2') 100 | .conv(5, 5, 64, 1, 1, name='icp5_out2')) 101 | 102 | (self.feed('icp4_out') 103 | .max_pool(3, 3, 1, 1, name='icp5_pool') 104 | .conv(1, 1, 64, 1, 1, name='icp5_out3')) 105 | 106 | (self.feed('icp4_out') 107 | .conv(1, 1, 128, 1, 1, name='icp5_out0')) 108 | 109 | (self.feed('icp5_out0', 110 | 'icp5_out1', 111 | 'icp5_out2', 112 | 'icp5_out3') 113 | .concat(3, name='icp5_out') 114 | .conv(1, 1, 144, 1, 1, name='icp6_reduction1') 115 | .conv(3, 3, 288, 1, 1, name='icp6_out1')) 116 | 117 | (self.feed('icp5_out') 118 | .conv(1, 1, 32, 1, 1, name='icp6_reduction2') 119 | .conv(5, 5, 64, 1, 1, name='icp6_out2')) 120 | 121 | (self.feed('icp5_out') 122 | .max_pool(3, 3, 1, 1, name='icp6_pool') 123 | .conv(1, 1, 64, 1, 1, name='icp6_out3')) 124 | 125 | (self.feed('icp5_out') 126 | .conv(1, 1, 112, 1, 1, name='icp6_out0')) 127 | 128 | (self.feed('icp6_out0', 129 | 'icp6_out1', 130 | 'icp6_out2', 131 | 'icp6_out3') 132 | .concat(3, name='icp6_out') 133 | .avg_pool(5, 5, 3, 3, padding='VALID', name='cls2_pool') 134 | .conv(1, 1, 128, 1, 1, name='cls2_reduction_pose') 135 | .fc(1024, name='cls2_fc1')) 136 | 137 | (self.feed('icp6_out') 138 | .conv(1, 1, 160, 1, 1, name='icp7_reduction1') 139 | .conv(3, 3, 320, 1, 1, name='icp7_out1')) 140 | 141 | (self.feed('icp6_out') 142 | .conv(1, 1, 32, 1, 1, name='icp7_reduction2') 143 | .conv(5, 5, 128, 1, 1, name='icp7_out2')) 144 | 145 | (self.feed('icp6_out') 146 | .max_pool(3, 3, 1, 1, name='icp7_pool') 147 | .conv(1, 1, 128, 1, 1, name='icp7_out3')) 148 | 149 | (self.feed('icp6_out') 150 | .conv(1, 1, 256, 1, 1, name='icp7_out0')) 151 | 152 | (self.feed('icp7_out0', 153 | 'icp7_out1', 154 | 'icp7_out2', 155 | 'icp7_out3') 156 | .concat(3, name='icp7_out') 157 | .max_pool(3, 3, 2, 2, name='icp8_in') 158 | .conv(1, 1, 160, 1, 1, name='icp8_reduction1') 159 | .conv(3, 3, 320, 1, 1, name='icp8_out1')) 160 | 161 | (self.feed('icp8_in') 162 | .conv(1, 1, 32, 1, 1, name='icp8_reduction2') 163 | .conv(5, 5, 128, 1, 1, name='icp8_out2')) 164 | 165 | (self.feed('icp8_in') 166 | .max_pool(3, 3, 1, 1, name='icp8_pool') 167 | .conv(1, 1, 128, 1, 1, name='icp8_out3')) 168 | 169 | (self.feed('icp8_in') 170 | .conv(1, 1, 256, 1, 1, name='icp8_out0')) 171 | 172 | (self.feed('icp8_out0', 173 | 'icp8_out1', 174 | 'icp8_out2', 175 | 'icp8_out3') 176 | .concat(3, name='icp8_out') 177 | .conv(1, 1, 192, 1, 1, name='icp9_reduction1') 178 | .conv(3, 3, 384, 1, 1, name='icp9_out1')) 179 | 180 | (self.feed('icp8_out') 181 | .conv(1, 1, 48, 1, 1, name='icp9_reduction2') 182 | .conv(5, 5, 128, 1, 1, name='icp9_out2')) 183 | 184 | (self.feed('icp8_out') 185 | .max_pool(3, 3, 1, 1, name='icp9_pool') 186 | .conv(1, 1, 128, 1, 1, name='icp9_out3')) 187 | 188 | (self.feed('icp8_out') 189 | .conv(1, 1, 384, 1, 1, name='icp9_out0')) 190 | 191 | (self.feed('icp9_out0', 192 | 'icp9_out1', 193 | 'icp9_out2', 194 | 'icp9_out3') 195 | .concat(3, name='icp9_out') 196 | .avg_pool(7, 7, 1, 1, padding='VALID', name='cls3_pool') 197 | .fc(2048, name='cls3_fc1')) -------------------------------------------------------------------------------- /GoogLeNet/weights/posenet.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ensv/RPNet/f4b3fbf510d47801e5b4a1e8b9001a5bb3038a1b/GoogLeNet/weights/posenet.npy -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RPNet: an End-to-End Network for Relative Camera Pose Estimation 2 | 3 | # Setting up the environement: 4 | - Python3 5 | - Tensorflow 1.6+ 6 | - Numpy 7 | - Scipy 8 | - matplotlib 9 | - skimage 10 | - opencv 11 | - pyquaternion (http://kieranwynn.github.io/pyquaternion/) 12 | - mayavi (for toy_example) 13 | 14 | # Data preparation 15 | 1. change the log_dir in abstract_network/setting.py 16 | 2. cd data 17 | 3. python3 absolute_cambridge.py # for absolute pose estimation 18 | 4. python3 relative_cambridge.py # for relative pose estimation 19 | 20 | If you wish to have different train and test set from our experiments, delete train_set.txt, test_set.txt and validation_set.txt in log_dir/relative_cambridge/dataset_name/ 21 | 22 | # Training: 23 | To train the network on the four datasets of Cambridge with default (best) parameters, simply run the following command: 24 | 25 | ```sh 26 | $ python3 posenet.py --train_test_phase=train # in posenet folder to train posenet 27 | ``` 28 | 29 | or 30 | 31 | ```sh 32 | $ python3 rpnetplus.py --train_test_phase=train # in rpnet folder to train posenet 33 | ``` 34 | Each training will be logged at $log_dir/absolute_cambridge_network or $log_dir/relative_cambridge_network. 35 | 36 | To see all the customizable parameters for each model, run with "--help" option. 37 | 38 | ```sh 39 | $ python3 rpnetplus.py --help 40 | ``` 41 | Train test split: https://goo.gl/vv3zxB 42 | 43 | # Results on Cambridge Dataset using PoseNet 44 | 45 | | | Paper | This implementation | 46 | |------------------|-------------|---------------------| 47 | | King's Colleges | 1.92m, 5.4 | 1.93m, 3.12 | 48 | | Old Hopistal | 2.31m, 5.40 | 2.41m, 4.81 | 49 | | Shop Facade | 1.46, 8.0 | 1.68, 7.07 | 50 | | St Mary's church | 2.65, 8.48 | 2.29, 5.90 | 51 | 52 | # Evaluation: 53 | To evaluate the trained network, simply run the following command: 54 | ```sh 55 | $ python3 rpnetplus.py --train_test_phase=test 56 | ``` 57 | It will generate a *.pkl file saving T(m), R(d) and T(d) in each conguration. 58 | 59 | # Toy example 60 | 61 | The relative pose is computed in the second camera's reference system, following OpenCV. To better understand how the to compute these values, as well as, the importance of different parameters (focal length, principle point ...), you can work on our toy example. 62 | 63 | ![3D rendering of the two camera poses and its projected points on the virtual image plan.](https://github.com/ensv/RPNet.git/toy_example/toy_example.png) 64 | 65 | # Notice 66 | All the codes and resources in GoogLeNet folder are mostly based on the work of https://github.com/kentsommer/tensorflow-posenet. 67 | -------------------------------------------------------------------------------- /abstract_network/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ensv/RPNet/f4b3fbf510d47801e5b4a1e8b9001a5bb3038a1b/abstract_network/__init__.py -------------------------------------------------------------------------------- /abstract_network/abstract.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jul 18, 2018 3 | 4 | @author: en 5 | ''' 6 | 7 | import tensorflow as tf 8 | import os 9 | import numpy as np 10 | from sklearn.externals import joblib 11 | from tensorflow.python.framework import ops 12 | from tensorflow.python.framework.errors import OutOfRangeError, InvalidArgumentError 13 | 14 | class AbstractNetwork(object): 15 | ''' 16 | classdocs 17 | ''' 18 | 19 | def __init__(self, setting, flags): 20 | 21 | 22 | # optimization params 23 | self.optimization = flags.optimization 24 | self.lr = flags.lr 25 | self.lr_decay_rate = flags.lr_decay_rate 26 | self.lr_decay_step = flags.lr_decay_step 27 | self.batch_size = flags.batch_size 28 | self.train_test_phase= flags.train_test_phase 29 | self.nb_epoches = flags.nb_epoches 30 | self.use_dropout = flags.use_dropout 31 | 32 | if hasattr(flags, 'regressor_dimension'): 33 | self.regressor_dimension = flags.regressor_dimension 34 | else: 35 | self.regressor_dimension = 1024 36 | 37 | # name of the model, checkpoint, result file 38 | self.network_name = flags.name 39 | self.experiment = flags.experiment 40 | 41 | self.dataset = flags.dataset 42 | self.stddev = 5e-2 43 | 44 | self.eval_interval_epoch = flags.eval_interval_epoch 45 | self.nb_run = flags.nb_run 46 | 47 | self.valid_result_fn = 'result_valid' 48 | self.test_result_fn = 'result_test' 49 | self.continue_training = flags.continue_training 50 | 51 | self.setting = setting 52 | self.max_iteration = 100000000000000 53 | self.prefetch_data = 500 54 | self.eval_interval_epoch = 50 55 | 56 | self.asyn_train = False 57 | 58 | self.cambridge_subset = ['KingsCollege', 'OldHospital', 'ShopFacade', 'StMarysChurch'] 59 | 60 | 61 | def _try_create_directory(self, path): 62 | try: os.mkdir(path) 63 | except: pass 64 | 65 | def _train_dir_path(self, suffix = '', lr_factor = 1., data_path_ = None): 66 | 67 | if data_path_ is None: 68 | path = self.setting.data_path 69 | else: 70 | path = data_path_ 71 | 72 | path += '/%s/'%self.dataset 73 | self._try_create_directory(path) 74 | 75 | path += '%s/'%(self.network_name) 76 | self._try_create_directory(path) 77 | 78 | path += '%s/'%(self.experiment) 79 | self._try_create_directory(path) 80 | 81 | path += '%s_%d_%0.6f_%0.2f_%d_%d%s'%(self.optimization, self.batch_size, lr_factor*self.lr, 82 | self.lr_decay_rate, self.lr_decay_step, self.nb_epoches, suffix) 83 | 84 | self._try_create_directory(path) 85 | 86 | for i in range(0, self.nb_run): 87 | tmp_path = path + '/%d'%i 88 | if self.train_test_phase == 'train': 89 | if self.asyn_train: 90 | return tmp_path 91 | if os.path.exists(tmp_path): 92 | if self.continue_training: 93 | if os.path.exists(tmp_path + '/done.txt'): 94 | continue 95 | else: 96 | with open(tmp_path + '/done.txt', 'w') as f: 97 | f.write('\n') 98 | return tmp_path 99 | else: 100 | continue 101 | else: 102 | self._try_create_directory(tmp_path) 103 | return tmp_path 104 | else : 105 | if os.path.exists(tmp_path + '/done.txt') and self.checkpoint_exists(tmp_path): 106 | tmp_fn = self.test_result_fn if self.train_test_phase =='test' else self.valid_result_fn 107 | 108 | print('**********************************') 109 | if os.path.exists(tmp_path + '/%s.pkl'%tmp_fn): 110 | print('--- \t result file exists') 111 | continue 112 | else: 113 | joblib.dump('1', tmp_path + '/%s.pkl'%tmp_fn, compress=3) 114 | return tmp_path 115 | return '' 116 | 117 | def checkpoint_exists(self, folder): 118 | for e in os.listdir(folder): 119 | if 'model.ckpt' in e: 120 | return True 121 | return False 122 | 123 | def prepare_inference(self): 124 | raise NotImplementedError 125 | 126 | def prepare_loss(self): 127 | raise NotImplementedError 128 | 129 | def before_valid_test(self, sess): 130 | raise NotImplementedError 131 | 132 | def before_train(self, sess): 133 | raise NotImplementedError 134 | 135 | def _get_train_test_set(self, folder_path = '../data/'): 136 | raise NotImplementedError 137 | 138 | def get_train_op(self, nb_replicas = 0): 139 | 140 | lr = tf.train.exponential_decay(self.lr, self.global_step, self.lr_decay_step * self.nb_iterations, self.lr_decay_rate, staircase=True) 141 | tf.summary.scalar('learning_rate', lr) # @UndefinedVariable 142 | 143 | self.total_loss = tf.add_n(tf.get_collection('losses'), name='total_loss') 144 | tf.summary.scalar('Train/total_loss', self.total_loss) # @UndefinedVariable 145 | 146 | if self.optimization == 'sgd': 147 | self.optimizer = tf.train.AdamOptimizer() 148 | elif self.optimization == 'sgdm': 149 | self.optimizer = tf.train.MomentumOptimizer(lr, 0.9, use_nesterov = True) 150 | elif self.optimization == 'adagrad': 151 | self.optimizer = tf.train.AdagradOptimizer(lr) 152 | else: 153 | raise NotImplementedError() 154 | 155 | if nb_replicas >0: 156 | self.optimizer = tf.train.SyncReplicasOptimizer(self.optimizer, replicas_to_aggregate=nb_replicas, total_num_replicas=nb_replicas) 157 | 158 | grads = self.optimizer.compute_gradients(self.total_loss) 159 | 160 | # Apply gradients. 161 | self.train_op = self.optimizer.apply_gradients(grads, global_step= self.global_step) 162 | 163 | # Add histograms for gradients, save only one just to see how small/big it is to adapt the lr 164 | # for grad, var in grads: 165 | # if grad is not None: 166 | # tf.summary.histogram(var.op.name + '/gradients', grad) # @UndefinedVariable 167 | # break 168 | 169 | self.summary_op = tf.summary.merge_all() # @UndefinedVariable 170 | 171 | 172 | def _get_nb_lines(self): 173 | 174 | if type(self.train_filenames) == type([]): 175 | return len(open(self.train_filenames[0].replace('train.tfrecord', 'train_set.txt'), 'r').readlines()) 176 | 177 | return len(open(self.train_filenames.replace('train.tfrecord', 'train_set.txt'), 'r').readlines()) 178 | 179 | 180 | def prepare_data(self, train_fn, test_valid_fn): 181 | 182 | if self.train_test_phase == 'train': 183 | dataset = tf.data.TFRecordDataset(self.train_filenames, num_parallel_reads=10) 184 | dataset = dataset.shuffle(self.setting.dataset_shuffle_size) 185 | dataset = dataset.map(train_fn, num_parallel_calls = 5) 186 | dataset = dataset.repeat(self.eval_interval_epoch) 187 | dataset = dataset.batch(self.batch_size) 188 | dataset = dataset.prefetch(self.prefetch_data) 189 | else: 190 | dataset = tf.data.TFRecordDataset(self.test_filenames) 191 | dataset = dataset.map(test_valid_fn) 192 | dataset = dataset.batch(self.batch_size) 193 | dataset = dataset.prefetch(self.prefetch_data) 194 | 195 | valid_dataset = tf.data.TFRecordDataset(self.valid_filenames) 196 | valid_dataset = valid_dataset.map(test_valid_fn) 197 | valid_dataset = valid_dataset.batch(self.batch_size) 198 | 199 | self.iterator = tf.data.Iterator.from_structure(dataset.output_types, dataset.output_shapes) 200 | self.training_init_op = self.iterator.make_initializer(dataset) 201 | self.validation_init_op = self.iterator.make_initializer(valid_dataset) 202 | 203 | if hasattr(self, 'use_extraLoss'): 204 | if self.use_extraLoss: 205 | self.input_X, self.input_Y, self.extra_input_Y1, self.extra_input_Y2 = self.iterator.get_next() 206 | else: 207 | self.input_X, self.input_Y = self.iterator.get_next() 208 | else: 209 | self.input_X, self.input_Y = self.iterator.get_next() 210 | 211 | self.global_step = tf.Variable(0, dtype='int64', trainable = False, name ='global_step') 212 | self.nb_iterations = self._get_nb_lines() // self.batch_size 213 | 214 | 215 | def _custom_evaluation(self, sess): 216 | 217 | # example of prototype 218 | # loss = [] 219 | # while True: 220 | # try: 221 | # loss.append(sess.run(self.tran_loss)) 222 | # except OutOfRangeError: 223 | # break 224 | # 225 | # # validation loss 226 | # loss = np.array(loss) 227 | # joblib.dump((loss, loss.mean()), '%s/%s.pkl'%(self.train_dir, self.test_result_fn), compress=3) 228 | raise NotImplementedError 229 | 230 | 231 | def evaluate_test(self): 232 | 233 | init = tf.global_variables_initializer() # @UndefinedVariable 234 | sess = tf.Session(config=tf.ConfigProto(log_device_placement=False, allow_soft_placement=True, 235 | gpu_options=tf.GPUOptions(per_process_gpu_memory_fraction=0.95))) 236 | sess.run(init) 237 | saver = tf.train.Saver(tf.global_variables(), max_to_keep = 5) # @UndefinedVariable 238 | 239 | saver.restore(sess, tf.train.latest_checkpoint(self.train_dir)) 240 | sess.run(self.training_init_op) 241 | 242 | self.before_valid_test(sess) 243 | self._custom_evaluation(sess) 244 | sess.close() 245 | ops.reset_default_graph() 246 | 247 | def evaluate_validation(self, sess): 248 | raise NotImplementedError 249 | 250 | 251 | def _define_additional_summaries(self): 252 | raise NotImplementedError 253 | 254 | 255 | def _load_pretrained_weight(self, sess): 256 | raise NotImplementedError 257 | 258 | 259 | def train(self): 260 | 261 | self._define_additional_summaries() 262 | init = tf.global_variables_initializer() # @UndefinedVariable 263 | 264 | sess = tf.Session(config=tf.ConfigProto(log_device_placement=False, allow_soft_placement=True, 265 | gpu_options=tf.GPUOptions(per_process_gpu_memory_fraction=0.95))) 266 | sess.run(init) 267 | 268 | self._load_pretrained_weight(sess) 269 | 270 | saver = tf.train.Saver(tf.global_variables(), max_to_keep = 30) # @UndefinedVariable 271 | 272 | self.summary_writer = tf.summary.FileWriter(self.train_dir, sess.graph) # @UndefinedVariable 273 | 274 | start_epoch = 0 275 | if tf.train.latest_checkpoint(self.train_dir) is not None: 276 | saver.restore(sess, tf.train.latest_checkpoint(self.train_dir)) 277 | start_epoch = int(tf.train.latest_checkpoint(self.train_dir).split('/')[-1].replace('model.ckpt-', '')) 278 | print ('resuming training from ', start_epoch) 279 | start_epoch = int(start_epoch) 280 | 281 | iters = sess.run(self.global_step) 282 | for epoch in range(start_epoch, self.nb_epoches, self.eval_interval_epoch): 283 | 284 | sess.run(self.training_init_op) 285 | self.before_train(sess) 286 | # train for half epoch 287 | while True: 288 | try: 289 | iters += 1 290 | if iters % 1000 == 0: 291 | try: 292 | summary_str, loss = sess.run([self.summary_op, self.total_loss]) 293 | self.summary_writer.add_summary(summary_str, iters) 294 | except InvalidArgumentError: # nan in summary graident 295 | self.done_training('nan in loss') 296 | sess.close() 297 | ops.reset_default_graph() 298 | return 299 | 300 | if np.isnan(loss): 301 | self.done_training('nan in loss') 302 | sess.close() 303 | ops.reset_default_graph() 304 | return 305 | else: 306 | sess.run(self.train_op) 307 | except OutOfRangeError: 308 | break 309 | 310 | if epoch % self.eval_interval_epoch != 0: continue 311 | self.evaluate_validation(sess) 312 | saver.save(sess, '%s/model.ckpt'%self.train_dir, global_step=epoch) 313 | if iters >= self.max_iteration: 314 | 315 | print('break, iters >= self.max_iteration') 316 | break 317 | self.evaluate_validation(sess) 318 | saver.save(sess, '%s/model.ckpt'%self.train_dir, global_step=self.nb_epoches) 319 | 320 | self.done_training() 321 | sess.close() 322 | ops.reset_default_graph() 323 | 324 | def done_training(self, message = 'finished'): 325 | with open('%s/done.txt'%self.train_dir, 'w') as f: 326 | f.write('done\n') 327 | f.write(message) 328 | 329 | def process(self): 330 | 331 | if self.train_dir == '': 332 | ops.reset_default_graph() 333 | print('train_dir = "", default graph has been reset ...') 334 | return 335 | print('\n') 336 | print('train_dir: %s'%self.train_dir) 337 | 338 | if self.train_test_phase == 'train': 339 | self.train() 340 | elif self.train_test_phase == 'test': 341 | self.evaluate_test() 342 | ops.reset_default_graph() 343 | 344 | #################################################### Specific to this project ######################################## 345 | def _l2_norm_loss(self, predicts, labels, weights, scope =''): 346 | if scope == '': 347 | loss = tf.reduce_mean(tf.sqrt(tf.reduce_sum(tf.square(labels - predicts), axis=1))) 348 | tf.add_to_collection('losses', loss*weights) 349 | else: 350 | loss = tf.reduce_mean(tf.sqrt(tf.reduce_sum(tf.square(labels - predicts), axis=1)), name = scope) 351 | tf.summary.scalar(scope, loss) # @UndefinedVariable 352 | tf.add_to_collection('losses', loss*weights) 353 | return loss * weights 354 | 355 | def quat_2_matrix_tf(self, q): 356 | a, b, c, d = q[:, 0:1], q[:, 1:2], q[:, 2:3], q[:, 3:] 357 | asq, bsq, csq, dsq = a**2, b**2, c**2, d**2 358 | ab, ac, ad = 2*a*b, 2*a*c, 2*a*d 359 | bc, bd = 2*b*c, 2*b*d 360 | cd = 2*c*d 361 | mat = [asq + bsq - csq -dsq, bc - ad, bd + ac, \ 362 | bc + ad, asq -bsq + csq -dsq, cd - ab, \ 363 | bd - ac, cd + ab, asq - bsq - csq + dsq] 364 | 365 | mat = tf.reshape(tf.concat(mat, axis=1), (-1, 3, 3)) 366 | return mat 367 | 368 | def get_relative_T_in_cam2_ref(self, R2, T1, T2): 369 | # T1, T2, shape: (batch, 3) 370 | 371 | if self.dataset in self.cambridge_subset: 372 | matrix = self.quat_2_matrix_tf(R2) 373 | new_c2 = - tf.matmul(matrix, tf.reshape(T2, (-1, 3, 1))) 374 | return tf.reshape(tf.matmul(matrix, tf.reshape(T1, (-1, 3, 1))) + new_c2 , (-1, 3, )) 375 | elif 'greyc' in self.dataset.lower(): 376 | matrix = self.quat_2_matrix_tf(R2) 377 | return tf.reshape(tf.matmul(matrix, tf.reshape((T1-T2), (-1, 3, 1)), True), (-1, 3)) 378 | 379 | 380 | 381 | def get_quaternion_rotation(self, pose1, pose2, name_): 382 | 383 | if 'greyc' in self.dataset.lower(): 384 | a1, b1, c1, d1 = pose1[:, 0:1], pose1[:, 1:2], pose1[:, 2:3], pose1[:, 3:] 385 | a2, b2, c2, d2 = pose2[:, 0:1], -1*pose2[:, 1:2], -1*pose2[:, 2:3], -1*pose2[:, 3:] 386 | elif self.dataset in self.cambridge_subset: 387 | a1, b1, c1, d1 = pose1[:, 0:1], -1*pose1[:, 1:2], -1*pose1[:, 2:3], -1*pose1[:, 3:] 388 | a2, b2, c2, d2 = pose2[:, 0:1], pose2[:, 1:2], pose2[:, 2:3], pose2[:, 3:] 389 | 390 | pose = tf.concat( ((a2*a1 - b2*b1 - c2*c1 - d2*d1), 391 | (a2*b1 + b2*a1 + c2*d1 - d2*c1), 392 | (a2*c1 - b2*d1 + c2*a1 + d2*b1), 393 | (a2*d1 + b2*c1 - c2*b1 + d2*a1) ), axis=1, name = name_) 394 | return pose 395 | 396 | def _define_regressor(self, net): 397 | 398 | # cls 1 399 | with tf.variable_scope("cls1"): 400 | x = tf.layers.flatten(net.layers['cls1_reduction_pose']) 401 | fc1 = tf.layers.dense(x, self.regressor_dimension, activation=tf.nn.relu, use_bias=True, # @UndefinedVariable 402 | kernel_initializer=tf.truncated_normal_initializer(stddev=self.stddev, dtype=tf.float32), 403 | bias_initializer=tf.constant_initializer(0.01, dtype=tf.float32), 404 | # kernel_regularizer=l2_regularizer(self.wd), 405 | # bias_regularizer=l2_regularizer(self.wd), 406 | name='cls1_fc1') 407 | 408 | 409 | aux1_xyz_logits = tf.layers.dense(fc1, 3, activation=None, use_bias=True, # @UndefinedVariable 410 | kernel_initializer=tf.truncated_normal_initializer(stddev=self.stddev, dtype=tf.float32), 411 | bias_initializer=tf.constant_initializer(0.01, dtype=tf.float32), 412 | # kernel_regularizer=l2_regularizer(self.wd), 413 | # bias_regularizer=l2_regularizer(self.wd), 414 | name='cls1_fc1_xyz') 415 | 416 | aux1_wpqr_logits = tf.layers.dense(fc1, 4, activation=None, use_bias=True, # @UndefinedVariable 417 | kernel_initializer=tf.truncated_normal_initializer(stddev=self.stddev, dtype=tf.float32), 418 | bias_initializer=tf.constant_initializer(0.01, dtype=tf.float32), 419 | # kernel_regularizer=l2_regularizer(self.wd), 420 | # bias_regularizer=l2_regularizer(self.wd), 421 | name='cls1_fc1_wpqr') 422 | # cls 2 423 | with tf.variable_scope("cls2"): 424 | x = tf.layers.flatten(net.layers['cls2_reduction_pose']) 425 | fc1 = tf.layers.dense(x, self.regressor_dimension, activation=tf.nn.relu, use_bias=True, # @UndefinedVariable 426 | kernel_initializer=tf.truncated_normal_initializer(stddev=self.stddev, dtype=tf.float32), 427 | bias_initializer=tf.constant_initializer(0.01, dtype=tf.float32), 428 | # kernel_regularizer=l2_regularizer(self.wd), 429 | # bias_regularizer=l2_regularizer(self.wd), 430 | name='cls2_fc1') 431 | 432 | 433 | aux2_xyz_logits = tf.layers.dense(fc1, 3, activation=None, use_bias=True, # @UndefinedVariable 434 | kernel_initializer=tf.truncated_normal_initializer(stddev=self.stddev, dtype=tf.float32), 435 | bias_initializer=tf.constant_initializer(0.01, dtype=tf.float32), 436 | # kernel_regularizer=l2_regularizer(self.wd), 437 | # bias_regularizer=l2_regularizer(self.wd), 438 | name='cls2_fc1_xyz') 439 | 440 | aux2_wpqr_logits = tf.layers.dense(fc1, 4, activation=None, use_bias=True, # @UndefinedVariable 441 | kernel_initializer=tf.truncated_normal_initializer(stddev=self.stddev, dtype=tf.float32), 442 | bias_initializer=tf.constant_initializer(0.01, dtype=tf.float32), 443 | # kernel_regularizer=l2_regularizer(self.wd), 444 | # bias_regularizer=l2_regularizer(self.wd), 445 | name='cls2_fc1_wpqr') 446 | # cls 3 447 | with tf.variable_scope("cls3"): 448 | x = tf.layers.flatten(net.layers['cls3_pool']) 449 | fc1 = tf.layers.dense(x, 2*self.regressor_dimension, activation=tf.nn.relu, use_bias=True, # @UndefinedVariable 450 | kernel_initializer=tf.truncated_normal_initializer(stddev=self.stddev, dtype=tf.float32), 451 | bias_initializer=tf.constant_initializer(0.01, dtype=tf.float32), 452 | # kernel_regularizer=l2_regularizer(self.wd), 453 | # bias_regularizer=l2_regularizer(self.wd), 454 | name='cls3_fc1') 455 | 456 | xyz_logits = tf.layers.dense(fc1, 3, activation=None, use_bias=True, # @UndefinedVariable 457 | kernel_initializer=tf.truncated_normal_initializer(stddev=self.stddev, dtype=tf.float32), 458 | bias_initializer=tf.constant_initializer(0.01, dtype=tf.float32), 459 | # kernel_regularizer=l2_regularizer(self.wd), 460 | # bias_regularizer=l2_regularizer(self.wd), 461 | name='cls3_fc1_xyz') 462 | 463 | wpqr_logits = tf.layers.dense(fc1, 4, activation=None, use_bias=True, # @UndefinedVariable 464 | kernel_initializer=tf.truncated_normal_initializer(stddev=self.stddev, dtype=tf.float32), 465 | bias_initializer=tf.constant_initializer(0.01, dtype=tf.float32), 466 | # kernel_regularizer=l2_regularizer(self.wd), 467 | # bias_regularizer=l2_regularizer(self.wd), 468 | name='cls3_fc1_wpqr') 469 | 470 | return xyz_logits, wpqr_logits, aux2_xyz_logits, aux2_wpqr_logits, aux1_xyz_logits, aux1_wpqr_logits 471 | -------------------------------------------------------------------------------- /abstract_network/setting.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jul 18, 2018 3 | 4 | @author: sovann 5 | ''' 6 | 7 | import os 8 | import socket 9 | 10 | 11 | class Setting(): 12 | 13 | def __init__(self, folder_name): 14 | 15 | # path to store all the data, train log and other results 16 | log_dir = '/data/chercheurs/en/RPNet/' 17 | # dataset 18 | self.dataset_shuffle_size = 2500 # dataset shuffle buffle size 19 | self.hostname = socket.gethostname() 20 | self.projectname = folder_name 21 | 22 | self.data_path = '%s/%s'%(log_dir, folder_name) 23 | print('data path is set to be ', self.data_path) 24 | try: os.makedirs(self.data_path) 25 | except: pass 26 | 27 | 28 | if __name__ == '__main__': 29 | pass 30 | -------------------------------------------------------------------------------- /data/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ensv/RPNet/f4b3fbf510d47801e5b4a1e8b9001a5bb3038a1b/data/__init__.py -------------------------------------------------------------------------------- /data/absolute_cambridge.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jul 19, 2018 3 | 4 | @author: sovann 5 | ''' 6 | import sys, os 7 | import cv2 8 | sys.path.append(os.path.dirname(os.path.realpath(__file__)) + '/../') 9 | from data.abstract import AbstractData 10 | import numpy as np 11 | from abstract_network.setting import Setting 12 | 13 | class AbsoluteCambridge(AbstractData): 14 | 15 | def __init__(self, name_): 16 | self.name = name_ 17 | 18 | self.setting = Setting('absolute_cambridge') 19 | dir_ = self.setting.data_path + '/' + name_ 20 | AbstractData.__init__(self, name_, dir_) 21 | 22 | def _maybe_download(self): 23 | 24 | if os.path.exists('%s/KingsCollege/dataset_train.txt'%self.setting.data_path): return 25 | 26 | current_dir = os.getcwd() 27 | os.chdir(self.setting.data_path) 28 | 29 | links = [ 30 | 'https://www.repository.cam.ac.uk/bitstream/handle/1810/251342/KingsCollege.zip', 31 | 'https://www.repository.cam.ac.uk/bitstream/handle/1810/251340/OldHospital.zip', 32 | 'https://www.repository.cam.ac.uk/bitstream/handle/1810/251336/ShopFacade.zip', 33 | 'https://www.repository.cam.ac.uk/bitstream/handle/1810/251294/StMarysChurch.zip'] 34 | 35 | for each in links: 36 | os.system('wget %s'%each) 37 | os.system("unzip '*.zip'") 38 | os.system(" find . -name '*.mp4' -type f -delete") 39 | os.system("rm -rf *.zip") 40 | 41 | os.chdir(current_dir) 42 | 43 | 44 | def write_2_text(self, annot, name): 45 | f = open('%s/%s_set.txt'%(self.path, name), 'w') 46 | f.write('Visual Landmark Dataset V1\nImageFile, Camera Position [X Y Z W P Q R]\n\n') 47 | for each in annot: 48 | f.write(' '.join(each.tolist()) + '\n') 49 | f.close() 50 | 51 | def save_other_information(self, **data): 52 | pass 53 | 54 | def generate_annotation(self): 55 | # train and validation set 56 | annot = open('%s/dataset_train.txt'%(self.path)).readlines()[3:] 57 | names = np.array([e.split('/')[-2] for e in annot]) 58 | annot = [e.split() for e in annot] 59 | 60 | unique_names, unique_counts = np.unique(names, return_counts = True) 61 | 62 | if unique_names.shape[0] >= 3: 63 | index = np.argsort(unique_counts) 64 | unique_names = unique_names[index][0] 65 | valid_index = np.where(names == unique_names)[0] 66 | else: 67 | index = np.arange(names.shape[0]) 68 | np.random.shuffle(index) # @UndefinedVariable 69 | valid_index = index[:int(index.shape[0]*0.2)] 70 | 71 | train_index = np.delete(np.arange(names.shape[0]), valid_index) 72 | 73 | for each in annot: 74 | each[0] = '%s/%s/'%(self.setting.data_path, self.name) + each[0] 75 | 76 | annot = np.array(annot) 77 | 78 | valid_annot = annot[valid_index] 79 | train_annot = annot[train_index] 80 | 81 | self.write_2_text(valid_annot, 'validation') 82 | self.write_2_text(train_annot, 'train') 83 | 84 | annot = open('%s/dataset_test.txt'%(self.path)).readlines()[3:] 85 | annot = [e.split() for e in annot] 86 | for each in annot: 87 | each[0] = '%s/%s/'%(self.setting.data_path, self.name) + each[0] 88 | annot = np.array(annot) 89 | self.write_2_text(annot, 'test') 90 | 91 | 92 | def verify_annotation(self): 93 | 94 | train = open('%s/train_set.txt'%(self.path)).readlines()[3:] 95 | train = [e.split()[0] for e in train] 96 | 97 | test = open('%s/test_set.txt'%(self.path)).readlines()[3:] 98 | test = [e.split()[0] for e in test] 99 | 100 | valid = open('%s/validation_set.txt'%(self.path)).readlines()[3:] 101 | valid = [e.split()[0] for e in valid] 102 | 103 | for e in train: 104 | assert e not in test 105 | assert e not in valid 106 | 107 | for e in valid: 108 | assert e not in test 109 | assert e not in train 110 | 111 | def load_annotation(self, train_test_phase): 112 | annot = open('%s/%s_set.txt'%(self.path, train_test_phase)).readlines()[3:] 113 | annot = np.array([e.split() for e in annot]) 114 | return annot 115 | 116 | def process(self, annot): 117 | images = np.zeros((len(annot), 256, 455, 3), 'uint8') 118 | for index, (path, _, _, _, _, _, _, _) in enumerate(annot): 119 | im = cv2.imread(path) # @UndefinedVariable 120 | im = cv2.resize(im, (455, 256)) # @UndefinedVariable 121 | images[index] = im 122 | images = images.astype('f4') 123 | images = images - np.mean(images, axis=0) 124 | return {'image': images, 'pose': annot[:, 1:].astype('f4')} 125 | 126 | 127 | if __name__ == '__main__': 128 | for subset in ['KingsCollege', 'OldHospital', 'ShopFacade', 'StMarysChurch']: 129 | AbsoluteCambridge(subset) 130 | -------------------------------------------------------------------------------- /data/abstract.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jul 18, 2018 3 | 4 | @author: sovann 5 | ''' 6 | import os 7 | import tensorflow as tf 8 | import numpy as np 9 | 10 | class AbstractData(): 11 | ''' 12 | classdocs 13 | ''' 14 | 15 | def _maybe_download(self): 16 | raise NotImplementedError 17 | 18 | 19 | def generate_annotation(self): 20 | raise NotImplementedError 21 | 22 | 23 | def verify_annotation(self): 24 | # time to check if valdiation example is accidentally included in test or train set or not 25 | # other verification to be defined 26 | raise NotImplementedError 27 | 28 | 29 | def save_to_tfrecord(self, train_test_phase, suffix = '', **data): 30 | 31 | # verify shape 32 | shapes = list(set([v.shape[0] for _, v in data.items()])) 33 | assert len(shapes) == 1 34 | 35 | # get all keys 36 | keys = [e for e in data.keys()] 37 | 38 | if suffix == '': 39 | filename = '%s/%s.tfrecord'%(self.path, train_test_phase) 40 | else: 41 | filename = '%s/%s_%s.tfrecord'%(self.path, train_test_phase, suffix) 42 | 43 | with tf.python_io.TFRecordWriter(filename) as tfrecord_writer: 44 | for j in range(0, shapes[0]): 45 | tmp_data = {e:data[e][j] for e in keys} 46 | tmp_data = {k:tf.train.Feature(bytes_list=tf.train.BytesList(value=[v.tostring()])) for k, v in tmp_data.items()} 47 | example = tf.train.Example(features=tf.train.Features(feature =tmp_data)) 48 | tfrecord_writer.write(example.SerializeToString()) 49 | 50 | 51 | def save_other_information(self, **data): 52 | raise NotImplementedError 53 | 54 | 55 | def load_annotation(self, train_test_phase): 56 | raise NotImplementedError 57 | 58 | def __init__(self, name, dir_, suffix = ''): 59 | ''' 60 | Constructor 61 | ''' 62 | self.name = name 63 | 64 | self.path = dir_ 65 | 66 | try: os.mkdir(self.path) 67 | except: pass 68 | 69 | self._maybe_download() 70 | 71 | self.generate_annotation() 72 | self.verify_annotation() 73 | 74 | for train_test_phase in ['train', 'test', 'validation']: 75 | 76 | filename = '%s/%s.tfrecord'%(self.path, train_test_phase) 77 | 78 | if suffix != '': 79 | filename = '%s/%s_%s.tfrecord'%(self.path, train_test_phase, suffix) 80 | if os.path.exists(filename): 81 | print('%s already exists ... '%filename) 82 | print('the program will skip ....') 83 | continue 84 | 85 | self.annot = self.load_annotation(train_test_phase) 86 | self.data = self.process(self.annot) 87 | self.save_to_tfrecord(train_test_phase, suffix, **self.data) 88 | if train_test_phase == 'train': 89 | self.save_other_information(**self.data) 90 | -------------------------------------------------------------------------------- /data/relative_cambridge.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jul 19, 2018 3 | 4 | @author: sovann 5 | ''' 6 | import sys, os 7 | import cv2 8 | import ctypes 9 | import multiprocessing 10 | from multiprocessing.pool import Pool 11 | from pyquaternion.quaternion import Quaternion 12 | import shutil 13 | sys.path.append(os.path.dirname(os.path.realpath(__file__)) + '/../') 14 | from data.abstract import AbstractData 15 | import numpy as np 16 | from abstract_network.setting import Setting 17 | 18 | images = None 19 | 20 | def create_share_ndarray(shape, dtype, cdtype): 21 | 22 | total_shape = 1 23 | for ele in shape: 24 | total_shape *= ele 25 | shared_base = multiprocessing.Array(cdtype, total_shape) # @UndefinedVariable 26 | shared_array = np.frombuffer(shared_base.get_obj(), dtype=dtype) 27 | shared_array = np.reshape(shared_array, shape) 28 | assert shared_array.base.base is shared_base.get_obj() 29 | return shared_array 30 | 31 | def create_share_from_arr(arr, cdtype): 32 | 33 | shared_arr = create_share_ndarray(arr.shape, arr.dtype, cdtype) 34 | shared_arr[:] = arr[:] 35 | return shared_arr 36 | 37 | 38 | def _read_image(param): 39 | index, each = param 40 | 41 | if not os.path.exists(each[0]): 42 | 43 | im1 = cv2.imread(each[0].replace('/data/chercheurs/en', '/home/2017025/sen01')) # @UndefinedVariable 44 | im1 = cv2.resize(im1, (455, 256)) # @UndefinedVariable 45 | 46 | im2 = cv2.imread(each[8].replace('/data/chercheurs/en', '/home/2017025/sen01')) # @UndefinedVariable 47 | im2 = cv2.resize(im2, (455, 256)) # @UndefinedVariable 48 | else: 49 | 50 | im1 = cv2.imread(each[0]) # @UndefinedVariable 51 | im1 = cv2.resize(im1, (455, 256)) # @UndefinedVariable 52 | 53 | im2 = cv2.imread(each[8]) # @UndefinedVariable 54 | im2 = cv2.resize(im2, (455, 256)) # @UndefinedVariable 55 | 56 | images[index] = np.array([im1, im2]) 57 | 58 | 59 | 60 | class RelativeCambridge(AbstractData): 61 | 62 | def __init__(self, name_): 63 | self.name = name_ 64 | 65 | self.setting = Setting('relative_cambridge') 66 | dir_ = self.setting.data_path + '/' + name_ 67 | AbstractData.__init__(self, name_, dir_) 68 | 69 | def _maybe_download(self): 70 | 71 | if os.path.exists('%s/KingsCollege'%self.setting.data_path): return 72 | 73 | current_dir = os.getcwd() 74 | os.chdir(self.setting.data_path) 75 | 76 | links = ['https://www.repository.cam.ac.uk/bitstream/handle/1810/251342/KingsCollege.zip', 77 | 'https://www.repository.cam.ac.uk/bitstream/handle/1810/251340/OldHospital.zip', 78 | 'https://www.repository.cam.ac.uk/bitstream/handle/1810/251336/ShopFacade.zip', 79 | 'https://www.repository.cam.ac.uk/bitstream/handle/1810/251294/StMarysChurch.zip'] 80 | 81 | shutil.copy('%s/rpnet.zip'%current_dir, './') 82 | os.system('unzip rpnet.zip') 83 | 84 | for each in links: 85 | os.system('wget %s'%each) 86 | os.system("unzip '*.zip'") 87 | os.system(" find . -name '*.mp4' -type f -delete") 88 | os.system("rm -rf *.zip") 89 | 90 | os.chdir(current_dir) 91 | 92 | 93 | def write_2_text(self, annot, name): 94 | if os.path.exists('%s/%s_set.txt'%(self.path, name)): 95 | print('file %s/%s_set.txt exists, gonna skip ... '%(self.path, name)) 96 | return 97 | f = open('%s/%s_set.txt'%(self.path, name), 'w') 98 | f.write('Visual Landmark Dataset V1\nImageFile, Camera Position [X Y Z W P Q R] ImageFile, Camera Position [X Y Z W P Q R]\n\n') 99 | for each in annot: 100 | f.write(' '.join(each.tolist()) + '\n') 101 | f.close() 102 | 103 | def save_other_information(self, **data): 104 | pass 105 | 106 | 107 | def _pair_image_in_sequence(self, annot, nb_pairs=8): 108 | pair_index = [] 109 | 110 | for i in np.arange(annot.shape[0]): 111 | j = np.array([e for e in np.arange(-20, 21) if e not in np.arange(-5, 6)]) + i 112 | np.random.shuffle(j) 113 | j = j[:nb_pairs] 114 | for each in j: 115 | if each >=0 and each < annot.shape[0]: 116 | pair_index.append((i, each)) 117 | pair_index = np.array(pair_index) 118 | 119 | assert np.sum(pair_index[:, 0] - pair_index[:, 1] == 0) == 0 120 | return pair_index 121 | 122 | 123 | def _pair_image(self, annot): 124 | 125 | names = np.array([e.split('/')[-2] for e in annot[:, 0]]) 126 | unique_names = np.unique(names) 127 | 128 | for e in unique_names: 129 | assert 'seq' in e or e in ['img_south', 'img_west', 'img_east', 'img_north', 'img'] 130 | 131 | pairs = [] 132 | for e in unique_names: 133 | tmp_annot = annot[names == e] 134 | index = np.argsort(tmp_annot[:, 0]) 135 | tmp_annot = tmp_annot[index] 136 | tmp_pair = self._pair_image_in_sequence(tmp_annot) 137 | pairs.append(tmp_annot[tmp_pair]) 138 | pairs = np.vstack(pairs).reshape((-1, 16)) 139 | 140 | return pairs 141 | 142 | 143 | def generate_annotation(self): 144 | # train and validation set 145 | annot = open('%s/dataset_train.txt'%(self.path)).readlines()[3:] 146 | names = np.array([e.split('/')[-2] for e in annot]) 147 | annot = [e.split() for e in annot] 148 | 149 | unique_names, unique_counts = np.unique(names, return_counts = True) 150 | 151 | if unique_names.shape[0] >= 3: 152 | index = np.argsort(unique_counts) 153 | unique_names = unique_names[index][0] 154 | valid_index = np.where(names == unique_names)[0] 155 | else: 156 | index = np.arange(names.shape[0]) 157 | np.random.shuffle(index) # @UndefinedVariable 158 | valid_index = index[:int(index.shape[0]*0.2)] 159 | 160 | train_index = np.delete(np.arange(names.shape[0]), valid_index) 161 | 162 | for each in annot: 163 | each[0] = '%s/%s/'%(self.setting.data_path, self.name) + each[0] 164 | 165 | annot = np.array(annot) 166 | 167 | valid_annot = annot[valid_index] 168 | train_annot = annot[train_index] 169 | 170 | valid_annot = self._pair_image(valid_annot) 171 | train_annot = self._pair_image(train_annot) 172 | 173 | np.random.shuffle(valid_annot) 174 | np.random.shuffle(train_annot) 175 | 176 | self.write_2_text(valid_annot, 'validation') 177 | self.write_2_text(train_annot, 'train') 178 | 179 | annot = open('%s/dataset_test.txt'%(self.path)).readlines()[3:] 180 | annot = [e.split() for e in annot] 181 | for each in annot: 182 | each[0] = '%s/%s/'%(self.setting.data_path, self.name) + each[0] 183 | annot = np.array(annot) 184 | annot = self._pair_image(annot) 185 | np.random.shuffle(annot) 186 | self.write_2_text(annot, 'test') 187 | 188 | 189 | def verify_annotation(self): 190 | 191 | train = open('%s/train_set.txt'%(self.path)).readlines()[3:] 192 | train = [e.split()[0] for e in train] 193 | 194 | test = open('%s/test_set.txt'%(self.path)).readlines()[3:] 195 | test = [e.split()[0] for e in test] 196 | 197 | valid = open('%s/validation_set.txt'%(self.path)).readlines()[3:] 198 | valid = [e.split()[0] for e in valid] 199 | 200 | for e in train: 201 | assert e not in test 202 | assert e not in valid 203 | 204 | for e in valid: 205 | assert e not in test 206 | assert e not in train 207 | 208 | def load_annotation(self, train_test_phase): 209 | annot = open('%s/%s_set.txt'%(self.path, train_test_phase)).readlines()[3:] 210 | annot = np.array([e.split() for e in annot]) 211 | return annot 212 | 213 | 214 | def get_relative_T_in_cam2_ref(self, R2, t1, t2): 215 | new_c2 = - np.dot(R2, t2) 216 | return np.dot(R2, t1) + new_c2 217 | 218 | def _rel_pose_after_rotation(self, each_pose): 219 | assert each_pose.shape[0] == 14 220 | 221 | rotat0 = Quaternion(axis=[0, 0, 1], degrees=0) 222 | rotat1 = Quaternion(axis=[0, 0, 1], degrees=-90) 223 | rotat2 = Quaternion(axis=[0, 0, 1], degrees=-180) 224 | rotat3 = Quaternion(axis=[0, 0, 1], degrees=-270) 225 | 226 | rotats = [rotat0, rotat1, rotat2, rotat3] 227 | 228 | q1 = Quaternion(each_pose[3:7]) 229 | q2 = Quaternion(each_pose[10:14]) 230 | t1 = each_pose[:3] 231 | t2 = each_pose[7:10] 232 | relative_rotation, relative_translation = [], [] 233 | 234 | pose1 = [] 235 | pose2 = [] 236 | for i in range(4): 237 | new_q1 = rotats[i] * q1 238 | pose1.append(np.concatenate((t1, new_q1.elements))) 239 | for j in range(4): 240 | new_q2 = rotats[j] * q2 241 | 242 | if i == 0: 243 | pose2.append(np.concatenate((t2, new_q2.elements))) 244 | 245 | relative_rotation.append((new_q2 * new_q1.inverse).elements) 246 | relative_translation.append(self.get_relative_T_in_cam2_ref(new_q2.rotation_matrix, t1, t2)) 247 | 248 | relative_rotation = np.array(relative_rotation) 249 | relative_translation = np.array(relative_translation) 250 | 251 | pose1 = np.array(pose1) 252 | pose2 = np.array(pose2) 253 | 254 | assert pose1.shape[0] == pose2.shape[0] == 4 and pose2.shape[1] == pose1.shape[1] == 7 255 | 256 | rel_pose = np.hstack((relative_translation, relative_rotation)).reshape((16, 7)) 257 | rel_pose = np.vstack((rel_pose, pose1, pose2, each_pose.reshape((2, 7)) )) 258 | return rel_pose.reshape((1, 26, 7)) 259 | 260 | 261 | def _get_relative_pose(self, pose): 262 | print(pose.shape) 263 | assert pose.shape[1] == 14 264 | pose = np.vstack([self._rel_pose_after_rotation(each) for each in pose]) 265 | print(pose.shape) 266 | assert pose.shape[1] == 26 and pose.shape[2] == 7 267 | return pose 268 | 269 | 270 | def process(self, annot): 271 | global images 272 | 273 | images = create_share_ndarray((len(annot), 2, 256, 455, 3), 'uint8', ctypes.c_uint8) 274 | pool = Pool(processes = multiprocessing.cpu_count() - 10) # @UndefinedVariable 275 | pool.map(_read_image, enumerate(annot)) 276 | pool.close() 277 | pool.join() 278 | 279 | # images = np.zeros((len(annot), 2, 256, 455, 3), 'uint8') 280 | # for index, each in enumerate(annot): 281 | # 282 | # im1 = cv2.imread(each[0]) # @UndefinedVariable 283 | # im1 = cv2.resize(im1, (455, 256)) # @UndefinedVariable 284 | # 285 | # im2 = cv2.imread(each[8]) # @UndefinedVariable 286 | # im2 = cv2.resize(im2, (455, 256)) # @UndefinedVariable 287 | # 288 | # images[index] = np.array([im1, im2]) 289 | images = images.astype('f4') 290 | images = images - images.mean(axis=0).mean(axis=0) 291 | 292 | relative_pose = self._get_relative_pose(np.hstack((annot[:, 1:8], annot[:, 9:])).astype('f4')) 293 | return {'image': images, 294 | 'pose': relative_pose.astype('f4'), 295 | 'extra':np.random.randint(0, 4, 2 * images.shape[0]).reshape((-1, 2)).astype('int32')} 296 | 297 | 298 | if __name__ == '__main__': 299 | for subset in ['GreatCourt', 'KingsCollege', 'OldHospital', 'ShopFacade', 'StMarysChurch', 'Street'][1:]: 300 | RelativeCambridge(subset) -------------------------------------------------------------------------------- /data/rpnet.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ensv/RPNet/f4b3fbf510d47801e5b4a1e8b9001a5bb3038a1b/data/rpnet.zip -------------------------------------------------------------------------------- /posenet/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ensv/RPNet/f4b3fbf510d47801e5b4a1e8b9001a5bb3038a1b/posenet/__init__.py -------------------------------------------------------------------------------- /posenet/posenet.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Apr 3, 2018 3 | 4 | @author: en 5 | ''' 6 | import argparse, sys, os 7 | from tensorflow.python.framework.errors_impl import OutOfRangeError 8 | import math 9 | from sklearn.externals import joblib 10 | sys.path.append(os.path.dirname(os.path.realpath(__file__)) + '/../') 11 | from abstract_network.setting import Setting 12 | from abstract_network.abstract import AbstractNetwork 13 | from GoogLeNet.googlenet import GoogLeNet 14 | import tensorflow as tf 15 | import numpy as np 16 | 17 | 18 | slim = tf.contrib.slim # @UndefinedVariable 19 | 20 | 21 | def train_preprocessing_cambridge_dataset(example_proto): 22 | features = {"image": tf.FixedLenFeature((), tf.string, default_value=''), 23 | "pose": tf.FixedLenFeature((), tf.string, default_value=''), 24 | } 25 | parsed_features = tf.parse_single_example(example_proto, features) 26 | 27 | feature = tf.decode_raw(parsed_features["image"], tf.float32) 28 | feature = tf.reshape(feature, (256, 455, 3)) 29 | pose = tf.decode_raw(parsed_features["pose"], tf.float32) 30 | 31 | feature = tf.random_crop(feature, (224, 224, 3)) 32 | return feature, pose 33 | 34 | 35 | def test_preprocessing_cambridge_dataset(example_proto): 36 | features = {"image": tf.FixedLenFeature((), tf.string, default_value=''), 37 | "pose": tf.FixedLenFeature((), tf.string, default_value=''), 38 | } 39 | parsed_features = tf.parse_single_example(example_proto, features) 40 | 41 | feature = tf.decode_raw(parsed_features["image"], tf.float32) 42 | feature = tf.reshape(feature, (256, 455, 3)) 43 | pose = tf.decode_raw(parsed_features["pose"], tf.float32) 44 | 45 | feature = feature[16:240, 116:340, :] 46 | return feature, pose 47 | 48 | 49 | class PoseNet(AbstractNetwork): 50 | ''' 51 | classdocs 52 | ''' 53 | 54 | 55 | def __init__(self, flags): 56 | ''' 57 | Constructor 58 | ''' 59 | 60 | flags.name = 'PoseNet' 61 | self.output_dim = 7 62 | 63 | self.wd = flags.wd 64 | self.beta = flags.beta 65 | setting = Setting('absolute_pose_network') 66 | AbstractNetwork.__init__(self, setting, flags) 67 | self.train_dir = self._train_dir_path('_%d_%.4f'%(self.beta, self.wd)) 68 | if self.train_dir == '': return 69 | self._get_train_test_set() 70 | 71 | self.prepare_data(train_preprocessing_cambridge_dataset, test_preprocessing_cambridge_dataset) 72 | 73 | self.prepare_inference() 74 | self.prepare_loss() 75 | self.get_train_op() 76 | self.max_iteration = 80000 77 | 78 | 79 | def _get_train_test_set(self): 80 | 81 | if self.dataset in self.cambridge_subset: 82 | path = self.setting.data_path.replace('absolute_pose_network', 'absolute_cambridge') + '/' + self.dataset 83 | self.train_filenames = '%s/train.tfrecord'%path 84 | self.test_filenames = '%s/test.tfrecord'%path 85 | self.valid_filenames = '%s/validation.tfrecord'%path 86 | else: 87 | raise NotImplementedError 88 | 89 | 90 | def before_train(self, sess): 91 | pass 92 | 93 | def before_valid_test(self, sess): 94 | pass 95 | 96 | def prepare_inference(self): 97 | 98 | # arg_scope = inception.inception_utils.inception_arg_scope(weight_decay=self.wd) 99 | # with slim.arg_scope(arg_scope): 100 | net = GoogLeNet({'data': self.input_X}) 101 | 102 | weight_vars = [v for v in tf.global_variables() if "weights" in v.name] 103 | print('weight var: ', len(weight_vars)) 104 | 105 | if self.wd > 0: 106 | for v in tf.global_variables(): 107 | if "weights" in v.name: 108 | tmp_loss = tf.nn.l2_loss(v) * self.wd 109 | tf.add_to_collection('losses', tmp_loss) 110 | 111 | self.xyz_logits = net.layers['cls3_fc_pose_xyz'] 112 | self.wpqr_logits = net.layers['cls3_fc_pose_wpqr'] 113 | self.aux1_xyz_logits = net.layers['cls1_fc_pose_xyz'] 114 | self.aux1_wpqr_logits = net.layers['cls1_fc_pose_wpqr'] 115 | self.aux2_xyz_logits = net.layers['cls2_fc_pose_xyz'] 116 | self.aux2_wpqr_logits = net.layers['cls2_fc_pose_wpqr'] 117 | 118 | self.net = net 119 | 120 | def prepare_loss(self): 121 | self.tran_loss = self._l2_norm_loss(self.xyz_logits, labels=self.input_Y[:, :3], weights=1.0, scope='Train/translation_loss') 122 | self.rotat_loss = self._l2_norm_loss(self.wpqr_logits, labels=self.input_Y[:, 3:], weights=self.beta, scope='Train/rotation_loss') 123 | 124 | # Aux classifiers 125 | self._l2_norm_loss(self.aux2_xyz_logits, labels=self.input_Y[:, :3], weights=.3, scope='Train/aux2_translation_loss') 126 | self._l2_norm_loss(self.aux2_wpqr_logits, labels=self.input_Y[:, 3:], weights=self.beta/3.33, scope='Train/aux2_rotation_loss') 127 | self._l2_norm_loss(self.aux1_xyz_logits, labels=self.input_Y[:, :3], weights=.3, scope='Train/aux1_translation_loss') 128 | self._l2_norm_loss(self.aux1_wpqr_logits, labels=self.input_Y[:, 3:], weights=self.beta/3.33, scope='Train/aux1_rotation_loss') 129 | 130 | def _load_pretrained_weight(self, sess): 131 | if self.dataset in self.cambridge_subset: 132 | self.net.load('../GoogLeNet/weights/posenet.npy', sess) 133 | else: 134 | raise NotImplementedError 135 | def _define_additional_summaries(self): 136 | self.valid_loss, self.valid_error = tf.Variable([0, 0], dtype='float32', trainable=False), tf.Variable([0, 0], dtype='float32', trainable=False) 137 | 138 | self.valid_summary = [tf.summary.scalar('Valid/tran_loss', self.valid_loss[0]), 139 | tf.summary.scalar('Valid/rotat_loss', self.valid_loss[1]), 140 | tf.summary.scalar('Valid/tran_error', self.valid_error[0]), 141 | tf.summary.scalar('Valid/rotat_error', self.valid_error[1]), 142 | ] # @UndefinedVariable 143 | 144 | def evaluate_validation(self, sess): 145 | # example of prototype 146 | 147 | self.before_valid_test(sess) 148 | sess.run(self.validation_init_op) 149 | 150 | loss = [] 151 | error = [[], []] 152 | # train for half epoch 153 | while True: 154 | try: 155 | tmp = sess.run([self.tran_loss, self.rotat_loss, self.xyz_logits, self.wpqr_logits, self.input_Y]) 156 | loss.append([tmp[0], tmp[1]]) 157 | tmp[3] = tmp[3]/ np.repeat(np.linalg.norm(tmp[3], axis=1).reshape((-1, 1)), 4, axis=1) 158 | d = np.abs(np.sum(np.multiply(tmp[3], tmp[4][:, 3:]), axis=1)) 159 | np.putmask(d, d> 1, 1) 160 | error[1].append(2 * np.arccos(d) * 180/math.pi) 161 | error[0].append(np.linalg.norm(tmp[2]-tmp[4][:, :3], axis=1)) 162 | except OutOfRangeError: 163 | break 164 | 165 | # validation loss 166 | loss = np.array(loss).mean(axis=0) 167 | sess.run(self.valid_loss.assign(loss)) 168 | 169 | # validation error 170 | error = [np.concatenate(error[0]), np.concatenate(error[1])] 171 | error = np.median(np.array(error), axis=1) 172 | sess.run(self.valid_error.assign(error)) 173 | 174 | summary_str = sess.run(self.valid_summary) 175 | for each in summary_str: 176 | self.summary_writer.add_summary(each, sess.run(tf.train.get_global_step())) 177 | 178 | def _custom_evaluation(self, sess): 179 | 180 | loss = [] 181 | error = [[], [], []] 182 | pose = [] 183 | # train for half epoch 184 | while True: 185 | try: 186 | tmp = sess.run([self.tran_loss, self.rotat_loss, self.xyz_logits, self.wpqr_logits, self.input_Y]) 187 | 188 | pose.append(np.hstack((tmp[2], tmp[3], tmp[4]))) 189 | loss.append([tmp[0], tmp[1]]) 190 | 191 | tmp[3] = tmp[3]/ np.repeat(np.linalg.norm(tmp[3], axis=1).reshape((-1, 1)), 4, axis=1) 192 | d = np.abs(np.sum(np.multiply(tmp[3], tmp[4][:, 3:]), axis=1)) 193 | np.putmask(d, d> 1, 1) 194 | error[1].append(2 * np.arccos(d) * 180/math.pi) 195 | error[0].append(np.linalg.norm(tmp[2]-tmp[4][:, :3], axis=1)) 196 | 197 | tmp[4][:, :3] = tmp[4][:, :3] / np.linalg.norm(tmp[4][:, :3], axis=1, keepdims=True) 198 | tmp[2] = tmp[2] / np.linalg.norm(tmp[2], axis=1, keepdims=True) 199 | error[2].append(np.arccos(np.sum(tmp[4][:, :3] * tmp[2], axis=1)) * 180/math.pi) 200 | except OutOfRangeError: 201 | break 202 | 203 | # all the pose 204 | pose = np.vstack(pose) 205 | np.save('%s/all_pose'%self.train_dir, pose) 206 | 207 | # validation loss 208 | loss = np.array(loss).mean(axis=0) 209 | 210 | # validation error 211 | error = [np.concatenate(error[0]), np.concatenate(error[1]), np.concatenate(error[2])] 212 | error = np.median(np.array(error), axis=1) 213 | 214 | joblib.dump(np.concatenate((loss, error)), '%s/%s.pkl'%(self.train_dir, self.test_result_fn), compress=3) 215 | 216 | def main(_): 217 | 218 | working_dir = os.getcwd() 219 | os.chdir(os.path.dirname(os.path.realpath(__file__))) 220 | 221 | if FLAGS.experiment == 'cambridge': 222 | beta = {'KingsCollege':500, 223 | 'OldHospital':100, 224 | 'ShopFacade':100, 225 | 'StMarysChurch':250, 226 | 'Street':2000} 227 | 228 | FLAGS.experiment = 'cambridge' 229 | for _ in range(2): 230 | for dataset in ['KingsCollege', 'OldHospital', 'ShopFacade', 'StMarysChurch']: 231 | for lr in [1e-5]: 232 | for wd in [1e-2]: 233 | FLAGS.wd = wd 234 | FLAGS.lr = lr 235 | FLAGS.dataset = dataset 236 | FLAGS.beta = beta[dataset] 237 | net = PoseNet(FLAGS) 238 | net.process() 239 | else: 240 | print ('unknown experimentation') 241 | raise NotImplementedError 242 | os.chdir(working_dir) 243 | 244 | 245 | if __name__ == '__main__': 246 | 247 | 248 | parser = argparse.ArgumentParser() 249 | 250 | parser.register("type", "bool", lambda v: v.lower() == "true") 251 | 252 | parser.add_argument("--optimization", default= 'sgdm', type=str, help= "Optimization strategy: adadelta, sgd, sgdm") 253 | parser.add_argument("--lr", type = float, default = 1e-5, help = "Initial learning rate") 254 | parser.add_argument("--lr_decay_rate", type = float, default = 0.90, help = 'decay rate for learning rate') 255 | parser.add_argument("--lr_decay_step", type = int, default = 80, help = 'nb of epochs to decay learning rate, use big number of step to avoid decaying the lr') 256 | parser.add_argument("--nb_epoches", type = int, default = 10000, help = "Nb of epochs to train the model, alternative to --max_iteration") 257 | parser.add_argument("--batch_size", type = int, default = 64, help="Number of example per batch") 258 | parser.add_argument("--wd", type = float, default = 1e-2, help="weight decay on each layer, default = 0.0 means no decay") 259 | 260 | parser.add_argument("--beta", type = int, default = 500, help="beta to weight between the two losses as in the paper") 261 | 262 | parser.add_argument("--train_test_phase", default = 'train', help = 'train, validation or test phase') 263 | parser.add_argument("--dataset", default = 'OldHospital', type=str, help ='dataset name to train the network') 264 | parser.add_argument("--eval_interval_epoch", type = int, default = 100, help="nb of epochs after which the evaluation on validation should take place") 265 | 266 | # specific to this architecture 267 | parser.add_argument("--nb_run", default = 1, type = int, help ='each run per configuration, useful to compute std') 268 | parser.add_argument("--experiment", default= 'cambridge', help='group of experiments') 269 | parser.add_argument("--use_dropout", type = int, default = 0, help='use dropout or not 0 or 1') 270 | parser.add_argument("--continue_training", type = int, default = 0, help='continue training on the logdir if done.txt is not yet created. Use for resuming training') 271 | 272 | FLAGS, unparsed = parser.parse_known_args() 273 | 274 | 275 | tf.app.run(main= main, argv=[sys.argv[0]] + unparsed) 276 | -------------------------------------------------------------------------------- /posenet/posenet_for_relative_pose.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jul 25, 2018 3 | 4 | @author: sovann 5 | ''' 6 | import numpy as np 7 | import os 8 | from pyquaternion.quaternion import Quaternion 9 | import math 10 | 11 | 12 | def quat2matrix(q): 13 | w, x, y, z = q 14 | sqw = w*w; 15 | sqx = x*x; 16 | sqy = y*y; 17 | sqz = z*z; 18 | 19 | # invs (inverse square length) is only required if quaternion is not already normalised 20 | invs = 1 / (sqx + sqy + sqz + sqw) 21 | m00 = ( sqx - sqy - sqz + sqw)*invs ; # since sqw + sqx + sqy + sqz =1/invs*invs 22 | m11 = (-sqx + sqy - sqz + sqw)*invs ; 23 | m22 = (-sqx - sqy + sqz + sqw)*invs ; 24 | 25 | tmp1 = x*y; 26 | tmp2 = z*w; 27 | m10 = 2.0 * (tmp1 + tmp2)*invs ; 28 | m01 = 2.0 * (tmp1 - tmp2)*invs ; 29 | 30 | tmp1 = x*z; 31 | tmp2 = y*w; 32 | m20 = 2.0 * (tmp1 - tmp2)*invs ; 33 | m02 = 2.0 * (tmp1 + tmp2)*invs ; 34 | tmp1 = y*z; 35 | tmp2 = x*w; 36 | m21 = 2.0 * (tmp1 + tmp2)*invs ; 37 | m12 = 2.0 * (tmp1 - tmp2)*invs ; 38 | 39 | return np.array([ 40 | [m00, m01, m02], 41 | [m10, m11, m12], 42 | [m20, m21, m22] 43 | ], 'f4') 44 | 45 | 46 | def get_relative_pose_np(r1, r2, t1, t2): 47 | 48 | r1, r2, t1, t2 = r1.astype('f4'), r2.astype('f4'), t1.astype('f4'), t2.astype('f4') 49 | pose = [] 50 | for i in range(t1.shape[0]): 51 | 52 | rr = (Quaternion(r2[i]) * Quaternion(r1[i]).inverse).elements 53 | R2 = Quaternion(r2[i]).rotation_matrix 54 | new_c2 = - np.dot(R2, t2[i]) 55 | rt = np.dot(R2, t1[i]) + new_c2 56 | 57 | pose.append(np.concatenate((rt, rr))) 58 | return np.array(pose) 59 | 60 | 61 | if __name__ == '__main__': 62 | 63 | prefix = '/data/chercheurs/en' 64 | if not os.path.exists(prefix): 65 | prefix = '/home/2017025/sen01' 66 | 67 | for dataset in ['KingsCollege', 'OldHospital', 'ShopFacade', 'StMarysChurch', 'Street']: 68 | base = '%s/DRP/absolute_cambridge_network/%s/PoseNetD/cambridge/'%(prefix, dataset) 69 | 70 | for each in os.listdir(base): 71 | print('%s/%s/0/all_pose.npy'%(base, each)) 72 | if not os.path.exists('%s/%s/0/all_pose.npy'%(base, each)): continue 73 | 74 | allpose = np.load('%s/%s/0/all_pose.npy'%(base, each)) 75 | 76 | if np.isnan(allpose.sum()): 77 | np.save('%s/%s/0/error'%(base, each), np.array(['nan', 'nan', 'nan'])) 78 | 79 | abspose = open('%s/DRP/absolute_cambridge/%s/test_set.txt'%(prefix, dataset)).readlines()[3:] 80 | abspose = np.array([e.split() for e in abspose]) 81 | 82 | 83 | assert abspose.shape[0] == allpose.shape[0] 84 | 85 | abspose = {'/'.join(abspose[i][0].split('/')[-2:]):allpose[i, :7] for i in range(abspose.shape[0]) \ 86 | if (abspose[i, 1:].astype('f4') - allpose[i, 7:]).sum() == 0} 87 | 88 | annot = open('%s/DRP/relative_cambridge/%s/test_set.txt'%(prefix, dataset)).readlines()[3:] 89 | annot = np.array([e.split() for e in annot]) 90 | gt_pose = get_relative_pose_np(annot[:, 4:8], annot[:, 12:16], annot[:, 1:4], annot[:, 9:12]) 91 | 92 | est_pose = [] 93 | for each_annot in annot: 94 | name1, name2 = '/'.join(each_annot[0].split('/')[-2:]), '/'.join(each_annot[8].split('/')[-2:]) 95 | 96 | pose1 = abspose[name1.replace('relative_cambridge', 'absolute_cambridge')] 97 | pose2 = abspose[name2.replace('relative_cambridge', 'absolute_cambridge')] 98 | 99 | q1, q2 = pose1[3:], pose2[3:] 100 | t1, t2 = pose1[:3], pose2[:3] 101 | 102 | rr = (Quaternion(q2) * Quaternion(q1).inverse).normalised 103 | 104 | R2 = Quaternion(q2).rotation_matrix 105 | new_c2 = - np.dot(R2, t2) 106 | rt = np.dot(R2, t1) + new_c2 107 | est_pose.append(np.concatenate((rt, rr.elements))) 108 | 109 | est_pose = np.array(est_pose) 110 | 111 | tm = np.linalg.norm(est_pose[:, :3] - gt_pose[:, :3], axis=1) 112 | d = np.abs(np.sum(est_pose[:, 3:] * gt_pose[:, 3:], axis=1)) 113 | np.putmask(d , d > 1, 1) 114 | np.putmask(d , d < -1, -1) 115 | rd = 2 * np.arccos(d) * 180/math.pi 116 | 117 | est_pose[:, :3] = est_pose[:, :3] / np.linalg.norm(est_pose[:, :3], axis=1, keepdims=True) 118 | gt_pose[:, :3] = gt_pose[:, :3] / np.linalg.norm(gt_pose[:, :3], axis=1, keepdims=True) 119 | 120 | d = np.sum(est_pose[:, :3] * gt_pose[:, :3], axis=1) 121 | np.putmask(d , d > 1, 1) 122 | np.putmask(d , d < -1, -1) 123 | td = np.arccos(d) * 180/math.pi 124 | 125 | error = np.array([tm, rd, td]).reshape((3, -1)) 126 | 127 | print(np.median(error, axis=1)) 128 | np.save('%s/%s/0/error'%(base, each), error) 129 | 130 | -------------------------------------------------------------------------------- /rpnet/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ensv/RPNet/f4b3fbf510d47801e5b4a1e8b9001a5bb3038a1b/rpnet/__init__.py -------------------------------------------------------------------------------- /rpnet/data_preprocessing.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Aug 22, 2018 3 | 4 | @author: en 5 | ''' 6 | import tensorflow as tf 7 | 8 | 9 | def train_cambridge_dataset(example_proto): 10 | features = {"image": tf.FixedLenFeature((), tf.string, default_value=''), 11 | "pose": tf.FixedLenFeature((), tf.string, default_value=''), 12 | } 13 | parsed_features = tf.parse_single_example(example_proto, features) 14 | 15 | feature = tf.decode_raw(parsed_features["image"], tf.float32) 16 | feature = tf.reshape(feature, (2, 256, 455, 3)) 17 | pose = tf.decode_raw(parsed_features["pose"], tf.float32) 18 | pose = tf.reshape(pose, (26, 7)) 19 | 20 | feature = tf.random_crop(feature, (2, 224, 224, 3)) 21 | 22 | return tf.cast(feature, tf.float32), pose[0]# pose[extra_R[0]*4 + extra_R[1]] 23 | 24 | 25 | def test_cambridge_dataset(example_proto): 26 | features = {"image": tf.FixedLenFeature((), tf.string, default_value=''), 27 | "pose": tf.FixedLenFeature((), tf.string, default_value=''), 28 | "extra": tf.FixedLenFeature((), tf.string, default_value=''), 29 | } 30 | parsed_features = tf.parse_single_example(example_proto, features) 31 | 32 | feature = tf.decode_raw(parsed_features["image"], tf.float32) 33 | feature = tf.reshape(feature, (2, 256, 455, 3)) 34 | pose = tf.decode_raw(parsed_features["pose"], tf.float32) 35 | pose = tf.reshape(pose, (26, 7)) 36 | feature = feature[:, 16:240, 116:340, :] 37 | return tf.cast(feature, tf.float32), pose[0] 38 | 39 | def train_cambridge_dataset_extra(example_proto): 40 | features = {"image": tf.FixedLenFeature((), tf.string, default_value=''), 41 | "pose": tf.FixedLenFeature((), tf.string, default_value=''), 42 | } 43 | parsed_features = tf.parse_single_example(example_proto, features) 44 | 45 | feature = tf.decode_raw(parsed_features["image"], tf.float32) 46 | feature = tf.reshape(feature, (2, 256, 455, 3)) 47 | pose = tf.decode_raw(parsed_features["pose"], tf.float32) 48 | pose = tf.reshape(pose, (26, 7)) 49 | 50 | feature = tf.random_crop(feature, (2, 224, 224, 3)) 51 | 52 | return tf.cast(feature, tf.float32), pose[0], pose[16], pose[20] 53 | 54 | def test_cambridge_dataset_extra(example_proto): 55 | features = {"image": tf.FixedLenFeature((), tf.string, default_value=''), 56 | "pose": tf.FixedLenFeature((), tf.string, default_value=''), 57 | "extra": tf.FixedLenFeature((), tf.string, default_value=''), 58 | } 59 | parsed_features = tf.parse_single_example(example_proto, features) 60 | 61 | feature = tf.decode_raw(parsed_features["image"], tf.float32) 62 | feature = tf.reshape(feature, (2, 256, 455, 3)) 63 | pose = tf.decode_raw(parsed_features["pose"], tf.float32) 64 | pose = tf.reshape(pose, (26, 7)) 65 | 66 | feature = feature[:, 16:240, 116:340, :] 67 | return tf.cast(feature, tf.float32), pose[0], pose[16], pose[20] 68 | 69 | 70 | 71 | def get_read_tfrecord_fn(dataset, extra_loss=False): 72 | print('**********************', dataset.lower(), '*****************************') 73 | if dataset in ['KingsCollege', 'OldHospital', 'ShopFacade', 'StMarysChurch']: 74 | if extra_loss: 75 | return train_cambridge_dataset_extra, test_cambridge_dataset_extra 76 | else: 77 | return train_cambridge_dataset, test_cambridge_dataset 78 | else: 79 | raise NotImplementedError -------------------------------------------------------------------------------- /rpnet/rpnet.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jul 20, 2018 3 | 4 | @author: en 5 | ''' 6 | 7 | import argparse, sys, os 8 | from tensorflow.python.framework.errors_impl import OutOfRangeError 9 | import math 10 | from sklearn.externals import joblib 11 | sys.path.append(os.path.dirname(os.path.realpath(__file__)) + '/../') 12 | import data_preprocessing # @UnresolvedImport 13 | from abstract_network.setting import Setting 14 | from abstract_network.abstract import AbstractNetwork 15 | from GoogLeNet.googlenet import GoogLeNet 16 | import tensorflow as tf 17 | import numpy as np 18 | 19 | 20 | slim = tf.contrib.slim # @UndefinedVariable 21 | 22 | 23 | 24 | class RPNet(AbstractNetwork): 25 | ''' 26 | classdocs 27 | ''' 28 | 29 | 30 | def __init__(self, flags): 31 | ''' 32 | Constructor 33 | ''' 34 | 35 | flags.name = 'RPNet' 36 | self.output_dim = 7 37 | 38 | self.use_extraLoss = flags.use_extraLoss 39 | self.wd = flags.wd 40 | self.beta = flags.beta 41 | setting = Setting('relative_pose_network') 42 | AbstractNetwork.__init__(self, setting, flags) 43 | self.train_dir = self._train_dir_path('_%d_%.4f_%d'%(self.beta, self.wd, self.use_extraLoss)) 44 | if self.train_dir == '': return 45 | 46 | self.eval_interval_epoch = 50 47 | self.prefetch_data = 500 48 | 49 | self._get_train_test_set() 50 | train_fn, test_fn = data_preprocessing.get_read_tfrecord_fn(self.dataset, self.use_extraLoss) 51 | self.prepare_data(train_fn, test_fn) 52 | self.prepare_inference() 53 | self.prepare_loss() 54 | self.get_train_op() 55 | 56 | self.max_iteration = 500000 57 | 58 | 59 | def _get_train_test_set(self): 60 | 61 | if self.dataset in self.cambridge_subset: 62 | path = self.setting.data_path.replace('relative_pose_network', 'relative_cambridge') + '/' + self.dataset 63 | self.train_filenames = '%s/train.tfrecord'%path 64 | self.test_filenames = '%s/test.tfrecord'%path 65 | self.valid_filenames = '%s/validation.tfrecord'%path 66 | else: 67 | raise NotImplementedError 68 | 69 | 70 | def before_train(self, sess): 71 | pass 72 | 73 | def before_valid_test(self, sess): 74 | pass 75 | 76 | def prepare_inference(self): 77 | 78 | with tf.variable_scope("siamese") as scope: 79 | net_1 = GoogLeNet({'data': self.input_X[:, 0]}) 80 | scope.reuse_variables() 81 | net_2 = GoogLeNet({'data': self.input_X[:, 1]}) 82 | 83 | weight_vars = [v for v in tf.global_variables() if "weights" in v.name] 84 | print('weight var: ', len(weight_vars)) 85 | 86 | if self.wd > 0: 87 | for v in tf.global_variables(): 88 | if "weights" in v.name: 89 | tmp_loss = tf.nn.l2_loss(v) * self.wd 90 | tf.add_to_collection('losses', tmp_loss) 91 | 92 | self.xyz_logits = self.get_relative_T_in_cam2_ref(net_2.layers['cls3_fc_pose_wpqr'], net_1.layers['cls3_fc_pose_xyz'], net_2.layers['cls3_fc_pose_xyz']) 93 | self.wpqr_logits = self.get_quaternion_rotation(net_1.layers['cls3_fc_pose_wpqr'], net_2.layers['cls3_fc_pose_wpqr'], name_='cls3_pose_wpqr') 94 | self.aux1_xyz_logits = self.get_relative_T_in_cam2_ref(net_2.layers['cls1_fc_pose_wpqr'], net_1.layers['cls1_fc_pose_xyz'], net_2.layers['cls1_fc_pose_xyz']) 95 | self.aux1_wpqr_logits = self.get_quaternion_rotation(net_1.layers['cls1_fc_pose_wpqr'], net_2.layers['cls1_fc_pose_wpqr'], name_='cls1_pose_wpqr') 96 | self.aux2_xyz_logits = self.get_relative_T_in_cam2_ref(net_2.layers['cls2_fc_pose_wpqr'], net_1.layers['cls2_fc_pose_xyz'], net_2.layers['cls2_fc_pose_xyz']) 97 | self.aux2_wpqr_logits = self.get_quaternion_rotation(net_1.layers['cls2_fc_pose_wpqr'], net_2.layers['cls2_fc_pose_wpqr'], name_='cls2_pose_wpqr') 98 | 99 | self.net_1 = net_1 100 | self.net_2 = net_2 101 | 102 | def extra_loss_4(self, net, pose): 103 | self._l2_norm_loss(net.layers['cls3_fc_pose_xyz'], labels=pose[:, :3], weights=self.translation_weight, scope='Train/Extra_1_translation_loss') 104 | self._l2_norm_loss(net.layers['cls3_fc_pose_wpqr'], labels=pose[:, 3:], weights=self.beta, scope='Train/Extra_1_rotation_loss') 105 | 106 | # Aux classifiers 107 | self._l2_norm_loss(net.layers['cls2_fc_pose_xyz'], labels=pose[:, :3], weights=self.translation_weight/ 3.33) 108 | self._l2_norm_loss(net.layers['cls2_fc_pose_wpqr'], labels=pose[:, 3:], weights=self.beta/3.33) 109 | self._l2_norm_loss(net.layers['cls1_fc_pose_xyz'], labels=pose[:, :3], weights=self.translation_weight/3.33) 110 | self._l2_norm_loss(net.layers['cls1_fc_pose_wpqr'], labels=pose[:, 3:], weights=self.beta/3.33) 111 | 112 | def prepare_loss(self): 113 | 114 | self.translation_weight = 1 115 | 116 | self.tran_loss = self._l2_norm_loss(self.xyz_logits, labels=self.input_Y[:, :3], weights=self.translation_weight, scope='Train/translation_loss') 117 | self.rotat_loss = self._l2_norm_loss(self.wpqr_logits, labels=self.input_Y[:, 3:], weights=self.beta, scope='Train/rotation_loss') 118 | 119 | # Aux classifiers 120 | self.tran_loss2 = self._l2_norm_loss(self.aux2_xyz_logits, labels=self.input_Y[:, :3], weights=self.translation_weight/3.33, scope='Train/aux2_translation_loss') 121 | self.rotat_loss2 = self._l2_norm_loss(self.aux2_wpqr_logits, labels=self.input_Y[:, 3:], weights=self.beta/3.33, scope='Train/aux2_rotation_loss') 122 | self.tran_loss1 = self._l2_norm_loss(self.aux1_xyz_logits, labels=self.input_Y[:, :3], weights=self.translation_weight/3.33, scope='Train/aux1_translation_loss') 123 | self.rotat_loss1 = self._l2_norm_loss(self.aux1_wpqr_logits, labels=self.input_Y[:, 3:], weights=self.beta/3.33, scope='Train/aux1_rotation_loss') 124 | 125 | if self.use_extraLoss: 126 | self.extra_loss_4(self.net_1, self.extra_input_Y1) 127 | self.extra_loss_4(self.net_2, self.extra_input_Y2) 128 | 129 | def _load_pretrained_weight(self, sess): 130 | self.net_1.load('../GoogLeNet/weights/posenet.npy', sess, 'siamese/') 131 | 132 | def _define_additional_summaries(self): 133 | self.valid_loss, self.valid_error = tf.Variable([0, 0], dtype='float32', trainable=False), tf.Variable([0, 0], dtype='float32', trainable=False) 134 | 135 | self.valid_summary = [tf.summary.scalar('Valid/tran_loss', self.valid_loss[0]), 136 | tf.summary.scalar('Valid/rotat_loss', self.valid_loss[1]), 137 | tf.summary.scalar('Valid/tran_error', self.valid_error[0]), 138 | tf.summary.scalar('Valid/rotat_error', self.valid_error[1]), 139 | ] # @UndefinedVariable 140 | 141 | def evaluate_validation(self, sess): 142 | # example of prototype 143 | 144 | self.before_valid_test(sess) 145 | sess.run(self.validation_init_op) 146 | 147 | loss = [] 148 | error = [[], []] 149 | # train for half epoch 150 | while True: 151 | try: 152 | tmp = sess.run([self.tran_loss, self.rotat_loss, self.xyz_logits, self.wpqr_logits, self.input_Y]) 153 | loss.append([tmp[0], tmp[1]]) 154 | tmp[3] = tmp[3]/ np.linalg.norm(tmp[3], axis=1, keepdims = True) 155 | d = np.abs(np.sum(np.multiply(tmp[3], tmp[4][:, 3:]), axis=1)) 156 | np.putmask(d, d> 1, 1) 157 | np.putmask(d, d< -1, -1) 158 | error[1].append(2 * np.arccos(d) * 180/math.pi) 159 | error[0].append(np.linalg.norm(tmp[2]-tmp[4][:, :3], axis=1)) 160 | except OutOfRangeError: 161 | break 162 | 163 | # validation loss 164 | loss = np.array(loss).mean(axis=0) 165 | sess.run(self.valid_loss.assign(loss)) 166 | 167 | # validation error 168 | error = [np.concatenate(error[0]), np.concatenate(error[1])] 169 | error = np.median(np.array(error), axis=1) 170 | sess.run(self.valid_error.assign(error)) 171 | 172 | summary_str = sess.run(self.valid_summary) 173 | for each in summary_str: 174 | self.summary_writer.add_summary(each, sess.run(tf.train.get_global_step())) 175 | 176 | def _custom_evaluation(self, sess): 177 | 178 | loss = [] 179 | error = [[], [], []] 180 | pose = [] 181 | # train for half epoch 182 | while True: 183 | try: 184 | tmp = sess.run([self.tran_loss, self.rotat_loss, self.xyz_logits, self.wpqr_logits, self.input_Y]) 185 | 186 | pose.append(np.hstack((tmp[2], tmp[3], tmp[4]))) 187 | loss.append([tmp[0], tmp[1]]) 188 | 189 | tmp[3] = tmp[3]/ np.linalg.norm(tmp[3], axis=1, keepdims=True) 190 | d = np.abs(np.sum(np.multiply(tmp[3], tmp[4][:, 3:]), axis=1)) 191 | np.putmask(d, d> 1, 1) 192 | np.putmask(d, d< -1, -1) 193 | error[1].append(2 * np.arccos(d) * 180/math.pi) 194 | error[0].append(np.linalg.norm(tmp[2]-tmp[4][:, :3], axis=1)) 195 | 196 | tmp[4][:, :3] = tmp[4][:, :3] / np.linalg.norm(tmp[4][:, :3], axis=1, keepdims=True) 197 | tmp[2] = tmp[2] / np.linalg.norm(tmp[2], axis=1, keepdims=True) 198 | error[2].append(np.arccos(np.sum(tmp[4][:, :3] * tmp[2], axis=1)) * 180/math.pi) 199 | except OutOfRangeError: 200 | break 201 | 202 | # all the pose 203 | pose = np.vstack(pose) 204 | np.save('%s/all_pose'%self.train_dir, pose) 205 | 206 | # validation loss 207 | loss = np.array(loss) 208 | np.save('%s/loss'%self.train_dir, pose) 209 | 210 | # validation error 211 | error = np.array([np.concatenate(error[0]), np.concatenate(error[1]), np.concatenate(error[2])]) 212 | joblib.dump(error, '%s/%s.pkl'%(self.train_dir, self.test_result_fn), compress=3) 213 | 214 | def main(_): 215 | 216 | working_dir = os.getcwd() 217 | os.chdir(os.path.dirname(os.path.realpath(__file__))) 218 | if FLAGS.experiment == 'cambridge': 219 | 220 | for dataset in ['KingsCollege', 'OldHospital', 'ShopFacade', 'StMarysChurch']: 221 | for lr in [1e-5]: 222 | for tmp_beta in [1]: 223 | FLAGS.beta = tmp_beta 224 | FLAGS.lr = lr 225 | FLAGS.dataset = dataset 226 | net = RPNet(FLAGS) 227 | net.process() 228 | 229 | else: 230 | print ('unknown experimentation') 231 | raise NotImplementedError 232 | os.chdir(working_dir) 233 | 234 | 235 | if __name__ == '__main__': 236 | 237 | 238 | parser = argparse.ArgumentParser() 239 | 240 | parser.register("type", "bool", lambda v: v.lower() == "true") 241 | 242 | parser.add_argument("--optimization", default= 'sgdm', type=str, help= "Optimization strategy: adadelta, sgd, sgdm") 243 | parser.add_argument("--lr", type = float, default = 1e-5, help = "Initial learning rate") 244 | parser.add_argument("--lr_decay_rate", type = float, default = 0.90, help = 'decay rate for learning rate') 245 | parser.add_argument("--lr_decay_step", type = int, default = 80, help = 'nb of epochs to decay learning rate, use big number of step to avoid decaying the lr') 246 | parser.add_argument("--nb_epoches", type = int, default = 10000, help = "Nb of epochs to train the model, alternative to --max_iteration") 247 | parser.add_argument("--batch_size", type = int, default = 64, help="Number of example per batch") 248 | parser.add_argument("--wd", type = float, default = 1e-2, help="weight decay on each layer, default = 0.0 means no decay") 249 | 250 | parser.add_argument("--beta", type = int, default = 1, help="beta to weight between the two losses as in the paper") 251 | 252 | parser.add_argument("--train_test_phase", default = 'train', help = 'train, validation or test phase') 253 | parser.add_argument("--dataset", default = 'OldHospital', type=str, help ='dataset name to train the network') 254 | parser.add_argument("--eval_interval_epoch", type = int, default = 100, help="nb of epochs after which the evaluation on validation should take place") 255 | 256 | # specific to this architecture 257 | parser.add_argument("--nb_run", default = 1, type = int, help ='each run per configuration, useful to compute std') 258 | parser.add_argument("--experiment", default= 'cambridge', help='group of experiments') 259 | parser.add_argument("--use_dropout", type = int, default = 0, help='use dropout or not 0 or 1') 260 | parser.add_argument("--continue_training", type = int, default = 0, help='continue training on the logdir if done.txt is not yet created. Use for resuming training') 261 | parser.add_argument("--use_extraLoss", type = int, default = 0, help='use extraLoss for absolute pose') 262 | 263 | 264 | FLAGS, unparsed = parser.parse_known_args() 265 | 266 | 267 | tf.app.run(main= main, argv=[sys.argv[0]] + unparsed) 268 | -------------------------------------------------------------------------------- /rpnet/rpnetfc.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jul 20, 2018 3 | 4 | @author: en 5 | ''' 6 | 7 | import argparse, sys, os 8 | from tensorflow.contrib.layers.python.layers.regularizers import l2_regularizer 9 | sys.path.append(os.path.dirname(os.path.realpath(__file__)) + '/../') 10 | from rpnet import data_preprocessing 11 | from rpnet import RPNet # @UnresolvedImport 12 | from abstract_network.setting import Setting 13 | from abstract_network.abstract import AbstractNetwork 14 | from GoogLeNet.googlenet_fc_big import GoogLeNetFCB 15 | import tensorflow as tf 16 | 17 | slim = tf.contrib.slim # @UndefinedVariable 18 | 19 | 20 | class RPNetFC(RPNet, AbstractNetwork): 21 | ''' 22 | classdocs 23 | ''' 24 | 25 | 26 | def __init__(self, flags): 27 | ''' 28 | Constructor 29 | ''' 30 | 31 | flags.name = 'RPNetFC' 32 | self.output_dim = 7 33 | 34 | self.wd = flags.wd 35 | self.beta = flags.beta 36 | setting = Setting('relative_pose_network') 37 | AbstractNetwork.__init__(self, setting, flags) 38 | self.train_dir = self._train_dir_path('_%d_%.4f'%(self.beta, self.wd)) 39 | if self.train_dir == '': return 40 | self.use_extraLoss = 0 41 | 42 | self.eval_interval_epoch = 50 43 | self.prefetch_data = 500 44 | 45 | self._get_train_test_set() 46 | train_fn, test_fn = data_preprocessing.get_read_tfrecord_fn(self.dataset) 47 | self.prepare_data(train_fn, test_fn) 48 | self.prepare_inference() 49 | self.prepare_loss() 50 | self.get_train_op() 51 | 52 | self.max_iteration = 750000 53 | 54 | # define the metric network architecture 55 | def _metric_nework(self, net1, net2, scope): 56 | 57 | x = tf.concat((net1, net2), axis = 1 , name = '%s_fusion'%scope) 58 | 59 | fc2 = tf.layers.dense(x, 128, activation=tf.nn.relu, use_bias=True, # @UndefinedVariable 60 | kernel_initializer=tf.truncated_normal_initializer(stddev=self.stddev, dtype=tf.float32), 61 | bias_initializer=tf.constant_initializer(0.01, dtype=tf.float32), 62 | kernel_regularizer=l2_regularizer(self.wd), 63 | bias_regularizer=l2_regularizer(self.wd), 64 | name='%s_fc2'%scope) 65 | 66 | 67 | tran = tf.layers.dense(fc2, 3, activation=None, use_bias=True, # @UndefinedVariable 68 | kernel_initializer=tf.truncated_normal_initializer(stddev=self.stddev, dtype=tf.float32), 69 | bias_initializer=tf.constant_initializer(0.01, dtype=tf.float32), 70 | kernel_regularizer=l2_regularizer(self.wd), 71 | bias_regularizer=l2_regularizer(self.wd), 72 | name='%s_fc3_translation'%scope) 73 | 74 | rotation = tf.layers.dense(fc2, 4, activation=None, use_bias=True, # @UndefinedVariable 75 | kernel_initializer=tf.truncated_normal_initializer(stddev=self.stddev, dtype=tf.float32), 76 | bias_initializer=tf.constant_initializer(0.01, dtype=tf.float32), 77 | kernel_regularizer=l2_regularizer(self.wd), 78 | bias_regularizer=l2_regularizer(self.wd), 79 | name='%s_fc3_rotation'%scope) 80 | 81 | 82 | return tran, rotation 83 | 84 | 85 | def prepare_inference(self): 86 | 87 | with tf.variable_scope("siamese") as scope: 88 | net_1 = GoogLeNetFCB({'data': self.input_X[:, 0]}) 89 | scope.reuse_variables() 90 | net_2 = GoogLeNetFCB({'data': self.input_X[:, 1]}) 91 | 92 | weight_vars = [v for v in tf.global_variables() if "weights" in v.name] 93 | print('weight var: ', len(weight_vars)) 94 | 95 | if self.wd > 0: 96 | for v in tf.global_variables(): 97 | if "weights" in v.name: 98 | tmp_loss = tf.nn.l2_loss(v) * self.wd 99 | tf.add_to_collection('losses', tmp_loss) 100 | 101 | 102 | self.xyz_logits, self.wpqr_logits = self._metric_nework(net_2.layers['cls3_fc1'], net_1.layers['cls3_fc1'], scope = 'cls3') 103 | self.aux1_xyz_logits, self.aux1_wpqr_logits = self._metric_nework(net_2.layers['cls1_fc1'], net_1.layers['cls1_fc1'], scope = 'cls1') 104 | self.aux2_xyz_logits, self.aux2_wpqr_logits = self._metric_nework(net_2.layers['cls2_fc1'], net_1.layers['cls2_fc1'], scope = 'cls2') 105 | 106 | self.net_1 = net_1 107 | self.net_2 = net_2 108 | 109 | 110 | def main(_): 111 | 112 | working_dir = os.getcwd() 113 | os.chdir(os.path.dirname(os.path.realpath(__file__))) 114 | if FLAGS.experiment == 'cambridge': 115 | 116 | for dataset in ['KingsCollege', 'OldHospital', 'StMarysChurch', 'ShopFacade']: 117 | for lr in [1e-3]: 118 | for wd in [1e-2]: 119 | for tmp_beta in [10]: 120 | FLAGS.wd = wd 121 | FLAGS.lr = lr 122 | FLAGS.beta = tmp_beta 123 | FLAGS.dataset = dataset 124 | net = RPNetFC(FLAGS) 125 | net.process() 126 | 127 | else: 128 | print ('unknown experimentation') 129 | raise NotImplementedError 130 | os.chdir(working_dir) 131 | 132 | 133 | if __name__ == '__main__': 134 | 135 | 136 | parser = argparse.ArgumentParser() 137 | 138 | parser.register("type", "bool", lambda v: v.lower() == "true") 139 | 140 | parser.add_argument("--optimization", default= 'sgdm', type=str, help= "use adadelta, sgd, sgdm") 141 | parser.add_argument("--lr", type = float, default = 1e-3, help = "Initial learning rate") 142 | parser.add_argument("--lr_decay_rate", type = float, default = 0.90, help = 'decay rate for learning rate') 143 | parser.add_argument("--lr_decay_step", type = int, default = 80, help = 'step to decay learning rate, use big number of step to avoid decaying the lr') 144 | parser.add_argument("--nb_epoches", type = int, default = 1000, help = "Nb of epoches to train the model, alternative to --max-step") 145 | parser.add_argument("--batch_size", type = int, default = 32, help="Number of example per batch") 146 | parser.add_argument("--wd", type = float, default = 1e-2, help="weight decay on each layer, default = 0.0 mean no decay") 147 | 148 | parser.add_argument("--beta", type = int, default = 1, help="beta to weight between the two losses as in the paper") 149 | 150 | parser.add_argument("--train_test_phase", default = 'train', help = 'train, validation or test') 151 | parser.add_argument("--dataset", default = 'OldHospital', type=str, help ='dataset name to train the network') 152 | parser.add_argument("--eval_interval_epoch", type = int, default = 100, help="nb of epoches after which the evaluation on validation set takes place") 153 | 154 | # specific to this architecture 155 | parser.add_argument("--nb_run", default = 1, type=int, help ='each run per configuration') 156 | parser.add_argument("--experiment", default= 'cambridge', help='group of experiments') 157 | parser.add_argument("--use_dropout", type = int, default = 0, help='use dropout or not 0 or 1') 158 | parser.add_argument("--continue_training", type = int, default = 0, help='continue training on the logdir if done.txt is not yet created') 159 | 160 | 161 | FLAGS, unparsed = parser.parse_known_args() 162 | 163 | 164 | tf.app.run(main= main, argv=[sys.argv[0]] + unparsed) 165 | -------------------------------------------------------------------------------- /rpnet/rpnetplus.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jul 20, 2018 3 | 4 | @author: en 5 | ''' 6 | 7 | import argparse, sys, os 8 | sys.path.append(os.path.dirname(os.path.realpath(__file__)) + '/../') 9 | from rpnet import RPNet, data_preprocessing # @UnresolvedImport 10 | from abstract_network.setting import Setting 11 | from abstract_network.abstract import AbstractNetwork 12 | import tensorflow as tf 13 | 14 | 15 | 16 | slim = tf.contrib.slim # @UndefinedVariable 17 | 18 | class RPNetPlus(RPNet, AbstractNetwork): 19 | ''' 20 | classdocs 21 | ''' 22 | 23 | 24 | def __init__(self, flags): 25 | ''' 26 | Constructor 27 | ''' 28 | 29 | flags.name = 'RPNetPlus' 30 | self.output_dim = 7 31 | 32 | self.use_extraLoss = flags.use_extraLoss 33 | self.wd = flags.wd 34 | self.beta = flags.beta 35 | self.alpha = flags.alpha 36 | self.gamma = flags.gamma 37 | self.use_aux = flags.use_aux 38 | setting = Setting('relative_pose_network') 39 | AbstractNetwork.__init__(self, setting, flags) 40 | self.train_dir = self._train_dir_path('_%d_%.4f_%d_%d_%d_%d'%(self.beta, self.wd, self.use_extraLoss, self.gamma, self.alpha, self.use_aux)) 41 | if self.train_dir == '': return 42 | 43 | self.eval_interval_epoch = 50 44 | self.prefetch_data = 500 45 | 46 | self._get_train_test_set() 47 | train_fn, test_fn = data_preprocessing.get_read_tfrecord_fn(self.dataset, self.use_extraLoss) 48 | self.prepare_data(train_fn, test_fn) 49 | self.prepare_inference() 50 | self.prepare_loss() 51 | self.get_train_op() 52 | 53 | self.max_iteration = 200000 54 | 55 | def extra_loss_4(self, net, pose): 56 | self._l2_norm_loss(net.layers['cls3_fc_pose_xyz'], labels=pose[:, :3], weights=1., scope='Train/Extra_1_translation_loss') 57 | self._l2_norm_loss(net.layers['cls3_fc_pose_wpqr'], labels=pose[:, 3:], weights=self.beta, scope='Train/Extra_1_rotation_loss') 58 | 59 | if self.use_aux: 60 | # Aux classifiers 61 | self._l2_norm_loss(net.layers['cls2_fc_pose_xyz'], labels=pose[:, :3], weights=1./ 3.33) 62 | self._l2_norm_loss(net.layers['cls2_fc_pose_wpqr'], labels=pose[:, 3:], weights=self.beta/3.33) 63 | self._l2_norm_loss(net.layers['cls1_fc_pose_xyz'], labels=pose[:, :3], weights=1./3.33) 64 | self._l2_norm_loss(net.layers['cls1_fc_pose_wpqr'], labels=pose[:, 3:], weights=self.beta/3.33) 65 | 66 | def prepare_loss(self): 67 | 68 | self.translation_weight = self.gamma 69 | 70 | self.tran_loss = self._l2_norm_loss(self.xyz_logits, labels=self.input_Y[:, :3], weights=self.translation_weight, scope='Train/translation_loss') 71 | self.rotat_loss = self._l2_norm_loss(self.wpqr_logits, labels=self.input_Y[:, 3:], weights=self.alpha, scope='Train/rotation_loss') 72 | 73 | if self.use_aux: 74 | # Aux classifiers 75 | self.tran_loss2 = self._l2_norm_loss(self.aux2_xyz_logits, labels=self.input_Y[:, :3], weights=self.translation_weight/3.33, scope='Train/aux2_translation_loss') 76 | self.rotat_loss2 = self._l2_norm_loss(self.aux2_wpqr_logits, labels=self.input_Y[:, 3:], weights=self.alpha/3.33, scope='Train/aux2_rotation_loss') 77 | self.tran_loss1 = self._l2_norm_loss(self.aux1_xyz_logits, labels=self.input_Y[:, :3], weights=self.translation_weight/3.33, scope='Train/aux1_translation_loss') 78 | self.rotat_loss1 = self._l2_norm_loss(self.aux1_wpqr_logits, labels=self.input_Y[:, 3:], weights=self.alpha/3.33, scope='Train/aux1_rotation_loss') 79 | 80 | if self.use_extraLoss: 81 | self.extra_loss_4(self.net_1, self.extra_input_Y1) 82 | self.extra_loss_4(self.net_2, self.extra_input_Y2) 83 | 84 | def main(_): 85 | 86 | working_dir = os.getcwd() 87 | os.chdir(os.path.dirname(os.path.realpath(__file__))) 88 | 89 | if FLAGS.experiment == 'cambridge': 90 | 91 | for dataset in ['KingsCollege', 'OldHospital', 'ShopFacade', 'StMarysChurch']: 92 | for lr in [1e-5]: 93 | FLAGS.lr = lr 94 | FLAGS.dataset = dataset 95 | net = RPNetPlus(FLAGS) 96 | net.process() 97 | else: 98 | print ('unknown experimentation') 99 | raise NotImplementedError 100 | os.chdir(working_dir) 101 | 102 | 103 | if __name__ == '__main__': 104 | 105 | 106 | parser = argparse.ArgumentParser() 107 | 108 | parser.register("type", "bool", lambda v: v.lower() == "true") 109 | 110 | parser.add_argument("--optimization", default= 'sgdm', type=str, help= "Optimization strategy: adadelta, sgd, sgdm") 111 | parser.add_argument("--lr", type = float, default = 1e-5, help = "Initial learning rate") 112 | parser.add_argument("--lr_decay_rate", type = float, default = 0.90, help = 'decay rate for learning rate') 113 | parser.add_argument("--lr_decay_step", type = int, default = 80, help = 'nb of epochs to decay learning rate, use big number of step to avoid decaying the lr') 114 | parser.add_argument("--nb_epoches", type = int, default = 10000, help = "Nb of epochs to train the model, alternative to --max_iteration") 115 | parser.add_argument("--batch_size", type = int, default = 64, help="Number of example per batch") 116 | parser.add_argument("--wd", type = float, default = 1e-2, help="weight decay on each layer, default = 0.0 means no decay") 117 | 118 | parser.add_argument("--beta", type = int, default = 1, help="beta to weight between the two losses as in the paper") 119 | 120 | parser.add_argument("--train_test_phase", default = 'train', help = 'train, validation or test phase') 121 | parser.add_argument("--dataset", default = 'OldHospital', type=str, help ='dataset name to train the network') 122 | parser.add_argument("--eval_interval_epoch", type = int, default = 100, help="nb of epochs after which the evaluation on validation should take place") 123 | 124 | # specific to this architecture 125 | parser.add_argument("--nb_run", default = 1, type = int, help ='each run per configuration, useful to compute std') 126 | parser.add_argument("--experiment", default= 'cambridge', help='group of experiments') 127 | parser.add_argument("--use_dropout", type = int, default = 0, help='use dropout or not 0 or 1') 128 | parser.add_argument("--continue_training", type = int, default = 0, help='continue training on the logdir if done.txt is not yet created. Use for resuming training') 129 | parser.add_argument("--use_extraLoss", type = int, default = 1, help='use extraLoss for absolute pose') 130 | parser.add_argument("--gamma", type = int, default = 10, help='weight for relative translation loss') 131 | parser.add_argument("--alpha", type = int, default = 100, help='weight for relative rotation loss') 132 | parser.add_argument("--use_aux", type = int, default = 0, help = "use auxilary loss of googLeNet or not") 133 | 134 | FLAGS, unparsed = parser.parse_known_args() 135 | 136 | 137 | tf.app.run(main= main, argv=[sys.argv[0]] + unparsed) 138 | -------------------------------------------------------------------------------- /surf/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ensv/RPNet/f4b3fbf510d47801e5b4a1e8b9001a5bb3038a1b/surf/__init__.py -------------------------------------------------------------------------------- /surf/relative_cambridge.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jul 25, 2018 3 | 4 | @author: sovann 5 | ''' 6 | 7 | import matplotlib # @UnusedImport 8 | import ctypes 9 | matplotlib.use('Agg') 10 | import matplotlib.pyplot as plt 11 | import numpy as np 12 | import math 13 | import cv2 # @UnresolvedImport 14 | from multiprocessing.pool import Pool 15 | import multiprocessing 16 | from pyquaternion.quaternion import Quaternion 17 | import os, sys 18 | sys.path.append(os.path.dirname(os.path.realpath(__file__)) + '/../') 19 | cv2.setNumThreads(0) # @UndefinedVariable 20 | 21 | path = '/data/chercheurs/en/' 22 | 23 | def create_share_ndarray(shape, dtype, cdtype): 24 | 25 | total_shape = 1 26 | for ele in shape: 27 | total_shape *= ele 28 | shared_base = multiprocessing.Array(cdtype, total_shape) # @UndefinedVariable 29 | shared_array = np.frombuffer(shared_base.get_obj(), dtype=dtype) 30 | shared_array = np.reshape(shared_array, shape) 31 | assert shared_array.base.base is shared_base.get_obj() 32 | return shared_array 33 | 34 | def create_share_from_arr(arr, cdtype): 35 | 36 | shared_arr = create_share_ndarray(arr.shape, arr.dtype, cdtype) 37 | shared_arr[:] = arr[:] 38 | return shared_arr 39 | 40 | 41 | def get_Rerror_in_degree(vec1, vec2): 42 | d = np.abs(np.sum(np.multiply(vec1, vec2), axis=1)) 43 | np.putmask(d, d> 1, 1) 44 | np.putmask(d, d< -1, -1) 45 | return 2 * np.arccos(d) * 180/math.pi 46 | 47 | def get_Terror_in_degree(vec1, vec2): 48 | d = np.sum(np.multiply(vec1, vec2), axis=1) 49 | np.putmask(d, d> 1, 1) 50 | np.putmask(d, d< -1, -1) 51 | return np.arccos(d) * 180/math.pi 52 | 53 | 54 | def get_cameraMatrix(f, cx = 455, cy=256): 55 | camera_mat = np.zeros((3, 3), 'f4') 56 | camera_mat[0, 0] = f / (1080/256) 57 | camera_mat[1, 1] = f / (1080/256) 58 | camera_mat[:2, 2] = (cx/2, cy/2) 59 | camera_mat[2, 2] = 1 60 | return camera_mat 61 | 62 | def normalize_point(point, mat): 63 | point = np.hstack((point, np.ones((point.shape[0], 1)))) 64 | return np.dot(np.linalg.inv(mat), point.T).T 65 | 66 | def get_relative_pose(idx): 67 | img1, img2 = annot[idx, 0], annot[idx, 8] 68 | 69 | f1 = txt['/'.join(img1.split('/')[-2:]).replace('.png', '.jpg')][0] 70 | f2 = txt['/'.join(img2.split('/')[-2:]).replace('.png', '.jpg')][0] 71 | 72 | im1 = images[idx, 0] 73 | im2 = images[idx, 1] 74 | 75 | assert im1.shape[0] < im1.shape[1] 76 | 77 | if center_crop: 78 | im1 = im1[16:240, 116:340] 79 | im2 = im2[16:240, 116:340] 80 | 81 | sift = cv2.xfeatures2d.SIFT_create() # @UndefinedVariable 82 | 83 | kp1, des1 = sift.detectAndCompute(im1, None) 84 | kp2, des2 = sift.detectAndCompute(im2, None) 85 | 86 | # create BFMatcher object 87 | bf = cv2.BFMatcher(cv2.NORM_L2, crossCheck=True) # @UndefinedVariable 88 | 89 | # Match descriptors. 90 | try: 91 | matches = bf.match(des1, des2) 92 | except cv2.error: # @UndefinedVariable 93 | return np.array([0.333, 0.333, 0.3334, 0.25, 0.25, 0.25, 0.25], 'f4') 94 | 95 | 96 | # Sort them in the order of their distance. 97 | matches = sorted(matches, key = lambda x:x.distance)[:100] 98 | 99 | # Draw first 10 matches. 100 | # draw_params = dict(#matchColor = (0,255,0), # draw matches in green color 101 | # singlePointColor = None, 102 | # flags = 2) 103 | # img3 = cv2.drawMatches(im1, kp1, im2, kp2, matches[:20], None, **draw_params) 104 | # plt.figure(figsize=(20,20)) 105 | # plt.imshow(img3) 106 | # plt.show() 107 | if center_crop: 108 | mat1 = get_cameraMatrix(f1, cx = 224-0.5, cy = 224) 109 | mat2 = get_cameraMatrix(f2, cx = 224-0.5, cy = 224) 110 | else: 111 | mat1 = get_cameraMatrix(f1, cx = 455, cy = 256) 112 | mat2 = get_cameraMatrix(f2, cx = 455, cy = 256) 113 | 114 | src = np.float32([ kp1[m.queryIdx].pt for m in matches ]).reshape(-1,2) 115 | dst = np.float32([ kp2[m.trainIdx].pt for m in matches ]).reshape(-1,2) 116 | 117 | src = normalize_point(src, mat1) 118 | dst = normalize_point(dst, mat2) 119 | 120 | src = src[:, :2] 121 | dst = dst[:, :2] 122 | 123 | try: 124 | E, mask = cv2.findEssentialMat(src, dst, cameraMatrix = np.identity(3, 'f4'), threshold = _threshold) # @UndefinedVariable 125 | 126 | _, R, t, mask, _ = cv2.recoverPose(E, src, dst, mask = mask, cameraMatrix = np.identity(3, 'f4'), distanceThresh = 50) # @UndefinedVariable 127 | except cv2.error: # @UndefinedVariable 128 | return np.array([0.333, 0.333, 0.3334, 0.25, 0.25, 0.25, 0.25], 'f4') 129 | 130 | # matchesMask = mask.ravel().tolist() 131 | # draw_params = dict(#matchColor = (0,255,0), # draw matches in green color 132 | # singlePointColor = None, 133 | # matchesMask = matchesMask, # draw only inliers 134 | # flags = 2) 135 | # img3 = cv2.drawMatches(im1, kp1, im2, kp2, matches, None, **draw_params) 136 | # plt.figure(figsize=(20,20)) 137 | # plt.imshow(img3) 138 | # plt.show() 139 | 140 | return np.concatenate((t.flatten(), np.array(Quaternion(matrix=R).elements).astype('f4').flatten())) 141 | 142 | def _save_plot(arr, name): 143 | 144 | cumulative = np.array([np.where(arr <= e)[0].shape[0] for e in range(180)]) / arr.shape[0] 145 | 146 | plt.figure(figsize=(8, 6)) 147 | plt.plot(range(180), cumulative) 148 | plt.grid(True) 149 | plt.title(name.split('_')[0]) 150 | plt.savefig(name, dpi = 300) 151 | plt.close() 152 | # plt.show(True) 153 | 154 | def save_plot(R_error, T_error, name): 155 | _save_plot(R_error, '%s/R.png'%name) 156 | _save_plot(T_error, '%s/T.png'%name) 157 | 158 | 159 | def get_Terror_in_meter(vec1, vec2): 160 | return np.sqrt(np.sum((vec1 - vec2)**2, axis=1 )) 161 | 162 | 163 | def get_relative_pose_np(r1, r2, t1, t2): 164 | 165 | r1, r2, t1, t2 = r1.astype('f4'), r2.astype('f4'), t1.astype('f4'), t2.astype('f4') 166 | pose = [] 167 | for i in range(t1.shape[0]): 168 | 169 | rr = (Quaternion(r2[i]) * Quaternion(r1[i]).inverse).elements 170 | R2 = Quaternion(r2[i]).rotation_matrix 171 | new_c2 = - np.dot(R2, t2[i]) 172 | rt = np.dot(R2, t1[i]) + new_c2 173 | 174 | pose.append(np.concatenate((rt / np.linalg.norm(rt), rr))) 175 | return np.array(pose) 176 | 177 | 178 | def _read_resize_image(idx): 179 | 180 | 181 | 182 | im1 = cv2.imread(annot[idx][0].replace('/data/chercheurs/en', path)) # @UndefinedVariable 183 | im1 = cv2.resize(im1, (455, 256)) # @UndefinedVariable 184 | 185 | im2 = cv2.imread(annot[idx][8].replace('/data/chercheurs/en', path)) # @UndefinedVariable 186 | im2 = cv2.resize(im2, (455, 256)) # @UndefinedVariable 187 | 188 | images[idx] = np.array([im1, im2]) 189 | 190 | 191 | if __name__ == '__main__': 192 | 193 | 194 | center_crop = 1 195 | 196 | for dataset in ['KingsCollege', 'OldHospital', 'ShopFacade', 'StMarysChurch']: 197 | 198 | _threshold = 1e-3 199 | if dataset == 'ShopFacade': 200 | ShopFacade = 1e-2 201 | print('\n====================================\n') 202 | print('dataset: ', dataset) 203 | 204 | train_test_set = 'test' 205 | 206 | base = '%s/DRP/relative_cambridge/%s'%(path, dataset) 207 | 208 | annot = open('%s/DRP/relative_cambridge/%s/test_set.txt'%(path, dataset)).readlines()[3:] 209 | annot = np.array([e.split() for e in annot]) 210 | gt_pose = get_relative_pose_np(annot[:, 4:8], annot[:, 12:16], annot[:, 1:4], annot[:, 9:12]) 211 | 212 | if os.path.exists('%s/images.npy'%base): 213 | images = np.load('%s/images.npy'%base) 214 | images = create_share_from_arr(images, ctypes.c_uint8) 215 | else: 216 | images = create_share_ndarray((len(annot), 2, 256, 455, 3), 'uint8', ctypes.c_uint8) 217 | pool = Pool(processes = 30) 218 | pool.map(_read_resize_image, range(annot.shape[0])) 219 | pool.close() 220 | pool.join() 221 | np.save('%s/images'%base, images) 222 | 223 | txt = [e for e in open('%s/DRP/relative_cambridge/%s/reconstruction.nvm'%(path, dataset)).readlines()[3:] if 'seq' in e] 224 | txt = {e.split('\t')[0]:np.array(e.split('\t')[1].split(), 'f4') for e in txt if len(e.split('\t')) >= 2} 225 | 226 | try: 227 | os.makedirs('%s/DRP/baseline/%s/SURF/%0.4f_%d/'%(path, dataset, _threshold, center_crop)) 228 | except: 229 | pass 230 | 231 | filename = '%s/DRP/baseline/%s/SURF/%0.4f_%d/'%(path, dataset, _threshold, center_crop) 232 | 233 | pool = Pool(processes=32) # @UndefinedVariable 234 | est_pose = pool.map(get_relative_pose, range(annot.shape[0]), chunksize=1) 235 | pool.close() 236 | pool.join() 237 | 238 | est_pose = np.vstack(est_pose) 239 | xyz, wpqr = est_pose[:, :3], est_pose[:, 3:] 240 | 241 | T = gt_pose[:, :3] 242 | R = gt_pose[:, 3:] 243 | 244 | R_error = get_Rerror_in_degree(wpqr, R) 245 | 246 | T_error = get_Terror_in_degree(xyz, T) 247 | T_error_meter = get_Terror_in_meter(xyz, T) 248 | 249 | print('Threshold:', _threshold) 250 | print('Rotation: ', np.median(R_error)) 251 | print('Translation: ', np.median(T_error)) 252 | print('Translation (meter):', np.median(T_error_meter)) 253 | print() 254 | 255 | -------------------------------------------------------------------------------- /surf/relative_cambridge_full_size.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jul 25, 2018 3 | 4 | @author: sovann 5 | ''' 6 | 7 | import matplotlib # @UnusedImport 8 | import ctypes 9 | matplotlib.use('Agg') 10 | import matplotlib.pyplot as plt 11 | import numpy as np 12 | import math 13 | import cv2 # @UnresolvedImport 14 | from multiprocessing.pool import Pool 15 | import multiprocessing 16 | from pyquaternion.quaternion import Quaternion 17 | import os, sys 18 | sys.path.append(os.path.dirname(os.path.realpath(__file__)) + '/../') 19 | cv2.setNumThreads(0) # @UndefinedVariable 20 | 21 | path = '/data/chercheurs/en/' 22 | 23 | 24 | def create_share_ndarray(shape, dtype, cdtype): 25 | 26 | total_shape = 1 27 | for ele in shape: 28 | total_shape *= ele 29 | shared_base = multiprocessing.Array(cdtype, total_shape) # @UndefinedVariable 30 | shared_array = np.frombuffer(shared_base.get_obj(), dtype=dtype) 31 | shared_array = np.reshape(shared_array, shape) 32 | assert shared_array.base.base is shared_base.get_obj() 33 | return shared_array 34 | 35 | def create_share_from_arr(arr, cdtype): 36 | 37 | shared_arr = create_share_ndarray(arr.shape, arr.dtype, cdtype) 38 | shared_arr[:] = arr[:] 39 | return shared_arr 40 | 41 | 42 | def get_Rerror_in_degree(vec1, vec2): 43 | d = np.abs(np.sum(np.multiply(vec1, vec2), axis=1)) 44 | np.putmask(d, d> 1, 1) 45 | np.putmask(d, d< -1, -1) 46 | return 2 * np.arccos(d) * 180/math.pi 47 | 48 | def get_Terror_in_degree(vec1, vec2): 49 | d = np.sum(np.multiply(vec1, vec2), axis=1) 50 | np.putmask(d, d> 1, 1) 51 | np.putmask(d, d< -1, -1) 52 | return np.arccos(d) * 180/math.pi 53 | 54 | 55 | def get_cameraMatrix(f, cx = 455, cy=256): 56 | camera_mat = np.zeros((3, 3), 'f4') 57 | camera_mat[0, 0] = f 58 | camera_mat[1, 1] = f 59 | camera_mat[:2, 2] = (cx/2, cy/2) 60 | camera_mat[2, 2] = 1 61 | return camera_mat 62 | 63 | def normalize_point(point, mat): 64 | point = np.hstack((point, np.ones((point.shape[0], 1)))) 65 | return np.dot(np.linalg.inv(mat), point.T).T 66 | 67 | def get_relative_pose(idx): 68 | img1, img2 = annot[idx, 0], annot[idx, 8] 69 | 70 | f1 = txt['/'.join(img1.split('/')[-2:]).replace('.png', '.jpg')][0] 71 | f2 = txt['/'.join(img2.split('/')[-2:]).replace('.png', '.jpg')][0] 72 | 73 | im1 = cv2.imread(img1, 0) # @UndefinedVariable 74 | im2 = cv2.imread(img2, 0) # @UndefinedVariable 75 | 76 | assert im1.shape[0] < im1.shape[1] 77 | 78 | sift = cv2.xfeatures2d.SIFT_create() # @UndefinedVariable 79 | 80 | kp1, des1 = sift.detectAndCompute(im1, None) 81 | kp2, des2 = sift.detectAndCompute(im2, None) 82 | 83 | # create BFMatcher object 84 | bf = cv2.BFMatcher(cv2.NORM_L2, crossCheck=True) # @UndefinedVariable 85 | 86 | # Match descriptors. 87 | try: 88 | matches = bf.match(des1, des2) 89 | except cv2.error: # @UndefinedVariable 90 | return np.array([0.333, 0.333, 0.3334, 0.25, 0.25, 0.25, 0.25], 'f4') 91 | 92 | 93 | # Sort them in the order of their distance. 94 | matches = sorted(matches, key = lambda x:x.distance)[:100] 95 | 96 | # Draw first 10 matches. 97 | # draw_params = dict(#matchColor = (0,255,0), # draw matches in green color 98 | # singlePointColor = None, 99 | # flags = 2) 100 | # img3 = cv2.drawMatches(im1, kp1, im2, kp2, matches[:20], None, **draw_params) 101 | # plt.figure(figsize=(20,20)) 102 | # plt.imshow(img3) 103 | # plt.show() 104 | 105 | mat1 = get_cameraMatrix(f1, cx = 1920, cy = 1080) 106 | mat2 = get_cameraMatrix(f2, cx = 1920, cy = 1080) 107 | 108 | src = np.float32([ kp1[m.queryIdx].pt for m in matches ]).reshape(-1,2) 109 | dst = np.float32([ kp2[m.trainIdx].pt for m in matches ]).reshape(-1,2) 110 | 111 | src = normalize_point(src, mat1) 112 | dst = normalize_point(dst, mat2) 113 | 114 | src = src[:, :2] 115 | dst = dst[:, :2] 116 | 117 | tmp_results = [] 118 | 119 | for _threshold in [1e-2, 1e-3, 1e-4]: 120 | 121 | try: 122 | E, mask = cv2.findEssentialMat(np.copy(src), np.copy(dst), cameraMatrix = np.identity(3, 'f4'), threshold = _threshold) # @UndefinedVariable 123 | _, R, t, mask, _ = cv2.recoverPose(E, np.copy(src), np.copy(dst), mask = mask, cameraMatrix = np.identity(3, 'f4'), distanceThresh = 50) # @UndefinedVariable 124 | except cv2.error: # @UndefinedVariable 125 | tmp_results.append(np.array([0.333, 0.333, 0.3334, 0.25, 0.25, 0.25, 0.25], 'f4')) 126 | continue 127 | tmp_results.append(np.concatenate((t.flatten(), np.array(Quaternion(matrix=R).elements).astype('f4').flatten()))) 128 | 129 | return np.array(tmp_results) 130 | 131 | def _save_plot(arr, name): 132 | 133 | cumulative = np.array([np.where(arr <= e)[0].shape[0] for e in range(180)]) / arr.shape[0] 134 | 135 | plt.figure(figsize=(8, 6)) 136 | plt.plot(range(180), cumulative) 137 | plt.grid(True) 138 | plt.title(name.split('_')[0]) 139 | plt.savefig(name, dpi = 300) 140 | plt.close() 141 | # plt.show(True) 142 | 143 | def save_plot(R_error, T_error, name): 144 | _save_plot(R_error, '%s/R.png'%name) 145 | _save_plot(T_error, '%s/T.png'%name) 146 | 147 | 148 | def get_Terror_in_meter(vec1, vec2): 149 | return np.sqrt(np.sum((vec1 - vec2)**2, axis=1 )) 150 | 151 | 152 | def get_relative_pose_np(r1, r2, t1, t2): 153 | 154 | r1, r2, t1, t2 = r1.astype('f4'), r2.astype('f4'), t1.astype('f4'), t2.astype('f4') 155 | pose = [] 156 | for i in range(t1.shape[0]): 157 | 158 | rr = (Quaternion(r2[i]) * Quaternion(r1[i]).inverse).elements 159 | R2 = Quaternion(r2[i]).rotation_matrix 160 | new_c2 = - np.dot(R2, t2[i]) 161 | rt = np.dot(R2, t1[i]) + new_c2 162 | 163 | pose.append(np.concatenate((rt / np.linalg.norm(rt), rr))) 164 | return np.array(pose) 165 | 166 | if __name__ == '__main__': 167 | 168 | 169 | for dataset in ['KingsCollege', 'OldHospital', 'ShopFacade', 'StMarysChurch']: 170 | print('\n====================================\n') 171 | print('dataset: ', dataset) 172 | 173 | train_test_set = 'test' 174 | 175 | base = '%s/DRP/relative_cambridge/%s'%(path, dataset) 176 | 177 | annot = open('%s/DRP/relative_cambridge/%s/test_set.txt'%(path, dataset)).readlines()[3:] 178 | annot = np.array([e.split() for e in annot]) 179 | gt_pose = get_relative_pose_np(annot[:, 4:8], annot[:, 12:16], annot[:, 1:4], annot[:, 9:12]) 180 | 181 | txt = [e for e in open('%s/DRP/relative_cambridge/%s/reconstruction.nvm'%(path, dataset)).readlines()[3:] if 'seq' in e] 182 | txt = {e.split('\t')[0]:np.array(e.split('\t')[1].split(), 'f4') for e in txt if len(e.split('\t')) >= 2} 183 | 184 | try: 185 | os.makedirs('%s/DRP/baseline/%s/SURF/full/'%(path, dataset)) 186 | except: 187 | pass 188 | 189 | 190 | pool = Pool(processes=32) # @UndefinedVariable 191 | est_pose = pool.map(get_relative_pose, range(annot.shape[0]), chunksize=1) 192 | pool.close() 193 | pool.join() 194 | 195 | est_pose = np.vstack(est_pose) 196 | est_pose = est_pose.reshape((gt_pose.shape[0], 3, 7)) 197 | 198 | for i, _threshold in enumerate([1e-2, 1e-3, 1e-4]): 199 | 200 | filename = '%s/DRP/baseline/%s/SURF/test_full_%f/'%(path, dataset, _threshold) 201 | 202 | try: 203 | os.makedirs(filename) 204 | except: 205 | pass 206 | 207 | 208 | xyz, wpqr = est_pose[:, i, :3], est_pose[:, i, 3:] 209 | 210 | T = gt_pose[:, :3] 211 | R = gt_pose[:, 3:] 212 | 213 | R_error = get_Rerror_in_degree(wpqr, R) 214 | 215 | T_error = get_Terror_in_degree(xyz, T) 216 | T_error_meter = get_Terror_in_meter(xyz, T) 217 | 218 | print('Threshold:', _threshold) 219 | print('Rotation: ', np.median(R_error)) 220 | print('Translation: ', np.median(T_error)) 221 | print('Translation (meter):', np.median(T_error_meter)) 222 | print() 223 | 224 | # np.save('%sResults'%filename, np.array([np.median(R_error), np.median(T_error), np.median(T_error_meter)])) 225 | # np.save('%sRerror'%filename, R_error) 226 | # np.save('%sTerror'%filename, T_error) 227 | # save_plot(R_error, T_error, filename) 228 | -------------------------------------------------------------------------------- /toy_example/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ensv/RPNet/f4b3fbf510d47801e5b4a1e8b9001a5bb3038a1b/toy_example/__init__.py -------------------------------------------------------------------------------- /toy_example/camera.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jun 19, 2018 3 | 4 | @author: en 5 | ''' 6 | 7 | import numpy as np 8 | import math 9 | from math import sin, cos 10 | from pyquaternion.quaternion import Quaternion 11 | import sys 12 | from numpy import arctan 13 | class Camera: 14 | ''' 15 | classdocs 16 | ''' 17 | 18 | 19 | def __init__(self, center_3d, pos = None, name = 'cam1'): 20 | ''' 21 | Constructor 22 | ''' 23 | 24 | self.focal = 1 25 | self.pp = (0,0) 26 | 27 | self.camera_matrix = np.zeros((3, 3), 'f4') 28 | self.camera_matrix[0, 0] = self.camera_matrix[1, 1] = self.focal 29 | self.camera_matrix[:2, 2] = self.pp 30 | self.camera_matrix[2, 2] = 1 31 | 32 | self.name = name 33 | if pos is None: 34 | self.pos = (np.random.randint(0, 100, 3) - 10) / 10 35 | # self.pos = (np.random.randint(0, 100, 3) - 50) / 10 36 | else: 37 | self.pos = pos 38 | 39 | self.center_vector = (center_3d - self.pos) / np.linalg.norm((center_3d - self.pos)) 40 | self.euler, self.orientation_vector = self.get_orientation_vector(self.center_vector) 41 | 42 | self.Proj = self.get_projection_matrix(self.euler, self.pos) 43 | 44 | def get_orientation_vector(self, ref_vec): 45 | while True: 46 | euler = np.random.randint(0, 180, 3000) * math.pi/180 47 | euler = euler.reshape((1000, 3)) 48 | vec = np.array([self.euler_2_vector(each) for each in euler]) 49 | 50 | angle = np.arccos(np.sum(vec * ref_vec.reshape((1, 3)), axis=1)) * 180/math.pi 51 | 52 | idx = np.where(angle <25)[0] 53 | if idx.shape[0] > 0: 54 | return euler[idx[0]], vec[idx[0]] 55 | 56 | def euler_2_vector(self, param): 57 | yaw, pitch, roll = param 58 | return np.array([ 59 | sin(yaw)*sin(roll) + cos(yaw) * sin(pitch) * cos(roll), 60 | -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), 61 | cos(pitch) * cos(roll) 62 | ]) 63 | 64 | 65 | # code from wikipedia (default convention) 66 | def angle2quat(self, arr): 67 | yaw, pitch, roll = arr 68 | cy = math.cos(yaw * 0.5); 69 | sy = math.sin(yaw * 0.5); 70 | cr = math.cos(roll * 0.5); 71 | sr = math.sin(roll * 0.5); 72 | cp = math.cos(pitch * 0.5); 73 | sp = math.sin(pitch * 0.5); 74 | w = cy * cr * cp + sy * sr * sp; 75 | x = cy * sr * cp - sy * cr * sp; 76 | y = cy * cr * sp + sy * sr * cp; 77 | z = sy * cr * cp - cy * sr * sp; 78 | return np.array([w, x, y, z]) 79 | 80 | def angle2mat(self, arr): 81 | t1, t2, t3 = arr 82 | return np.array([ 83 | math.cos(t1)*math.cos(t2), 84 | math.cos(t1)*math.sin(t2)*math.sin(t3)-math.sin(t1)*math.cos(t3), 85 | math.cos(t1)*math.sin(t2)*math.cos(t3)+math.sin(t1)*math.sin(t3), 86 | math.sin(t1)*math.cos(t2), 87 | math.sin(t1)*math.sin(t2)*math.sin(t3)+math.cos(t1)*math.cos(t3), 88 | -math.sin(t2), 89 | math.cos(t2)*math.sin(t3), 90 | math.cos(t2)*math.cos(t3), 91 | ]).reshape((3, 3)).astype('f4') 92 | 93 | def get_projection_matrix(self, R, T): 94 | 95 | q = self.angle2quat(R) 96 | mat = self.quat2matrix(q) 97 | 98 | R = np.zeros((3, 4), 'f4') 99 | R[:3, :3] = mat.T 100 | R[:, 3] = - np.dot(mat.T, T) 101 | 102 | return R 103 | 104 | 105 | # code: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm 106 | def quat2matrix(self, q): 107 | w, x, y, z = q 108 | sqw = w*w; 109 | sqx = x*x; 110 | sqy = y*y; 111 | sqz = z*z; 112 | 113 | # invs (inverse square length) is only required if quaternion is not already normalised 114 | invs = 1 / (sqx + sqy + sqz + sqw) 115 | m00 = ( sqx - sqy - sqz + sqw)*invs ; # since sqw + sqx + sqy + sqz =1/invs*invs 116 | m11 = (-sqx + sqy - sqz + sqw)*invs ; 117 | m22 = (-sqx - sqy + sqz + sqw)*invs ; 118 | 119 | tmp1 = x*y; 120 | tmp2 = z*w; 121 | m10 = 2.0 * (tmp1 + tmp2)*invs ; 122 | m01 = 2.0 * (tmp1 - tmp2)*invs ; 123 | 124 | tmp1 = x*z; 125 | tmp2 = y*w; 126 | m20 = 2.0 * (tmp1 - tmp2)*invs ; 127 | m02 = 2.0 * (tmp1 + tmp2)*invs ; 128 | tmp1 = y*z; 129 | tmp2 = x*w; 130 | m21 = 2.0 * (tmp1 + tmp2)*invs ; 131 | m12 = 2.0 * (tmp1 - tmp2)*invs ; 132 | 133 | return np.array([ 134 | [m00, m01, m02], 135 | [m10, m11, m12], 136 | [m20, m21, m22] 137 | ], 'f4') 138 | 139 | def plot_projected_point(self, mlab): 140 | 141 | tmp = self.point3d[self.mask] 142 | tmp1 = self.image_in_world_reference[self.mask] 143 | 144 | for t_indx in range(min(40, tmp.shape[0])): 145 | t_2_im1 = np.vstack((tmp[t_indx], tmp1[t_indx])) 146 | mlab.plot3d(t_2_im1[:, 0], t_2_im1[:, 1], t_2_im1[:, 2], color=(1, 0, 1), line_width=0.01) 147 | 148 | def plot_pos_orientation(self, mlab): 149 | mlab.text3d(self.pos[0], self.pos[1], self.pos[2], self.name, scale = 0.1) 150 | mlab.quiver3d(self.pos[0], self.pos[1], self.pos[2], self.orientation_vector[0], self.orientation_vector[1], self.orientation_vector[2])#, extent=np.concatenate((T, 2*T))) 151 | 152 | def project_3dpoint_onto_2d(self, point3d): 153 | self.point3d = point3d 154 | 155 | self.image_in_camera_reference_with_depth = (np.dot(self.Proj[:3, :3], point3d.T) + self.Proj[:3, 3:4]).T 156 | self.mask = np.ones((self.point3d.shape[0], ), 'bool') 157 | 158 | # self.mask = self.image_in_camera_reference_with_depth[:, 2] > self.focal 159 | # 160 | # if self.mask.sum() < 10: 161 | # print('the camera is too close to the point ...') 162 | # sys.exit(1) 163 | 164 | self.image_in_camera_reference = self.image_in_camera_reference_with_depth / self.image_in_camera_reference_with_depth[:, 2:] 165 | 166 | self.image_in_camera_reference = np.dot(self.camera_matrix, self.image_in_camera_reference.T).T 167 | 168 | # kind of cheating to plot the correct value in world coordinate 169 | tmp = self.image_in_camera_reference + 0 170 | tmp[:, 2] = self.focal 171 | 172 | self.image_in_world_reference = np.dot(self.Proj[:3, :3].T, (tmp - self.Proj[:3, 3]).T).T 173 | 174 | # idx = np.argmax(self.image_in_camera_reference[:, 0]) 175 | # print(self.point3d[idx], '\n', self.Proj, '\n', self.image_in_camera_reference[idx], '\n', self.image_in_camera_reference_with_depth[idx]) 176 | 177 | # def get_RT_in_camera_reference(self, R, T): 178 | # T_cam = np.dot(self.Proj[:3, :3].T, (T - self.Proj[:3, 3]).reshape((3, 1))) 179 | # R_cam = R#np.dot(Proj[:3, :3].T, R.flatten() - Proj[:3, 3]) 180 | # 181 | # T_cam = T_cam.flatten() 182 | # T_cam = T_cam/ np.linalg.norm(T_cam) 183 | # return R_cam, T_cam 184 | 185 | def relative_rotation(self, camera2): 186 | # inverse of quaternion 187 | q2 = camera2.angle2quat(camera2.euler) 188 | q1 = self.angle2quat(self.euler) 189 | q2 = q2 * np.array([1., -1., -1., -1.]) 190 | tmp = [q2[0]*q1[0] - q2[1]*q1[1] - q2[2]*q1[2] - q2[3]*q1[3], \ 191 | q2[0]*q1[1] + q2[1]*q1[0] + q2[2]*q1[3] - q2[3]*q1[2], \ 192 | q2[0]*q1[2] - q2[1]*q1[3] + q2[2]*q1[0] + q2[3]*q1[1], \ 193 | q2[0]*q1[3] + q2[1]*q1[2] - q2[2]*q1[1] + q2[3]*q1[0] ] 194 | 195 | return np.array(tmp) 196 | 197 | def get_relative_T_in_world_reference(self, camera1): 198 | return self.pos - camera1.pos 199 | 200 | def relative_T(self, camera2): 201 | T = np.dot(camera2.Proj[:3, :3], self.pos) + camera2.Proj[:3, 3] 202 | return T / np.linalg.norm(T) 203 | 204 | def print_relative_error(self, est_R, est_T, camera2): 205 | 206 | est_R = np.array(Quaternion(matrix=est_R).elements).flatten() 207 | gt_R = self.relative_rotation(camera2) 208 | diff_R = min(np.abs(np.dot(est_R, gt_R)), 1) 209 | print('Relative R:', 2 * np.arccos(diff_R) * 180 /math.pi ) 210 | 211 | gt_T = self.relative_T(camera2) 212 | # print(gt_T, est_T.flatten()) 213 | diff_T = np.dot(est_T.flatten(), gt_T) 214 | diff_T = min(1, diff_T) 215 | diff_T = max(-1, diff_T) 216 | print('Relative T:', np.arccos(diff_T) * 180 /math.pi ) 217 | 218 | return 2 * np.arccos(diff_R) * 180 /math.pi , np.arccos(diff_T) * 180 /math.pi -------------------------------------------------------------------------------- /toy_example/main_demo.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jun 19, 2018 3 | 4 | @author: en 5 | ''' 6 | 7 | import matplotlib.pyplot as plt 8 | from numpy import pi, sin, cos, mgrid 9 | import numpy as np 10 | from mayavi import mlab 11 | import cv2 12 | from matplotlib.patches import ConnectionPatch 13 | from camera import Camera # @UnresolvedImport 14 | 15 | 16 | def get_3d_function(): 17 | dphi, dtheta = pi/250.0, pi/250.0 18 | [phi,theta] = mgrid[0:pi+dphi*1.5:dphi,0:2*pi+dtheta*1.5:dtheta] 19 | m0 = 4; m1 = 3; m2 = 2; m3 = 3; m4 = 6; m5 = 2; m6 = 6; m7 = 4; 20 | r = sin(m0*phi)**m1 + cos(m2*phi)**m3 + sin(m4*theta)**m5 + cos(m6*theta)**m7 21 | x = r*sin(phi)*cos(theta) 22 | y = r*cos(phi) 23 | z = r*sin(phi)*sin(theta) 24 | 25 | return np.concatenate((x, y, z)).reshape((3, -1)).T, x.shape 26 | 27 | def plot_3d(xyz, r_xyz, shape_, cam1, cam2): 28 | s = mlab.mesh(xyz[:, 0].reshape(shape_), xyz[:, 1].reshape(shape_), xyz[:, 2].reshape(shape_)) 29 | mlab.axes( x_axis_visibility= True, y_axis_visibility=True, z_axis_visibility=True) 30 | 31 | # plot camera position 32 | pos = np.concatenate((cam1.pos, cam2.pos)).reshape((2, 3)) 33 | mlab.plot3d(pos[:, 0], pos[:, 1], pos[:, 2], color=(0, 0, 1), line_width=0.5) 34 | 35 | T = cam2.get_relative_T_in_world_reference(cam1) 36 | mlab.quiver3d(cam1.pos[0], cam1.pos[1], cam1.pos[2], T[0], T[1], T[2])#, extent=np.concatenate((T, 2*T))) 37 | 38 | # plot random point 39 | mlab.points3d(r_xyz[:, 0], r_xyz[:, 1], r_xyz[:, 2], scale_factor = 0.1, color = (1, 1, 1)) 40 | for i in range(r_xyz[:, 0].shape[0]): 41 | mlab.text3d(r_xyz[i, 0], r_xyz[i, 1], r_xyz[i, 2], 'P%d'%i, scale = 0.1) 42 | 43 | 44 | # plot the position of the camera and its directional vector 45 | cam1.plot_pos_orientation(mlab) 46 | cam2.plot_pos_orientation(mlab) 47 | 48 | # plot projected point on each camera filter 49 | cam1.plot_projected_point(mlab) 50 | cam2.plot_projected_point(mlab) 51 | 52 | mlab.show(stop=True) 53 | 54 | def plot_projected_point(im1, im2, title, block_): 55 | # Plot images 56 | fig = plt.figure() 57 | plt.suptitle(title) 58 | ax1 = fig.add_subplot(121) 59 | ax2 = fig.add_subplot(122) 60 | 61 | for i in range(im1.shape[0]): 62 | try: 63 | con = ConnectionPatch(xyA=im2[i, :2], xyB=im1[i, :2], coordsA="data", coordsB="data", 64 | axesA=ax2, axesB=ax1, color="red") 65 | ax2.add_artist(con) 66 | except: 67 | pass 68 | ax1.grid() 69 | ax1.plot(im1[:, 0], im1[:, 1],'ko') 70 | ax2.plot(im2[:, 0], im2[:, 1],'ko') 71 | ax2.grid() 72 | plt.show(block=block_) 73 | 74 | if __name__ == '__main__': 75 | 76 | show_gui = True 77 | nb_points = 100 78 | 79 | error= [] 80 | 81 | for _ in range(100): 82 | 83 | xyz, xyz_shape = get_3d_function() 84 | 85 | cam1 = Camera(center_3d=np.mean(xyz, axis=0), name = 'cam1') 86 | cam2 = Camera(center_3d=np.mean(xyz, axis=0), pos = cam1.pos + np.random.randint(20, 30, 3) /30, name = 'cam2') 87 | 88 | idx = np.random.randint(0, xyz.shape[0], nb_points) 89 | 90 | random_xyz = xyz[idx] 91 | 92 | cam1.project_3dpoint_onto_2d(random_xyz) 93 | cam2.project_3dpoint_onto_2d(random_xyz) 94 | 95 | if show_gui: 96 | plot_projected_point(cam1.image_in_camera_reference, cam2.image_in_camera_reference, 'Projected points', True) 97 | plot_3d(xyz, random_xyz, xyz_shape, cam1, cam2) 98 | 99 | src, dst = cam1.image_in_camera_reference[:, :3], cam2.image_in_camera_reference[:, :3] 100 | 101 | # mask to eliminate point that has the depth < 1 in the camera frame reference 102 | mask1, mask2 = cam1.mask, cam2.mask 103 | pair_mask = mask1 * mask2 104 | 105 | src = src[pair_mask] 106 | dst = dst[pair_mask] 107 | 108 | src = src[:, :2] 109 | dst = dst[:, :2] 110 | camera_mat = np.identity(3, 'f4') 111 | 112 | E, _mask = cv2.findEssentialMat(src, dst, cameraMatrix = np.identity(3, 'f4'), threshold= 0.001) # @UndefinedVariable 113 | _, est_R, est_t, _mask, point3d = cv2.recoverPose(E, src, dst, cameraMatrix = np.identity(3, 'f4'), distanceThresh = 50, mask=_mask) # @UndefinedVariable 114 | 115 | print('---------------') 116 | error.append(cam1.print_relative_error(est_R, est_t, cam2)) 117 | if show_gui: 118 | plot_four_solution(E, ) 119 | 120 | print('Mean:', np.mean(error, axis=0)) 121 | print('Median:', np.median(error, axis=0)) -------------------------------------------------------------------------------- /toy_example/toy_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ensv/RPNet/f4b3fbf510d47801e5b4a1e8b9001a5bb3038a1b/toy_example/toy_example.png -------------------------------------------------------------------------------- /utility/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ensv/RPNet/f4b3fbf510d47801e5b4a1e8b9001a5bb3038a1b/utility/__init__.py -------------------------------------------------------------------------------- /utility/correct_checkpoint.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jun 29, 2018 3 | 4 | @author: en 5 | ''' 6 | import os, sys 7 | 8 | def should_correct_cpt(path): 9 | 10 | txt = open('%s/checkpoint'%path).readlines()[0] 11 | 12 | tmp = txt.split('"') 13 | 14 | if os.path.exists(tmp[1]+'.index'): 15 | 16 | if path == tmp[1].split('/model.ckpt')[0]: 17 | return False 18 | else: 19 | return True 20 | return True 21 | 22 | def correct_path(path): 23 | path = os.path.abspath(path) 24 | txt = open('%s/checkpoint'%path).readlines() 25 | with open('%s/checkpoint'%path, 'w') as f: 26 | for each in txt: 27 | first, cpt_path, end = each.split('"') 28 | fld_path, _ = cpt_path.split('/model.ckpt') 29 | cpt_path = cpt_path.replace(fld_path, path) 30 | f.write('%s"%s"%s'%(first, cpt_path, end)) 31 | 32 | if __name__ == '__main__': 33 | # fd = '/data/chercheurs/en/campose/networks/ShopFacade/RelativePoseNetRT/gridsearch/' 34 | fd = sys.argv[1] #'/data/chercheurs/en/campose/networks/ShopFacade/RelativePoseNetRT/gridsearch/' 35 | 36 | for each_folder in os.listdir(fd): 37 | if len(each_folder) < 3: continue 38 | tmp_path = fd + '/'+ each_folder 39 | for each_train in os.listdir(tmp_path): 40 | if should_correct_cpt('%s/%s/'%(tmp_path, each_train)): 41 | print('corrected checkpoint path at', each_folder, each_train) 42 | correct_path('%s/%s/'%(tmp_path, each_train)) 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /utility/delete_old_checkpoint.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Jul 23, 2018 3 | 4 | @author: en 5 | ''' 6 | import sys 7 | import os 8 | import numpy as np 9 | 10 | if __name__ == '__main__': 11 | path = sys.argv[1] 12 | 13 | for each in os.listdir(path): 14 | for e in os.listdir(path + '/' + each): 15 | try: 16 | int(e) 17 | except: 18 | continue 19 | 20 | try: 21 | cpt = [e for e in open('%s/%s/%s/checkpoint'%(path, each, e), 'r').readlines() if e.startswith('all_model_checkpoint_paths')] 22 | except: 23 | continue 24 | cpt = [e.split('"')[1] for e in cpt] 25 | cpt = np.array([(int(e.split('model.ckpt-')[1]), e) for e in cpt if len(e.split('model.ckpt-')) == 2]) 26 | 27 | index = np.argsort(cpt[:, 0].astype('i4')) 28 | cpt = cpt[index] 29 | 30 | for _, file_path in cpt[:-30]: 31 | try: 32 | os.remove('%s.data-00000-of-00001'%file_path) 33 | os.remove('%s.index'%file_path) 34 | os.remove('%s.meta'%file_path) 35 | except: 36 | pass -------------------------------------------------------------------------------- /utility/printing_function.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Created on Aug 23, 2018 3 | 4 | @author: en 5 | ''' 6 | import os 7 | from sklearn.externals import joblib 8 | import numpy as np 9 | 10 | 11 | def print_results(path): 12 | for each in os.listdir(path): 13 | try: 14 | print(each, joblib.load('%s/%s/0/result_test.pkl'%(path, each))) 15 | except: 16 | print(each) 17 | 18 | def print_results_rpnet(path): 19 | for each in os.listdir(path): 20 | try: 21 | print(each, np.median(joblib.load('%s/%s/0/result_validation.pkl'%(path, each)), axis=1)) 22 | except: 23 | print(each) 24 | 25 | def write_done_to_config(): 26 | for each in os.listdir('.'): 27 | if os.path.exists('%s/0/done.txt'%(each)): continue 28 | with open('%s/0/done.txt'%each, 'w') as f: 29 | f.write('') 30 | 31 | --------------------------------------------------------------------------------