├── .gitignore
├── README.md
├── config_utils.py
├── data
├── cityscapes_loader.py
├── kitti_odometry_loader.py
├── kitti_raw_loader.py
├── odometry_train.txt
├── odometry_val.txt
├── prepare_train_data.py
├── static_frames.txt
└── test_scenes_eigen.txt
├── datasets
├── __init__.py
└── kitti
│ ├── __init__.py
│ ├── kitti_depth_evaluation.py
│ ├── kitti_odometry_dataset.py
│ ├── kitti_odometry_evaluation.py
│ ├── kitti_raw_dataset.py
│ └── kitti_raw_transformed.py
├── download_tensorflow_odem.sh
├── evaluate.py
├── experiments
├── sfm_learner_v1.yml
├── sfm_learner_v1_eval.yml
├── sfm_learner_v1_odom.yml
├── sfm_learner_v1_odom_eval.yml
├── sfm_learner_v1_odom_test.yml
├── sfm_learner_v1_ssim.yml
└── sfm_learner_v1_test.yml
├── imgs
├── odom_10.png
├── output_1.png
├── output_2.png
├── output_3.png
├── result_10.png
└── result_9.png
├── inference.py
├── kitti_eval
├── __init__.py
├── depth_util.py
├── odom_util.py
└── test_files_eigen.txt
├── models
├── __init__.py
├── base_model.py
├── disp_net.py
├── pose_net.py
├── spational_transformer_sampler_interp.py
├── transform.py
└── utils.py
├── requirements.txt
└── train.py
/.gitignore:
--------------------------------------------------------------------------------
1 | **/.idea/**
2 | *.pyc
3 | tmp.py
4 | __pycache__/*
5 | *.c
6 | *.so
7 | *.pyc
8 | *.html
9 | results
10 | build
11 | *_img
12 | *.caffemodel
13 | *.npz*
14 | images/
15 | pose_data
16 | weights
17 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # SfMLearner Chainer version
2 | This codebase implements the system described in the paper:
3 |
4 | Unsupervised Learning of Depth and Ego-Motion from Video [link](https://people.eecs.berkeley.edu/~tinghuiz/projects/SfMLearner/)
5 | See the [project webpage](https://people.eecs.berkeley.edu/~tinghuiz/projects/SfMLearner/) for more details.
6 |
7 | TF code: https://github.com/tinghuiz/SfMLearner
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 | ## Preparing training data
18 | In order to train the model using the provided code, the data needs to be formatted in a certain manner.
19 |
20 | ### Depth
21 | For [KiTTI](http://www.cvlibs.net/datasets/kitti/raw_data.php), first download the dataset using this [script](http://www.cvlibs.net/download.php?file=raw_data_downloader.zip) provided on the official website, and then run the following command
22 | ```bash
23 | python data/prepare_train_data.py /path/to/KITTI_raw --dataset-format kitti_raw --static-frames ./data/static_frames.txt --dump-root /path/to/KITTI_formatted --height 128 --width 416 --num-threads 8
24 | ```
25 |
26 | ### Odometry
27 | This script generates only training data.
28 | Remove '2011_09_26_drive_0067' sequence because there is no data at kitti server.
29 | ```bash
30 | python data/prepare_train_data.py /path/to/KITTI_raw --dataset-format kitti_odom --static-frames ./data/static_frames.txt --dump-root /path/to/KITTI_formatted --height 128 --width 416 --num-threads 8
31 | ```
32 |
33 | ## Training using KiTTI Raw Dataset
34 | Once the data are formatted following the above instructions, you should be able to train the model by running the following command
35 |
36 | ### Depth
37 | ```bash
38 | python3 train.py experiments/sfm_learner_v1.yml
39 | ```
40 |
41 | ### Odometry
42 | ```bash
43 | python3 train.py experiments/sfm_learner_v1_odom.yml
44 | ```
45 |
46 | ## Evaluation using KiTTI Raw Dataset
47 | If you finish training models using above scripts, you should be able to evaluate your trained model.
48 |
49 | ### Depth
50 | You can obtain the single-view depth predictions on the KITTI eigen test split formatted properly for evaluation by running.
51 | You could download pretrained model from [here](https://www.dropbox.com/s/v1t4b1vao9ucqva/depth_exp02smooth01.npz)
52 | ```bash
53 | python evaluate.py experiments/sfm_learner_v1_eval.yml
54 | ```
55 |
56 | ### Odometry
57 | You can obtain the 5-snipped odometry predictions on the KITTI odometry dataset. This scripts use kitti raw dataset directly.
58 | ```bash
59 | python evaluate.py experiments/sfm_learner_v1_odom_eval.yml --mode odom
60 | ```
61 |
62 | | abs_rel | sq_rel | rms | log_rms | a1 | a2 | a3 |
63 | |:--------------:|:---------------:|:---------------:|:---------------:|:---------------:|:---------------:|:---------------:|
64 | | **0.1779** | **1.3594** | **6.2696** | **0.2570** | **0.7390** | **0.9075** | **0.9647** |
65 |
66 | ## Inference using KiTTI Raw Dataset
67 | ### Depth
68 | You could download pretrained model from [here](https://www.dropbox.com/s/v1t4b1vao9ucqva/depth_exp02smooth01.npz)
69 | ```bash
70 | # For kitti formatted dataset
71 | python inference.py experiments/sfm_learner_v1_test.yml
72 | # For a image
73 | python inference.py experiments/sfm_learner_v1_test.yml --img_path /path/to/img --save 1 --width 416 --height 128
74 | ```
75 |
76 | ### odometry
77 | ```bash
78 | # Create predicted trajectory
79 | python inference.py experiments/sfm_learner_v1_odom_test.yml --mode odom
80 | # Visualize trajectories
81 | python inference.py experiments/sfm_learner_v1_odom_test.yml --mode odom --gt_file ./kitti_eval/pose_data/ground_truth/10_full.txt --pred_file ./test.txt
82 | ```
83 |
--------------------------------------------------------------------------------
/config_utils.py:
--------------------------------------------------------------------------------
1 | #!/usr/env/bin python3
2 | # -*- coding: utf-8 -*-
3 |
4 | import argparse
5 | import importlib
6 | import os
7 | import sys
8 | import json
9 | import numpy as np
10 | import os
11 | import subprocess
12 | import shutil
13 | import yaml
14 |
15 | try:
16 | import cupy as cp
17 | except:
18 | cp = None
19 | print("Please install cupy if you want to use gpus")
20 |
21 | from sklearn.model_selection import train_test_split
22 |
23 | import chainer
24 | from chainer import iterators
25 | from chainer.training import extensions
26 |
27 | import chainercv
28 |
29 | from models import base_model
30 | # from extension_util import lr_utils
31 |
32 | from collections import OrderedDict
33 | yaml.add_constructor(yaml.resolver.BaseResolver.DEFAULT_MAPPING_TAG,
34 | lambda loader, node: OrderedDict(loader.construct_pairs(node)))
35 |
36 |
37 | SEED = 0
38 |
39 | def parse_dict(dic, key, value=None):
40 | return value if not key in dic else dic[key]
41 |
42 | def parse_args():
43 | parser = argparse.ArgumentParser()
44 | parser.add_argument('config', default='default.yml', type=str, help='configure file')
45 | parser.add_argument('--img_path', type=str, help='image path')
46 | parser.add_argument('--width', type=int, default='416', help='input width')
47 | parser.add_argument('--height', type=int, default='128', help='input height')
48 | parser.add_argument('--save', type=int, default=-1, help="Save results or not")
49 | parser.add_argument('--mode', default='depth',
50 | choices=["depth", "odom"], help="mode")
51 | parser.add_argument('--gt_file', type=str)
52 | parser.add_argument('--pred_file', type=str)
53 |
54 | args = parser.parse_args()
55 | config = yaml.load(open(args.config))
56 |
57 | SEED = parse_dict(config, "seed", 8964)
58 | np.random.seed(SEED)
59 | if cp:
60 | cp.random.seed(SEED)
61 |
62 | if config["mode"] == "Test":
63 | chainer.global_config.train = False
64 | chainer.global_config.enable_backprop = False
65 | return config, args
66 |
67 | subprocess.check_call(["mkdir", "-p", config["results"]])
68 | shutil.copy(args.config, os.path.join(config['results'], args.config.split('/')[-1]))
69 | return config
70 |
71 | def parse_trigger(trigger):
72 | return (int(trigger[0]), trigger[1])
73 |
74 | def create_extension(trainer, test_iter, model, config, devices=None):
75 | """Create extension for training models"""
76 | for key, ext in config.items():
77 | if key == "Evaluator":
78 | cl = getattr(extensions, ext['name'])
79 | args = parse_dict(ext, 'args', {})
80 | if devices:
81 | args['device'] = devices['main']
82 | trainer.extend(cl(
83 | test_iter, model, **args), trigger=ext['trigger'])
84 | elif key == "dump_graph":
85 | cl = getattr(extensions, key)
86 | trainer.extend(cl(ext['name']))
87 | elif key == 'snapshot':
88 | cl = getattr(extensions, key)
89 | trigger = parse_trigger(ext['trigger'])
90 | trainer.extend(cl(), trigger=trigger)
91 | elif key == 'snapshot_object':
92 | cl = getattr(extensions, key)
93 | trigger = parse_trigger(ext['trigger'])
94 | trainer.extend(cl(model, 'sfm_learner_{.updater.iteration}'),
95 | trigger=trigger)
96 | elif key == 'LogReport':
97 | cl = getattr(extensions, key)
98 | trigger = parse_trigger(ext['trigger'])
99 | trainer.extend(cl(trigger=trigger))
100 | elif key == "PrintReport":
101 | cl = getattr(extensions, key)
102 | report_list = ext['name'].split(' ')
103 | trigger = parse_trigger(ext['trigger'])
104 | trainer.extend(cl(report_list), trigger=trigger)
105 | elif key == "ProgressBar":
106 | cl = getattr(extensions, key)
107 | trainer.extend(cl(update_interval=ext['update_interval']))
108 | elif key == 'observe_lr':
109 | cl = getattr(extensions, key)
110 | trainer.extend(cl())
111 | elif key == "PolynomialShift":
112 | cl = getattr(lr_utils, key)
113 | trigger = parse_trigger(ext['trigger'])
114 | len_dataset = len(trainer.updater.get_iterator('main').dataset)
115 | batchsize = trainer.updater.get_iterator('main').batch_size
116 | args = parse_dict(ext, 'args', {})
117 | args.update({'len_dataset': len_dataset, 'batchsize': batchsize,
118 | 'stop_trigger': trainer.stop_trigger})
119 | trainer.extend(cl(**args))
120 | return trainer
121 |
122 | def create_updater(train_iter, optimizer, config, devices):
123 | if "MultiprocessParallelUpdater" in config['name']:
124 | Updater = chainer.training.updaters.MultiprocessParallelUpdater
125 | # Updater = MyMultiprocessParallelUpdater
126 | return Updater(train_iter, optimizer, devices=devices)
127 |
128 | Updater = getattr(chainer.training, config['name'])
129 | if "Standard" in config['name']:
130 | device = None if devices is None else devices['main']
131 | return Updater(train_iter, optimizer, device=device)
132 | else:
133 | return Updater(train_iter, optimizer, devices=devices)
134 |
135 | def create_optimizer(config, model):
136 | Optimizer = getattr(chainer.optimizers, config['name'])
137 | opt = Optimizer(**config['args'])
138 | opt.setup(model)
139 | if 'hook' in config.keys():
140 | for key, value in config['hook'].items():
141 | hook = getattr(chainer.optimizer, key)
142 | opt.add_hook(hook(value))
143 | return opt
144 |
145 | def create_iterator_test(test_data, config):
146 | Iterator = getattr(chainer.iterators, config['name'])
147 | args = parse_dict(config, 'args', {})
148 | args['repeat'] = False
149 | args['shuffle'] = False
150 | test_iter = Iterator(test_data, config['test_batchsize'], **args)
151 | return test_iter
152 |
153 | def create_iterator(train_data, test_data, config, devices, updater_name):
154 | Iterator = getattr(chainer.iterators, config['name'])
155 | args = parse_dict(config, 'args', {})
156 | if 'MultiprocessParallelUpdater' in updater_name:
157 | train_iter = [
158 | chainer.iterators.MultiprocessIterator(i,
159 | config['train_batchsize'],
160 | **args)
161 | for i in chainer.datasets.split_dataset_n_random(train_data, len(devices))]
162 | else:
163 | train_iter = Iterator(train_data, config['train_batchsize'], **args)
164 |
165 | test_iter = None
166 | if test_data is not None:
167 | args['repeat'] = False
168 | test_iter = Iterator(test_data, config['test_batchsize'], **args)
169 | return train_iter, test_iter
170 |
171 | def parse_devices(gpus, updater_name):
172 | if gpus:
173 | devices = {'main': gpus[0]}
174 | if not 'MultiprocessParallelUpdater' in updater_name:
175 | chainer.cuda.get_device_from_id(gpus[0]).use()
176 | for gid in gpus[1:]:
177 | devices['gpu{}'.format(gid)] = gid
178 | return devices
179 | return None
180 |
181 | def get_class_weight(config):
182 | path = parse_dict(config, 'class_weight', None)
183 | if path:
184 | class_weight = np.load(path)
185 | return class_weight
186 | else:
187 | None
188 |
189 | def get_class(mod):
190 | assert len(mod) > 0, (name, mod)
191 | m = sys.modules[
192 | mod] if mod in sys.modules else importlib.import_module(mod)
193 | return m
194 |
195 | def load_dataset_test(config):
196 | test_config = config['test']
197 | cl = get_class(test_config['module'])
198 | test_loader = getattr(cl, test_config['name'])
199 | test_data = test_loader(**test_config['args'])
200 | return test_data
201 |
202 | def load_dataset(config):
203 | train_config = config['train']
204 | cl = get_class(train_config['module'])
205 | train_loader = getattr(cl, train_config['name'])
206 | train_data = train_loader(**train_config['args'])
207 | test_data = None
208 | if 'valid' in config.keys():
209 | test_config = config['valid']
210 | cl = get_class(test_config['module'])
211 | test_loader = getattr(cl, test_config['name'])
212 | test_data = test_loader(**test_config['args'])
213 | return train_data, test_data
214 |
215 | def get_model(config):
216 | cl = get_class(config['module'])
217 | Model = getattr(cl, config['name'])
218 | pretrained_model = parse_dict(config, 'pretrained_model', None)
219 | return Model(config["architecture"], pretrained_model=pretrained_model)
220 |
--------------------------------------------------------------------------------
/data/cityscapes_loader.py:
--------------------------------------------------------------------------------
1 | from __future__ import division
2 | import json
3 | import numpy as np
4 | import scipy.misc
5 | from path import Path
6 | from tqdm import tqdm
7 |
8 |
9 | class cityscapes_loader(object):
10 | def __init__(self,
11 | dataset_dir,
12 | split='train',
13 | crop_bottom=True, # Get rid of the car logo
14 | img_height=171,
15 | img_width=416):
16 | self.dataset_dir = Path(dataset_dir)
17 | self.split = split
18 | # Crop out the bottom 25% of the image to remove the car logo
19 | self.crop_bottom = crop_bottom
20 | self.img_height = img_height
21 | self.img_width = img_width
22 | self.min_speed = 2
23 | self.scenes = (self.dataset_dir/'leftImg8bit_sequence'/split).dirs()
24 | print('Total scenes collected: {}'.format(len(self.scenes)))
25 |
26 | def collect_scenes(self, city):
27 | img_files = sorted(city.files('*.png'))
28 | scenes = {}
29 | connex_scenes = {}
30 | connex_scene_data_list = []
31 | for f in img_files:
32 | scene_id,frame_id = f.basename().split('_')[1:3]
33 | if scene_id not in scenes.keys():
34 | scenes[scene_id] = []
35 | scenes[scene_id].append(frame_id)
36 |
37 | # divide scenes into connexe sequences
38 | for scene_id in scenes.keys():
39 | previous = None
40 | connex_scenes[scene_id] = []
41 | for id in scenes[scene_id]:
42 | if previous is None or int(id) - int(previous) > 1:
43 | current_list = []
44 | connex_scenes[scene_id].append(current_list)
45 | current_list.append(id)
46 | previous = id
47 |
48 | # create scene data dicts, and subsample scene every two frames
49 | for scene_id in connex_scenes.keys():
50 | intrinsics = self.load_intrinsics(city, scene_id)
51 | for subscene in connex_scenes[scene_id]:
52 | frame_speeds = [self.load_speed(city, scene_id, frame_id) for frame_id in subscene]
53 | connex_scene_data_list.append({'city':city,
54 | 'scene_id': scene_id,
55 | 'rel_path': city.basename()+'_'+scene_id+'_'+subscene[0]+'_0',
56 | 'intrinsics': intrinsics,
57 | 'frame_ids':subscene[0::2],
58 | 'speeds':frame_speeds[0::2]})
59 | connex_scene_data_list.append({'city':city,
60 | 'scene_id': scene_id,
61 | 'rel_path': city.basename()+'_'+scene_id+'_'+subscene[0]+'_1',
62 | 'intrinsics': intrinsics,
63 | 'frame_ids': subscene[1::2],
64 | 'speeds': frame_speeds[1::2]})
65 | return connex_scene_data_list
66 |
67 | def load_intrinsics(self, city, scene_id):
68 | city_name = city.basename()
69 | camera_folder = self.dataset_dir/'camera'/self.split/city_name
70 | camera_file = camera_folder.files('{}_{}_*_camera.json'.format(city_name, scene_id))[0]
71 | frame_id = camera_file.split('_')[2]
72 | frame_path = city/'{}_{}_{}_leftImg8bit.png'.format(city_name, scene_id, frame_id)
73 |
74 | with open(camera_file, 'r') as f:
75 | camera = json.load(f)
76 | fx = camera['intrinsic']['fx']
77 | fy = camera['intrinsic']['fy']
78 | u0 = camera['intrinsic']['u0']
79 | v0 = camera['intrinsic']['v0']
80 | intrinsics = np.array([[fx, 0, u0],
81 | [0, fy, v0],
82 | [0, 0, 1]])
83 |
84 | img = scipy.misc.imread(frame_path)
85 | h,w,_ = img.shape
86 | zoom_y = self.img_height/h
87 | zoom_x = self.img_width/w
88 |
89 | intrinsics[0] *= zoom_x
90 | intrinsics[1] *= zoom_y
91 | return intrinsics
92 |
93 | def load_speed(self, city, scene_id, frame_id):
94 | city_name = city.basename()
95 | vehicle_folder = self.dataset_dir/'vehicle_sequence'/self.split/city_name
96 | vehicle_file = vehicle_folder/'{}_{}_{}_vehicle.json'.format(city_name, scene_id, frame_id)
97 | with open(vehicle_file, 'r') as f:
98 | vehicle = json.load(f)
99 | return vehicle['speed']
100 |
101 | def get_scene_imgs(self, scene_data):
102 | cum_speed = np.zeros(3)
103 | print(scene_data['city'].basename(), scene_data['scene_id'], scene_data['frame_ids'][0])
104 | for i,frame_id in enumerate(scene_data['frame_ids']):
105 | cum_speed += scene_data['speeds'][i]
106 | speed_mag = np.linalg.norm(cum_speed)
107 | if speed_mag > self.min_speed:
108 | yield self.load_image(scene_data['city'], scene_data['scene_id'], frame_id), frame_id
109 | cum_speed *= 0
110 |
111 | def load_image(self, city, scene_id, frame_id):
112 | img_file = city/'{}_{}_{}_leftImg8bit.png'.format(city.basename(),
113 | scene_id,
114 | frame_id)
115 | if not img_file.isfile():
116 | return None
117 | img = scipy.misc.imread(img_file)
118 | img = scipy.misc.imresize(img, (self.img_height, self.img_width))[:int(self.img_height*0.75)]
119 | return img
120 |
--------------------------------------------------------------------------------
/data/kitti_odometry_loader.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | from path import Path
3 | import scipy.misc
4 |
5 |
6 | class KittiOdometryLoader(object):
7 | def __init__(self, dataset_dir, static_frames_file=None,
8 | img_height=128, img_width=416, seq_length=3,
9 | train_list=None, val_list=None):
10 | dir_path = Path(__file__).realpath().dirname()
11 | static_frames_file = Path(static_frames_file)
12 | self.dataset_dir = Path(dataset_dir)
13 | self.img_height = img_height
14 | self.img_width = img_width
15 | self.cam_ids = ['02', '03']
16 | self.data_list = []
17 | self.parse_data_list(train_list, 'train')
18 | # self.parse_data_list(val_list, 'val')
19 | self.scenes = []
20 | self.scenes_info = {}
21 | self.collect_train_folders()
22 | self.collect_static_frames(static_frames_file)
23 |
24 | def parse_data_list(self, data_list, label):
25 | with open(data_list, 'r') as f:
26 | data_list = f.readlines()
27 | data_list = [[label] + d[:-1].split(" ") for d in data_list]
28 | self.data_list += data_list
29 |
30 | def collect_static_frames(self, static_frames_file):
31 | with open(static_frames_file, 'r') as f:
32 | frames = f.readlines()
33 | self.static_frames = {}
34 | for fr in frames:
35 | if fr == '\n':
36 | continue
37 | date, drive, frame_id = fr.split(' ')
38 | curr_fid = '%.10d' % (np.int(frame_id[:-1]))
39 | if drive not in self.static_frames.keys():
40 | self.static_frames[drive] = []
41 | self.static_frames[drive].append(curr_fid)
42 |
43 | def collect_train_folders(self):
44 | for data in self.data_list:
45 | drive_set = self.dataset_dir/data[1]/data[2]
46 | self.scenes.append(drive_set)
47 | self.scenes_info[str(drive_set.name)] = [data[0], data[3], data[4]]
48 |
49 | def collect_scenes(self, drive):
50 | train_scenes = []
51 | for c in self.cam_ids:
52 | oxts = sorted((drive/'oxts'/'data').files('*.txt'))
53 | scene_data = {'cid': c, 'dir': drive, 'speed': [], 'frame_id': [], 'rel_path': drive.name + '_' + c}
54 | for n, f in enumerate(oxts):
55 | metadata = np.genfromtxt(f)
56 | speed = metadata[8:11]
57 | scene_data['speed'].append(speed)
58 | scene_data['frame_id'].append('{:010d}'.format(n))
59 | res = self.load_image(scene_data, 0)
60 | if res is None:
61 | return []
62 | scene_data['intrinsics'] = self.get_intrinsics(scene_data, res[1], res[2])
63 | train_scenes.append(scene_data)
64 | return train_scenes
65 |
66 | def get_scene_imgs(self, scene_data, from_speed=False):
67 | drive = str(scene_data['dir'].name)
68 | scene_info = self.scenes_info[drive]
69 | start = int(scene_info[1])
70 | end = int(scene_info[2])
71 | for (i, frame_id) in enumerate(scene_data['frame_id']):
72 | if (drive not in self.static_frames.keys()) or \
73 | (frame_id not in self.static_frames[drive]):
74 | if int(frame_id) >= start and int(frame_id) <= end:
75 | # print(frame_id, start, end)
76 | yield self.load_image(scene_data, i)[0], frame_id
77 |
78 | def get_intrinsics(self, scene_data, zoom_x, zoom_y):
79 | calib_file = scene_data['dir'].parent/'calib_cam_to_cam.txt'
80 |
81 | filedata = self.read_raw_calib_file(calib_file)
82 | P_rect = np.reshape(filedata['P_rect_' + scene_data['cid']], (3, 4))
83 | intrinsics = P_rect[:, :3]
84 | intrinsics[0] *= zoom_x
85 | intrinsics[1] *= zoom_y
86 | return intrinsics
87 |
88 | def load_image(self, scene_data, tgt_idx):
89 | img_file = scene_data['dir']/'image_{}'.format(scene_data['cid'])/'data'/scene_data['frame_id'][tgt_idx]+'.png'
90 | if not img_file.isfile():
91 | return None
92 | img = scipy.misc.imread(img_file)
93 | zoom_y = self.img_height/img.shape[0]
94 | zoom_x = self.img_width/img.shape[1]
95 | img = scipy.misc.imresize(img, (self.img_height, self.img_width))
96 | return img, zoom_x, zoom_y
97 |
98 | def read_raw_calib_file(self, filepath):
99 | # From https://github.com/utiasSTARS/pykitti/blob/master/pykitti/utils.py
100 | """Read in a calibration file and parse into a dictionary."""
101 | data = {}
102 |
103 | with open(filepath, 'r') as f:
104 | for line in f.readlines():
105 | key, value = line.split(':', 1)
106 | # The only non-float values in these files are dates, which
107 | # we don't care about anyway
108 | try:
109 | data[key] = np.array([float(x) for x in value.split()])
110 | except ValueError:
111 | pass
112 | return data
113 |
--------------------------------------------------------------------------------
/data/kitti_raw_loader.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | from path import Path
3 | import scipy.misc
4 |
5 |
6 | class KittiRawLoader(object):
7 | def __init__(self,
8 | dataset_dir,
9 | static_frames_file=None,
10 | img_height=128,
11 | img_width=416,
12 | seq_length=3):
13 | dir_path = Path(__file__).realpath().dirname()
14 | test_scene_file = dir_path/'test_scenes_eigen.txt'
15 | static_frames_file = Path(static_frames_file)
16 | with open(test_scene_file, 'r') as f:
17 | test_scenes = f.readlines()
18 | self.test_scenes = [t[:-1] for t in test_scenes]
19 | self.dataset_dir = Path(dataset_dir)
20 | self.img_height = img_height
21 | self.img_width = img_width
22 | self.cam_ids = ['02', '03']
23 | self.date_list = ['2011_09_26', '2011_09_28', '2011_09_29',
24 | '2011_09_30', '2011_10_03']
25 | self.collect_train_folders()
26 | self.collect_static_frames(static_frames_file)
27 |
28 | def collect_static_frames(self, static_frames_file):
29 | with open(static_frames_file, 'r') as f:
30 | frames = f.readlines()
31 | self.static_frames = {}
32 | for fr in frames:
33 | if fr == '\n':
34 | continue
35 | date, drive, frame_id = fr.split(' ')
36 | curr_fid = '%.10d' % (np.int(frame_id[:-1]))
37 | if drive not in self.static_frames.keys():
38 | self.static_frames[drive] = []
39 | self.static_frames[drive].append(curr_fid)
40 |
41 | def collect_train_folders(self):
42 | self.scenes = []
43 | for date in self.date_list:
44 | drive_set = (self.dataset_dir/date).dirs()
45 | for dr in drive_set:
46 | if dr.name[:-5] not in self.test_scenes:
47 | self.scenes.append(dr)
48 |
49 | def collect_scenes(self, drive):
50 | train_scenes = []
51 | for c in self.cam_ids:
52 | oxts = sorted((drive/'oxts'/'data').files('*.txt'))
53 | scene_data = {'cid': c, 'dir': drive, 'speed': [], 'frame_id': [], 'rel_path': drive.name + '_' + c}
54 | for n, f in enumerate(oxts):
55 | metadata = np.genfromtxt(f)
56 | speed = metadata[8:11]
57 | scene_data['speed'].append(speed)
58 | scene_data['frame_id'].append('{:010d}'.format(n))
59 | res = self.load_image(scene_data, 0)
60 | if res is None:
61 | return []
62 | scene_data['intrinsics'] = self.get_intrinsics(scene_data, res[1], res[2])
63 | train_scenes.append(scene_data)
64 | return train_scenes
65 |
66 | def get_scene_imgs(self, scene_data):
67 | drive = str(scene_data['dir'].name)
68 | for (i,frame_id) in enumerate(scene_data['frame_id']):
69 | if (drive not in self.static_frames.keys()) or (frame_id not in self.static_frames[drive]):
70 | yield self.load_image(scene_data, i)[0], frame_id
71 |
72 | def get_intrinsics(self, scene_data, zoom_x, zoom_y):
73 | calib_file = scene_data['dir'].parent/'calib_cam_to_cam.txt'
74 |
75 | filedata = self.read_raw_calib_file(calib_file)
76 | P_rect = np.reshape(filedata['P_rect_' + scene_data['cid']], (3, 4))
77 | intrinsics = P_rect[:, :3]
78 | intrinsics[0] *= zoom_x
79 | intrinsics[1] *= zoom_y
80 | return intrinsics
81 |
82 | def load_image(self, scene_data, tgt_idx):
83 | img_file = scene_data['dir']/'image_{}'.format(scene_data['cid'])/'data'/scene_data['frame_id'][tgt_idx]+'.png'
84 | if not img_file.isfile():
85 | return None
86 | img = scipy.misc.imread(img_file)
87 | zoom_y = self.img_height/img.shape[0]
88 | zoom_x = self.img_width/img.shape[1]
89 | img = scipy.misc.imresize(img, (self.img_height, self.img_width))
90 | return img, zoom_x, zoom_y
91 |
92 | def read_raw_calib_file(self, filepath):
93 | # From https://github.com/utiasSTARS/pykitti/blob/master/pykitti/utils.py
94 | """Read in a calibration file and parse into a dictionary."""
95 | data = {}
96 |
97 | with open(filepath, 'r') as f:
98 | for line in f.readlines():
99 | key, value = line.split(':', 1)
100 | # The only non-float values in these files are dates, which
101 | # we don't care about anyway
102 | try:
103 | data[key] = np.array([float(x) for x in value.split()])
104 | except ValueError:
105 | pass
106 | return data
107 |
--------------------------------------------------------------------------------
/data/odometry_train.txt:
--------------------------------------------------------------------------------
1 | 2011_10_03 2011_10_03_drive_0027_sync 000000 004540
2 | 2011_10_03 2011_10_03_drive_0042_sync 000000 001100
3 | 2011_10_03 2011_10_03_drive_0034_sync 000000 004660
4 | 2011_09_30 2011_09_30_drive_0016_sync 000000 000270
5 | 2011_09_30 2011_09_30_drive_0018_sync 000000 002760
6 | 2011_09_30 2011_09_30_drive_0020_sync 000000 001100
7 | 2011_09_30 2011_09_30_drive_0027_sync 000000 001100
8 | 2011_09_30 2011_09_30_drive_0028_sync 001100 005170
9 |
--------------------------------------------------------------------------------
/data/odometry_val.txt:
--------------------------------------------------------------------------------
1 | 9 2011_09_30 2011_09_30_drive_0033_sync 000000 001590
2 | 10 2011_09_30 2011_09_30_drive_0034_sync 000000 001200
3 |
--------------------------------------------------------------------------------
/data/prepare_train_data.py:
--------------------------------------------------------------------------------
1 | from __future__ import division
2 | import argparse
3 | import scipy.misc
4 | import numpy as np
5 | from joblib import Parallel, delayed
6 | from tqdm import tqdm
7 | from path import Path
8 |
9 | parser = argparse.ArgumentParser()
10 | parser.add_argument("dataset_dir", metavar='DIR',
11 | help='path to original dataset')
12 | parser.add_argument("--dataset-format", type=str, required=True,
13 | choices=["kitti_raw", "kitti_odom", "cityscapes"])
14 | parser.add_argument("--static-frames", default=None,
15 | help="list of imgs to discard for being static, if not set will discard them based on speed \
16 | (careful, on KITTI some frames have incorrect speed)")
17 | parser.add_argument("--dump-root", type=str, required=True, help="Where to dump the data")
18 | parser.add_argument("--height", type=int, default=128, help="image height")
19 | parser.add_argument("--width", type=int, default=416, help="image width")
20 | parser.add_argument("--num-threads", type=int, default=4, help="number of threads to use")
21 |
22 | args = parser.parse_args()
23 |
24 |
25 | def dump_example(scene):
26 | scene_list = data_loader.collect_scenes(scene)
27 | for scene_data in scene_list:
28 | dump_dir = args.dump_root/scene_data['rel_path']
29 | dump_dir.makedirs_p()
30 | intrinsics = scene_data['intrinsics']
31 | fx = intrinsics[0, 0]
32 | fy = intrinsics[1, 1]
33 | cx = intrinsics[0, 2]
34 | cy = intrinsics[1, 2]
35 |
36 | dump_cam_file = dump_dir/'cam.txt'
37 | with open(dump_cam_file, 'w') as f:
38 | f.write('%f,0.,%f,0.,%f,%f,0.,0.,1.' % (fx, cx, fy, cy))
39 |
40 | for img, frame_nb in data_loader.get_scene_imgs(scene_data):
41 | dump_img_file = dump_dir/'{}.jpg'.format(frame_nb)
42 | scipy.misc.imsave(dump_img_file, img)
43 |
44 | if len(dump_dir.files('*.jpg')) < 3:
45 | dump_dir.rmtree()
46 |
47 |
48 | def main():
49 | args.dump_root = Path(args.dump_root)
50 | args.dump_root.mkdir_p()
51 |
52 | global data_loader
53 |
54 | if args.dataset_format == 'kitti':
55 | from kitti_raw_loader import KittiRawLoader
56 | data_loader = KittiRawLoader(args.dataset_dir,
57 | static_frames_file=args.static_frames,
58 | img_height=args.height,
59 | img_width=args.width)
60 |
61 | elif args.dataset_format == "kitti_odom":
62 | from kitti_odometry_loader import KittiOdometryLoader
63 | data_loader = KittiOdometryLoader(args.dataset_dir,
64 | static_frames_file=args.static_frames,
65 | img_height=args.height,
66 | img_width=args.width,
67 | seq_length=5,
68 | train_list="./data/odometry_train.txt",
69 | val_list="./data/odometry_val.txt")
70 |
71 | elif args.dataset_format == 'cityscapes':
72 | raise("Not Implemented Error")
73 | else:
74 | raise("Please use assigned argument by dataset_format")
75 |
76 | print('Retrieving frames')
77 | Parallel(n_jobs=args.num_threads)(delayed(dump_example)(scene) for scene in tqdm(data_loader.scenes))
78 | # Split into train/val
79 | print('Generating train val lists')
80 | np.random.seed(8964)
81 | subfolders = args.dump_root.dirs()
82 | with open(args.dump_root / 'train.txt', 'w') as tf:
83 | with open(args.dump_root / 'val.txt', 'w') as vf:
84 | for s in tqdm(subfolders):
85 | if np.random.random() < 0.1 and args.dataset_format != "kitti_odom":
86 | vf.write('{}\n'.format(s.name))
87 | else:
88 | tf.write('{}\n'.format(s.name))
89 |
90 |
91 | if __name__ == '__main__':
92 | main()
93 |
--------------------------------------------------------------------------------
/data/test_scenes_eigen.txt:
--------------------------------------------------------------------------------
1 | 2011_09_26_drive_0117
2 | 2011_09_28_drive_0002
3 | 2011_09_26_drive_0052
4 | 2011_09_30_drive_0016
5 | 2011_09_26_drive_0059
6 | 2011_09_26_drive_0027
7 | 2011_09_26_drive_0020
8 | 2011_09_26_drive_0009
9 | 2011_09_26_drive_0013
10 | 2011_09_26_drive_0101
11 | 2011_09_26_drive_0046
12 | 2011_09_26_drive_0029
13 | 2011_09_26_drive_0064
14 | 2011_09_26_drive_0048
15 | 2011_10_03_drive_0027
16 | 2011_09_26_drive_0002
17 | 2011_09_26_drive_0036
18 | 2011_09_29_drive_0071
19 | 2011_10_03_drive_0047
20 | 2011_09_30_drive_0027
21 | 2011_09_26_drive_0086
22 | 2011_09_26_drive_0084
23 | 2011_09_26_drive_0096
24 | 2011_09_30_drive_0018
25 | 2011_09_26_drive_0106
26 | 2011_09_26_drive_0056
27 | 2011_09_26_drive_0023
28 | 2011_09_26_drive_0093
29 |
--------------------------------------------------------------------------------
/datasets/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pfnet/sfm-learner-chainer/03f09adc776041210747ab633270af5191028400/datasets/__init__.py
--------------------------------------------------------------------------------
/datasets/kitti/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pfnet/sfm-learner-chainer/03f09adc776041210747ab633270af5191028400/datasets/kitti/__init__.py
--------------------------------------------------------------------------------
/datasets/kitti/kitti_depth_evaluation.py:
--------------------------------------------------------------------------------
1 | #/usr/bin/env python3
2 | # -*- coding: utf-8 -*-
3 |
4 | import glob
5 | import numpy as np
6 | import os
7 | import random
8 | from scipy.misc import imread
9 | from collections import Counter
10 | from tqdm import tqdm
11 | import datetime
12 | from chainer import functions as F
13 | from chainer import dataset
14 |
15 | def load_as_float_norm(path):
16 | img = imread(path).astype(np.float32).transpose(2, 0, 1)
17 | return img / (255. * 0.5) - 1
18 |
19 | class KittiDepthEvaluation(dataset.DatasetMixin):
20 |
21 | """Dataset class for a task on `Kitti Depth Evaluation`_.
22 |
23 | Args:
24 | data_dir (string): Path to the dataset directory. The directory should
25 | contain at least three directories, :obj:`training`, `testing`
26 | and `ImageSets`.
27 | """
28 | def __init__(self, data_dir=None, test_files=None, seq_len=3,
29 | height=128, width=416, resize=True,
30 | min_depth=1e-3, max_depth=80):
31 | with open(os.path.join(test_files), 'r') as f:
32 | file_pathes = f.read().split('\n')
33 |
34 | self.base_dir = data_dir
35 | self.file_pathes = file_pathes[:-1] if not file_pathes[-1] else file_pathes
36 | self.seq_len = seq_len
37 | self.demi_len = (self.seq_len - 1) // 2
38 | self.read_scene_data()
39 | self.height = height
40 | self.width = width
41 | self.resize = resize
42 | self.min_depth = float(min_depth)
43 | self.max_depth = float(max_depth)
44 |
45 | def read_scene_data(self):
46 | self.calib_dir_list, self.velo_file_list = [], []
47 | self.imgs_file_list, self.cams = [], []
48 | demi_len = (self.seq_len - 1) // 2
49 | src_iter = [i for i in range(-demi_len, demi_len+1) if i != 0]
50 | for file_path in self.file_pathes:
51 | date, scene, cam_id, _, index = file_path[:-4].split('/')
52 | scene_dir = os.path.join(self.base_dir, date, scene)
53 | img_dir = os.path.join(scene_dir, cam_id, 'data')
54 | tgt_img_path = os.path.join(img_dir, '{}.png'.format(index))
55 | src_imgs_path = [os.path.join(img_dir, '{:010d}.png'.format(int(index) + si)) for si in src_iter]
56 | velo_path = os.path.join(scene_dir, 'velodyne_points/data/{}.bin'.format(index))
57 | if int(index) != 0 and os.path.exists(src_imgs_path[-1]):
58 | self.calib_dir_list.append(os.path.join(self.base_dir, date))
59 | self.velo_file_list.append(velo_path)
60 | self.imgs_file_list.append([tgt_img_path, src_imgs_path])
61 | self.cams.append(int(cam_id[-2:]))
62 |
63 | def __len__(self):
64 | return len(self.imgs_file_list)
65 |
66 | def get_example(self, i):
67 | if i % 100 == 0 and i != 0:
68 | percentage = i * 100 / len(self.imgs_file_list)
69 | print("Progress: {0:d}%".format(int(percentage)))
70 |
71 | calib_dir = self.calib_dir_list[i]
72 | imgs_path = self.imgs_file_list[i]
73 | tgt_img_path = imgs_path[0]
74 | # src_imgs_path = imgs_path[1]
75 | velo_path = self.velo_file_list[i]
76 | tgt_img = load_as_float_norm(tgt_img_path)
77 | # src_imgs = [load_as_float_norm(path) for path in src_imgs_path]
78 | orig_shape = tgt_img.shape[:2]
79 | gt_depth = generate_depth_map(calib_dir, velo_path, tgt_img.shape[1:],
80 | self.cams[i])
81 | tgt_img = F.resize_images(tgt_img[None], (self.height, self.width)).data[0]
82 | # src_imgs = F.resize_images(np.array(src_imgs, dtype='f'), (self.height, self.width)).data
83 | mask = generate_mask(gt_depth, self.min_depth, self.max_depth)
84 | return tgt_img, [], [], gt_depth, mask
85 |
86 | width_to_focal = dict()
87 | width_to_focal[1242] = 721.5377
88 | width_to_focal[1241] = 718.856
89 | width_to_focal[1224] = 707.0493
90 | width_to_focal[1238] = 718.3351
91 |
92 | def load_velodyne_points(file_name):
93 | points = np.fromfile(file_name, dtype='f').reshape(-1, 4)
94 | points[:,3] = 1
95 | return points
96 |
97 | def read_calib_file(path):
98 | float_chars = set("0123456789.e+- ")
99 | data = {}
100 | with open(path, 'r') as f:
101 | for line in f.readlines():
102 | key, value = line.split(':', 1)
103 | value = value.strip()
104 | data[key] = value
105 | if float_chars.issuperset(value):
106 | # try to cast to float array
107 | try:
108 | data[key] = np.array(list(map(float, value.split(' '))))
109 | except ValueError:
110 | # casting error: data[key] already eq. value, so pass
111 | pass
112 | return data
113 |
114 | def get_focal_length_baseline(calib_dir, cam=2):
115 | cam2cam = read_calib_file(calib_dir + 'calib_cam_to_cam.txt')
116 | P2_rect = cam2cam['P_rect_02'].reshape(3,4)
117 | P3_rect = cam2cam['P_rect_03'].reshape(3,4)
118 |
119 | # cam 2 is left of camera 0 -6cm
120 | # cam 3 is to the right +54cm
121 | b2 = P2_rect[0,3] / -P2_rect[0,0]
122 | b3 = P3_rect[0,3] / -P3_rect[0,0]
123 | baseline = b3-b2
124 |
125 | if cam == 2:
126 | focal_length = P2_rect[0,0]
127 | elif cam == 3:
128 | focal_length = P3_rect[0,0]
129 |
130 | return focal_length, baseline
131 |
132 |
133 | def sub2ind(matrixSize, rowSub, colSub):
134 | m, n = matrixSize
135 | return rowSub * (n-1) + colSub - 1
136 |
137 |
138 | def generate_depth_map(calib_dir, velo_file_name, im_shape, cam=2):
139 | # load calibration files
140 | cam2cam = read_calib_file(os.path.join(calib_dir, 'calib_cam_to_cam.txt'))
141 | velo2cam = read_calib_file(os.path.join(calib_dir, 'calib_velo_to_cam.txt'))
142 | velo2cam = np.hstack((velo2cam['R'].reshape(3,3), velo2cam['T'][..., np.newaxis]))
143 | velo2cam = np.vstack((velo2cam, np.array([0, 0, 0, 1.0])))
144 |
145 | # compute projection matrix velodyne->image plane
146 | R_cam2rect = np.eye(4)
147 | R_cam2rect[:3,:3] = cam2cam['R_rect_00'].reshape(3,3)
148 | P_rect = cam2cam['P_rect_0'+str(cam)].reshape(3,4)
149 | P_velo2im = np.dot(np.dot(P_rect, R_cam2rect), velo2cam)
150 |
151 | # load velodyne points and remove all behind image plane (approximation)
152 | # each row of the velodyne data is forward, left, up, reflectance
153 | velo = load_velodyne_points(velo_file_name)
154 | velo = velo[velo[:, 0] >= 0, :]
155 |
156 | # project the points to the camera
157 | velo_pts_im = np.dot(P_velo2im, velo.T).T
158 | velo_pts_im[:, :2] = velo_pts_im[:,:2] / velo_pts_im[:,-1:]
159 |
160 | # check if in bounds
161 | # use minus 1 to get the exact same value as KITTI matlab code
162 | velo_pts_im[:, 0] = np.round(velo_pts_im[:,0]) - 1
163 | velo_pts_im[:, 1] = np.round(velo_pts_im[:,1]) - 1
164 | val_inds = (velo_pts_im[:, 0] >= 0) & (velo_pts_im[:, 1] >= 0)
165 | val_inds = val_inds & (velo_pts_im[:,0] < im_shape[1]) & (velo_pts_im[:,1] < im_shape[0])
166 | velo_pts_im = velo_pts_im[val_inds, :]
167 |
168 | # project to image
169 | depth = np.zeros((im_shape))
170 | depth[velo_pts_im[:, 1].astype(np.int), velo_pts_im[:, 0].astype(np.int)] = velo_pts_im[:, 2]
171 |
172 | # find the duplicate points and choose the closest depth
173 | inds = sub2ind(depth.shape, velo_pts_im[:, 1], velo_pts_im[:, 0])
174 | dupe_inds = [item for item, count in Counter(inds).items() if count > 1]
175 | for dd in dupe_inds:
176 | pts = np.where(inds == dd)[0]
177 | x_loc = int(velo_pts_im[pts[0], 0])
178 | y_loc = int(velo_pts_im[pts[0], 1])
179 | depth[y_loc, x_loc] = velo_pts_im[pts, 2].min()
180 | depth[depth < 0] = 0
181 | return depth
182 |
183 | def generate_mask(gt_depth, min_depth, max_depth):
184 | mask = np.logical_and(gt_depth > min_depth,
185 | gt_depth < max_depth)
186 | # crop used by Garg ECCV16 to reprocude Eigen NIPS14 results
187 | # if used on gt_size 370x1224 produces a crop of [-218, -3, 44, 1180]
188 | gt_height, gt_width = gt_depth.shape
189 | crop = np.array([0.40810811 * gt_height, 0.99189189 * gt_height,
190 | 0.03594771 * gt_width, 0.96405229 * gt_width]).astype(np.int32)
191 |
192 | crop_mask = np.zeros(mask.shape)
193 | crop_mask[crop[0]:crop[1],crop[2]:crop[3]] = 1
194 | mask = np.logical_and(mask, crop_mask)
195 | return mask
196 |
--------------------------------------------------------------------------------
/datasets/kitti/kitti_odometry_dataset.py:
--------------------------------------------------------------------------------
1 | #/usr/bin/env python3
2 | # -*- coding: utf-8 -*-
3 |
4 | import glob
5 | import numpy as np
6 | import os
7 | import random
8 | from scipy.misc import imread
9 |
10 | from chainer import dataset
11 |
12 | def load_as_float_norm(path):
13 | img = imread(path).astype(np.float32).transpose(2, 0, 1)
14 | return img / (255. * 0.5) - 1
15 |
16 |
17 |
18 | class KittiOdometryDataset(dataset.DatasetMixin):
19 |
20 | """Dataset class for a task on `Kitti Raw Dataset`_.
21 |
22 | Args:
23 | data_dir (string): Path to the dataset directory. The directory should
24 | contain at least three directories, :obj:`training`, `testing`
25 | and `ImageSets`.
26 | split ({'train', 'val'}): Select from dataset splits used in
27 | KiTTi Raw Dataset.
28 | """
29 | def __init__(self, data_dir=None, seq_len=3, split='train'):
30 | with open(os.path.join(data_dir, "{}.txt".format(split)), 'r') as f:
31 | dir_indexes = f.read().split('\n')
32 |
33 | if not dir_indexes[-1]:
34 | dir_indexes = dir_indexes[:-1]
35 |
36 | self.dir_pathes = [os.path.join(data_dir, index) for index in dir_indexes]
37 | self.seq_len = seq_len
38 | self.samples = self.crawl_folders()
39 | print('{} num sample'.format(split), len(self.samples))
40 |
41 | def crawl_folders(self):
42 | sequence_set = []
43 | demi_len = (self.seq_len - 1)//2
44 | for dir_path in self.dir_pathes:
45 | calib_path = os.path.join(dir_path, 'cam.txt')
46 | intrinsics = np.genfromtxt(calib_path, delimiter=',')
47 | intrinsics = intrinsics.astype(np.float32).reshape((3, 3))
48 | imgs = glob.glob(os.path.join(dir_path, '*.jpg'))
49 | imgs.sort()
50 | if len(imgs) < self.seq_len:
51 | continue
52 | for i in range(demi_len, len(imgs)-demi_len):
53 | sample = {'intrinsics': intrinsics, 'tgt': imgs[i],
54 | 'ref_imgs': []}
55 | for j in range(-demi_len, demi_len + 1):
56 | if j != 0:
57 | sample['ref_imgs'].append(imgs[i+j])
58 | sequence_set.append(sample)
59 | random.shuffle(sequence_set)
60 | return sequence_set
61 |
62 | def __len__(self):
63 | return len(self.samples)
64 |
65 | def save_img(self, tgt_img, ref_imgs):
66 | import cv2
67 | cv2.imwrite('tgt.png', tgt_img.transpose(1, 2, 0).astype('i'))
68 | cv2.imwrite('src1.png', ref_imgs[0].transpose(1, 2, 0).astype('i'))
69 | cv2.imwrite('src2.png', ref_imgs[1].transpose(1, 2, 0).astype('i'))
70 |
71 | def get_example(self, i):
72 | sample = self.samples[i]
73 | tgt_img = load_as_float_norm(sample['tgt'])
74 | ref_imgs = [load_as_float_norm(ref_img) for ref_img in sample['ref_imgs']]
75 | intrinsics = np.copy(sample['intrinsics'])
76 | return tgt_img, ref_imgs, intrinsics, np.linalg.inv(intrinsics)
77 |
--------------------------------------------------------------------------------
/datasets/kitti/kitti_odometry_evaluation.py:
--------------------------------------------------------------------------------
1 | #/usr/bin/env python3
2 | # -*- coding: utf-8 -*-
3 |
4 | import glob
5 | import numpy as np
6 | import os
7 | import random
8 | from scipy.misc import imread
9 | from collections import Counter
10 | from tqdm import tqdm
11 | import datetime
12 | from chainer import functions as F
13 | from chainer import dataset
14 | import cv2
15 |
16 | def load_as_float_norm(path):
17 | img = imread(path).astype(np.float32).transpose(2, 0, 1)
18 | return img / (255. * 0.5) - 1
19 |
20 | class KittiOdometryEvaluation(dataset.DatasetMixin):
21 |
22 | """Dataset class for a task on `Kitti Depth Evaluation`_.
23 |
24 | Args:
25 | data_dir (string): Path to the dataset directory. The directory should
26 | contain at least three directories, :obj:`training`, `testing`
27 | and `ImageSets`.
28 | """
29 | def __init__(self, data_dir=None, test_files=None, gt_dir=None,
30 | seq_len=3, height=128, width=416, seq_list=None):
31 | self.base_dir = data_dir
32 | self.seq_len = seq_len
33 | self.demi_len = (self.seq_len - 1) // 2
34 | self.height = height
35 | self.width = width
36 | self.seq_list = ['9', '10'] if seq_list is None else [str(seq_list)]
37 | self.data_list = []
38 | self.parse_data_list(test_files)
39 | self.imgs_file_list = []
40 | self.calib_dir_list = []
41 | self.gt_pose_list = []
42 | for data_list in self.data_list:
43 | self.read_scene_data(data_list)
44 |
45 | self.parse_gt_dirs(gt_dir)
46 |
47 | def parse_data_list(self, data_file):
48 | with open(data_file, 'r') as f:
49 | data_list = f.readlines()
50 | for d in data_list:
51 | if d.split(' ')[0] in self.seq_list:
52 | self.data_list += [d[:-1].split(' ')]
53 |
54 | def parse_gt_dirs(self, gt_dir):
55 | self.gt_files = glob.glob(os.path.join(gt_dir, "*.txt"))
56 | self.gt_files.sort()
57 | if not len(self.gt_files):
58 | print("There is not groudtruth data under {}".format(gt_dir))
59 |
60 | def read_scene_data(self, data_list):
61 | seq_id, date, drive, start, end = data_list
62 | data_dir = os.path.join(self.base_dir, date, drive)
63 | oxts_dir = os.path.join(data_dir, 'oxts')
64 | image_dir = os.path.join(data_dir, 'image_02/data')
65 | image_list = glob.glob(os.path.join(image_dir, '*.png'))
66 | image_list.sort()
67 | image_list = image_list[int(start):int(end) + 1]
68 | num_list = len(image_list)
69 | demi_len = (self.seq_len - 1) // 2
70 | src_iter = [i for i in range(-demi_len, demi_len+1) if i != 0]
71 | num_output = num_list - (self.seq_len - 1)
72 |
73 | for i in range(demi_len, num_list - demi_len):
74 | tgt_img_path = image_list[i]
75 | src_imgs_path = [image_list[i + si] for si in src_iter]
76 | self.calib_dir_list.append(os.path.join(self.base_dir, date))
77 | self.imgs_file_list.append([tgt_img_path, src_imgs_path])
78 |
79 | def __len__(self):
80 | return len(self.imgs_file_list)
81 |
82 | def get_example(self, i):
83 | if i % 100 == 0 and i != 0:
84 | percentage = i * 100 / len(self.imgs_file_list)
85 | print("Progress: {0:d}%".format(int(percentage)))
86 | calib_dir = self.calib_dir_list[i]
87 | imgs_path = self.imgs_file_list[i]
88 | tgt_img_path = imgs_path[0]
89 | src_imgs_path = imgs_path[1]
90 | tgt_img = load_as_float_norm(tgt_img_path)
91 | src_imgs = [load_as_float_norm(path) for path in src_imgs_path]
92 | gt_pose = read_file_list(self.gt_files[i])
93 | orig_shape = tgt_img.shape[:2]
94 | tgt_img = F.resize_images(tgt_img[None], (self.height, self.width)).data[0]
95 | src_imgs = F.resize_images(np.array(src_imgs, dtype='f'), (self.height, self.width)).data
96 | return tgt_img, src_imgs, [], gt_pose
97 |
98 |
99 | def read_file_list(filename):
100 | """
101 | Reads a trajectory from a text file.
102 |
103 | File format:
104 | The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched)
105 | and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp.
106 |
107 | Input:
108 | filename -- File name
109 |
110 | Output:
111 | dict -- dictionary of (stamp,data) tuples
112 |
113 | """
114 | with open(filename, 'r') as f:
115 | data = f.read()
116 | lines = data.replace(","," ").replace("\t"," ").split("\n")
117 | data_list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
118 | return np.array([l for l in data_list if len(l)>1], dtype='f')
119 |
--------------------------------------------------------------------------------
/datasets/kitti/kitti_raw_dataset.py:
--------------------------------------------------------------------------------
1 | #/usr/bin/env python3
2 | # -*- coding: utf-8 -*-
3 |
4 | import glob
5 | import numpy as np
6 | import os
7 | import random
8 | from scipy.misc import imread
9 |
10 | from chainer import dataset
11 |
12 | def load_as_float_norm(path):
13 | img = imread(path).astype(np.float32).transpose(2, 0, 1)
14 | return img / (255. * 0.5) - 1
15 |
16 | class KittiRawDataset(dataset.DatasetMixin):
17 |
18 | """Dataset class for a task on `Kitti Raw Dataset`_.
19 |
20 | Args:
21 | data_dir (string): Path to the dataset directory. The directory should
22 | contain at least three directories, :obj:`training`, `testing`
23 | and `ImageSets`.
24 | split ({'train', 'val'}): Select from dataset splits used in
25 | KiTTi Raw Dataset.
26 | """
27 | def __init__(self, data_dir=None, seq_len=3, split='train'):
28 | with open(os.path.join(data_dir, "{}.txt".format(split)), 'r') as f:
29 | dir_indexes = f.read().split('\n')
30 |
31 | if not dir_indexes[-1]:
32 | dir_indexes = dir_indexes[:-1]
33 |
34 | self.dir_pathes = [os.path.join(data_dir, index) for index in dir_indexes]
35 | self.seq_len = seq_len
36 | self.samples = self.crawl_folders()
37 | print('{} num sample'.format(split), len(self.samples))
38 |
39 | def crawl_folders(self):
40 | sequence_set = []
41 | demi_len = (self.seq_len - 1)//2
42 | for dir_path in self.dir_pathes:
43 | calib_path = os.path.join(dir_path, 'cam.txt')
44 | intrinsics = np.genfromtxt(calib_path, delimiter=',')
45 | intrinsics = intrinsics.astype(np.float32).reshape((3, 3))
46 | imgs = glob.glob(os.path.join(dir_path, '*.jpg'))
47 | imgs.sort()
48 | if len(imgs) < self.seq_len:
49 | continue
50 | for i in range(demi_len, len(imgs)-demi_len):
51 | sample = {'intrinsics': intrinsics, 'tgt': imgs[i],
52 | 'ref_imgs': []}
53 | for j in range(-demi_len, demi_len + 1):
54 | if j != 0:
55 | sample['ref_imgs'].append(imgs[i+j])
56 | sequence_set.append(sample)
57 | random.shuffle(sequence_set)
58 | return sequence_set
59 |
60 | def __len__(self):
61 | return len(self.samples)
62 |
63 | def save_img(self, tgt_img, ref_imgs):
64 | import cv2
65 | cv2.imwrite('tgt.png', tgt_img.transpose(1, 2, 0).astype('i'))
66 | cv2.imwrite('src1.png', ref_imgs[0].transpose(1, 2, 0).astype('i'))
67 | cv2.imwrite('src2.png', ref_imgs[1].transpose(1, 2, 0).astype('i'))
68 |
69 | def get_example(self, i):
70 | sample = self.samples[i]
71 | tgt_img = load_as_float_norm(sample['tgt'])
72 | ref_imgs = [load_as_float_norm(ref_img) for ref_img in sample['ref_imgs']]
73 | intrinsics = np.copy(sample['intrinsics'])
74 | return tgt_img, ref_imgs, intrinsics, np.linalg.inv(intrinsics)
75 |
--------------------------------------------------------------------------------
/datasets/kitti/kitti_raw_transformed.py:
--------------------------------------------------------------------------------
1 | #/usr/bin/env python3
2 | # -*- coding: utf-8 -*-
3 |
4 | from functools import partial
5 | import time
6 |
7 | import cv2 as cv
8 | import numpy as np
9 | from PIL import Image
10 | import copy
11 |
12 | from chainer import datasets
13 | from datasets.kitti.kitti_raw_dataset import KittiRawDataset
14 | from chainer import functions as F
15 |
16 |
17 | def make_intrinsics_matrix(fx, fy, cx, cy):
18 | intrinsics = np.array([[fx, 0., cx],
19 | [0., fy, cy],
20 | [0., 0., 1.]], dtype='f')
21 | return intrinsics
22 |
23 | def data_augmentation(tgt_img, src_imgs, intrinsics):
24 | """Data augmentation for training models.
25 |
26 | Args:
27 | tgt_img(ndarray): Shape is (3, H, W)
28 | src_img(list): Shape is [S, 3, H, W]
29 | intrinsics(ndarray): Shape is (3, 3)
30 | """
31 | # Random scaling
32 | def random_scaling(imgs, intrinsics):
33 | batch_size, _, in_h, in_w = imgs.shape
34 | scaling = np.random.uniform(1, 1.15, 2)
35 | x_scaling = scaling[0]
36 | y_scaling = scaling[1]
37 | out_h = int(in_h * y_scaling)
38 | out_w = int(in_w * x_scaling)
39 | imgs = F.resize_images(imgs, [out_h, out_w]).data
40 | fx = intrinsics[0,0] * x_scaling
41 | fy = intrinsics[1,1] * y_scaling
42 | cx = intrinsics[0,2] * x_scaling
43 | cy = intrinsics[1,2] * y_scaling
44 | intrinsics = make_intrinsics_matrix(fx, fy, cx, cy)
45 | return imgs, intrinsics
46 |
47 | # Random cropping
48 | def random_cropping(imgs, intrinsics, out_h, out_w):
49 | batch_size, _, in_h, in_w = imgs.shape
50 | offset_y = int(np.random.randint(0, in_h - out_h + 1))
51 | offset_x = int(np.random.randint(0, in_w - out_w + 1))
52 | imgs = imgs[:, :, offset_y:offset_y+out_h, offset_x:offset_x+out_w]
53 | fx = intrinsics[0,0]
54 | fy = intrinsics[1,1]
55 | cx = intrinsics[0,2] - offset_x
56 | cy = intrinsics[1,2] - offset_y
57 | intrinsics = make_intrinsics_matrix(fx, fy, cx, cy)
58 | return imgs, intrinsics
59 |
60 | # Random flip
61 | def random_flip(imgs, intrinsics):
62 | batch_size, _, in_h, in_w = imgs.shape
63 | if np.random.rand() < 0.5:
64 | imgs = imgs[:, :, :, ::-1]
65 | intrinsics[0, 2] = in_w - intrinsics[0, 2]
66 | return imgs, intrinsics
67 |
68 | _, out_h, out_w = tgt_img.shape
69 | imgs = np.concatenate((tgt_img[np.newaxis, :], src_imgs))
70 | imgs, intrinsics = random_scaling(imgs, intrinsics)
71 | imgs, intrinsics = random_cropping(imgs, intrinsics, out_h, out_w)
72 | imgs, intrinsics = random_flip(imgs, intrinsics)
73 | # im = tf.cast(im, dtype=tf.uint8)
74 | return imgs[0], imgs[1:], intrinsics
75 |
76 | def get_multi_scale_intrinsics(intrinsics, n_scales):
77 | """Scale the intrinsics accordingly for each scale
78 | Args:
79 | intrinsics: Intrinsics for original image. Shape is (3, 3).
80 | n_scales(int): Number of scale.
81 | Returns:
82 | multi_intrinsics: Multi scale intrinsics.
83 | """
84 | multi_intrinsics = []
85 |
86 | for s in range(n_scales):
87 | fx = intrinsics[0, 0]/(2 ** s)
88 | fy = intrinsics[1, 1]/(2 ** s)
89 | cx = intrinsics[0, 2]/(2 ** s)
90 | cy = intrinsics[1, 2]/(2 ** s)
91 | down_intrinsics = make_intrinsics_matrix(fx, fy, cx, cy)
92 | multi_intrinsics.append(down_intrinsics)
93 | return multi_intrinsics
94 |
95 | def _transform(inputs, n_scale=4, ):
96 | tgt_img, src_imgs, intrinsics, inv_intrinsics = inputs
97 | del inputs
98 |
99 | tgt_img, src_imgs, intrinsics = data_augmentation(tgt_img, src_imgs,
100 | intrinsics)
101 | intrinsics = get_multi_scale_intrinsics(intrinsics, n_scale)
102 | return tgt_img, src_imgs, intrinsics, intrinsics
103 |
104 |
105 | class KittiRawTransformed(datasets.TransformDataset):
106 | def __init__(self, data_dir=None, seq_len=3, split='train',
107 | n_scale=4, ):
108 | self.d = KittiRawDataset(
109 | data_dir=data_dir, seq_len=seq_len, split=split)
110 | t = partial(
111 | _transform, n_scale=4, )
112 | super().__init__(self.d, t)
113 |
--------------------------------------------------------------------------------
/download_tensorflow_odem.sh:
--------------------------------------------------------------------------------
1 | URL=http://people.eecs.berkeley.edu/~tinghuiz/projects/SfMLearner/pose_eval_data.tar
2 | TAR_FILE=./kitti_eval/pose_eval_data.tar
3 | wget -N $URL -O $TAR_FILE
4 | tar -xvf $TAR_FILE -C ./kitti_eval/
5 | rm $TAR_FILE
6 |
--------------------------------------------------------------------------------
/evaluate.py:
--------------------------------------------------------------------------------
1 | #!/usr/env/bin python3
2 | # -*- coding: utf-8 -*-
3 |
4 | import argparse
5 | import numpy as np
6 | import sys
7 | import subprocess
8 | import os
9 | import yaml
10 |
11 | import chainer
12 | from chainer import cuda, optimizers, serializers
13 | from chainer import training
14 | from chainer import functions as F
15 |
16 | import cv2
17 | from config_utils import *
18 | import matplotlib.pyplot as plt
19 |
20 | chainer.cuda.set_max_workspace_size(1024 * 1024 * 1024)
21 | os.environ["CHAINER_TYPE_CHECK"] = "0"
22 |
23 | from collections import OrderedDict
24 | yaml.add_constructor(yaml.resolver.BaseResolver.DEFAULT_MAPPING_TAG,
25 | lambda loader, node: OrderedDict(loader.construct_pairs(node)))
26 |
27 | from kitti_eval.depth_util import print_depth_stats, compute_depth_errors
28 | from kitti_eval.odom_util import print_odom_stats, compute_odom_errors
29 | from kitti_eval.odom_util import convert_eval_format
30 |
31 | def evaluate_odom(config, args):
32 | """Evaluate odometry prediction of sfm_learner by online"""
33 | test_data = load_dataset_test(config["dataset"])
34 | test_iter = create_iterator_test(test_data,
35 | config['iterator'])
36 | model = get_model(config["model"])
37 | devices = parse_devices(config['gpus'], config['updater']['name'])
38 | gpu_id = None if devices is None else devices['main']
39 | if devices:
40 | chainer.cuda.get_device_from_id(gpu_id).use()
41 | model.to_gpu(gpu_id)
42 |
43 | batchsize = config['iterator']['test_batchsize']
44 |
45 | index = 0
46 | num_data = len(test_iter.dataset)
47 | odom_error_all = []
48 | print("Start odometry evaluation")
49 | for batch in test_iter:
50 | batch = chainer.dataset.concat_examples(batch, gpu_id)
51 | tgt_img, ref_imgs, _, gt_pose = batch
52 | _, pred_pose, _ = model.inference(tgt_img, ref_imgs,
53 | None, None, is_depth=False,
54 | is_pose=True, is_exp=False)
55 | pred_pose = chainer.cuda.to_cpu(F.concat(pred_pose, axis=0).data)
56 | pred_pose = np.insert(pred_pose, 2, np.zeros((1, 6)), axis=0)
57 | gt_pose = chainer.cuda.to_cpu(gt_pose[0])
58 | pred_pose = convert_eval_format(pred_pose, gt_pose)
59 | odom_error = compute_odom_errors(pred_pose, gt_pose)
60 | if odom_error == False:
61 | continue
62 | odom_error_all.append(odom_error)
63 | odom_error_all = np.array(odom_error_all)
64 | print_odom_stats(odom_error_all)
65 |
66 | def evaluate_depth(config, args):
67 | """Evaluate depth prediction of sfm_learner by online."""
68 | model = get_model(config["model"])
69 | devices = parse_devices(config['gpus'], config['updater']['name'])
70 | test_data = load_dataset_test(config["dataset"])
71 | test_iter = create_iterator_test(test_data,
72 | config['iterator'])
73 |
74 | gpu_id = None if devices is None else devices['main']
75 | if devices:
76 | chainer.cuda.get_device_from_id(gpu_id).use()
77 | model.to_gpu(gpu_id)
78 |
79 | min_depth = test_data.min_depth
80 | max_depth = test_data.max_depth
81 | batchsize = config['iterator']['test_batchsize']
82 |
83 | index = 0
84 | num_data = len(test_iter.dataset)
85 | sum_errors = np.array([0. for i in range(7)], dtype='f')
86 | print("Start depth evaluation")
87 | for batch in test_iter:
88 | batch = chainer.dataset.concat_examples(batch, gpu_id)
89 | tgt_img, ref_imgs, _, gt_depth, mask = batch
90 | pred_depth, _, _ = model.inference(tgt_img, ref_imgs,
91 | None, None,
92 | is_depth=True, is_pose=False)
93 | batchsize = pred_depth.shape[0]
94 | pred_depth = F.resize_images(pred_depth, gt_depth.shape[1:]).data
95 | pred_depth = F.clip(pred_depth, min_depth, max_depth).data[:, 0]
96 | pred_depth = chainer.cuda.to_cpu(pred_depth)
97 | mask = chainer.cuda.to_cpu(mask)
98 | gt_depth = chainer.cuda.to_cpu(gt_depth)
99 | pred_depth = pred_depth[mask]
100 | gt_depth = gt_depth[mask]
101 | scale_factor = np.median(gt_depth) / np.median(pred_depth)
102 | pred_depth *= scale_factor
103 | sum_errors += compute_depth_errors(gt_depth, pred_depth) / num_data
104 | print_depth_stats(sum_errors)
105 |
106 | def main():
107 | config, args = parse_args()
108 | if args.mode == "depth":
109 | evaluate_depth(config, args)
110 | elif args.mode == "odom":
111 | evaluate_odom(config, args)
112 |
113 | if __name__ == '__main__':
114 | main()
115 |
--------------------------------------------------------------------------------
/experiments/sfm_learner_v1.yml:
--------------------------------------------------------------------------------
1 | end_trigger: [200000, "iteration"]
2 | results: results/depth
3 | gpus: [0]
4 | mode: Train
5 | seed: 1
6 |
7 | model:
8 | module: models.base_model
9 | name: SFMLearner
10 | pretrained_model:
11 | path: #
12 | download: # https://0000_model.npz
13 | architecture:
14 | smooth_reg: 0.
15 | exp_reg: 0.
16 | seq_len: 3
17 |
18 | dataset:
19 | train:
20 | module: datasets.kitti.kitti_raw_transformed
21 | name: KittiRawTransformed
22 | args:
23 | data_dir: ../dataset/KITTI_formatted
24 | # data_dir: /mnt/sakuradata10-striped/datasets/KITTI_formatted
25 | split: train
26 | seq_len: 3
27 | n_scale: 4
28 |
29 | # valid:
30 | # module: datasets.kitti.kitti_raw_dataset
31 | # name: KittiRawDataset
32 | # args:
33 | # data_dir: ../dataset/KITTI_formatted
34 | # # data_dir: /mnt/sakuradata10-striped/datasets/KITTI_formatted
35 | # split: val
36 | # seq_len: 3
37 |
38 | updater:
39 | name: StandardUpdater
40 |
41 | iterator:
42 | name: MultiprocessIterator
43 | train_batchsize: 4
44 | test_batchsize: 4
45 | args:
46 | n_processes: 4
47 | n_prefetch: 2
48 | shared_mem: 60000000
49 |
50 | optimizer:
51 | name: Adam
52 | args:
53 | alpha: 0.0002
54 | beta1: 0.9
55 | beta2: 0.999
56 | hook:
57 | WeightDecay: 0.0002
58 |
59 | extension:
60 | #Evaluator:
61 | # name: Evaluator
62 | # trigger: [1, 'epoch']
63 | snapshot:
64 | trigger: [200000, "iteration"]
65 | snapshot_object:
66 | trigger: [5000, "iteration"]
67 | LogReport:
68 | trigger: [100, "iteration"]
69 | PrintReport:
70 | name:
71 | epoch
72 | iteration
73 | main/total_loss
74 | main/pixel_loss
75 | main/smooth_loss
76 | main/exp_loss
77 | trigger: [100, "iteration"]
78 | ProgressBar:
79 | update_interval: 10
80 | observe_lr:
81 |
--------------------------------------------------------------------------------
/experiments/sfm_learner_v1_eval.yml:
--------------------------------------------------------------------------------
1 | gpus: []
2 | mode: Test
3 | seed: 1
4 | min_depth: 0.001
5 | max_depth: 80
6 |
7 | model:
8 | module: models.base_model
9 | name: SFMLearner
10 | pretrained_model:
11 | path: # model.npz
12 | download: # https://0000_model.npz
13 | architecture:
14 | smooth_reg: 0.
15 | exp_reg: 0.
16 | seq_len: 3
17 |
18 | dataset:
19 | test:
20 | module: datasets.kitti.kitti_depth_evaluation
21 | name: KittiDepthEvaluation
22 | args:
23 | data_dir: ../dataset/kitti_raw
24 | test_files: ./kitti_eval/test_files_eigen.txt
25 | # data_dir: /home/user/.chainer/dataset/pfnet/chainer/raw
26 | seq_len: 3
27 | min_depth: 0.001
28 | max_depth: 80
29 |
30 | updater:
31 | name: StandardUpdater
32 |
33 | iterator:
34 | name: SerialIterator
35 | test_batchsize: 1
36 | # args:
37 | # n_processes: 4
38 | # n_prefetch: 2
39 | # shared_mem: 60000000
40 |
--------------------------------------------------------------------------------
/experiments/sfm_learner_v1_odom.yml:
--------------------------------------------------------------------------------
1 | end_trigger: [200000, "iteration"]
2 | results: results/odom
3 | gpus: [0]
4 | mode: Train
5 | seed: 1
6 |
7 | model:
8 | module: models.base_model
9 | name: SFMLearner
10 | pretrained_model:
11 | path: #
12 | download: # https://0000_model.npz
13 | architecture:
14 | smooth_reg: 0.1
15 | exp_reg: 0.2
16 | seq_len: 5
17 |
18 | dataset:
19 | train:
20 | module: datasets.kitti.kitti_raw_transformed
21 | name: KittiRawTransformed
22 | args:
23 | data_dir: ../dataset/KITTI_odom
24 | # data_dir: /mnt/sakuradata10-striped/datasets/KITTI_formatted
25 | split: train
26 | seq_len: 5
27 | n_scale: 4
28 |
29 | # valid:
30 | # module: datasets.kitti.kitti_raw_dataset
31 | # name: KittiRawDataset
32 | # args:
33 | # data_dir: ../dataset/KITTI_odom
34 | # # data_dir: /mnt/sakuradata10-striped/datasets/KITTI_formatted
35 | # split: train
36 | # seq_len: 5
37 |
38 | updater:
39 | name: StandardUpdater
40 |
41 | iterator:
42 | name: MultiprocessIterator
43 | train_batchsize: 4
44 | test_batchsize: 4
45 | args:
46 | n_processes: 4
47 | n_prefetch: 2
48 | shared_mem: 60000000
49 |
50 | optimizer:
51 | name: Adam
52 | args:
53 | alpha: 0.0002
54 | beta1: 0.9
55 | beta2: 0.999
56 | hook:
57 | WeightDecay: 0.0002
58 |
59 | extension:
60 | #Evaluator:
61 | # name: Evaluator
62 | # trigger: [1, 'epoch']
63 | snapshot:
64 | trigger: [200000, "iteration"]
65 | snapshot_object:
66 | trigger: [5000, "iteration"]
67 | LogReport:
68 | trigger: [100, "iteration"]
69 | PrintReport:
70 | name:
71 | epoch
72 | iteration
73 | main/total_loss
74 | main/pixel_loss
75 | main/smooth_loss
76 | main/exp_loss
77 | trigger: [100, "iteration"]
78 | ProgressBar:
79 | update_interval: 10
80 | observe_lr:
81 |
--------------------------------------------------------------------------------
/experiments/sfm_learner_v1_odom_eval.yml:
--------------------------------------------------------------------------------
1 | gpus: [1]
2 | mode: Test
3 | seed: 1
4 |
5 | model:
6 | module: models.base_model
7 | name: SFMLearner
8 | pretrained_model:
9 | path: model.npz # ./weights/exp02smooth01/model.npz
10 | download: # https://0000_model.npz
11 | architecture:
12 | smooth_reg: 0.
13 | exp_reg: 0.
14 | seq_len: 5
15 |
16 | dataset:
17 | test:
18 | module: datasets.kitti.kitti_odometry_evaluation
19 | name: KittiOdometryEvaluation
20 | args:
21 | data_dir: ../dataset/kitti_raw
22 | test_files: ./data/odometry_val.txt
23 | gt_dir: ./kitti_eval/pose_data/ground_truth/10
24 | seq_list: 10
25 | # data_dir: /home/user/.chainer/dataset/pfnet/chainer/raw
26 | seq_len: 5
27 |
28 | updater:
29 | name: StandardUpdater
30 |
31 | iterator:
32 | name: SerialIterator
33 | train_batchsize: 1
34 | test_batchsize: 1
35 |
--------------------------------------------------------------------------------
/experiments/sfm_learner_v1_odom_test.yml:
--------------------------------------------------------------------------------
1 | gpus: []
2 | mode: Test
3 | seed: 1
4 |
5 | model:
6 | module: models.base_model
7 | name: SFMLearner
8 | pretrained_model:
9 | path: #./results/odem/model.npz
10 | download: # https://0000_model.npz
11 | architecture:
12 | smooth_reg: 0.
13 | exp_reg: 0.
14 | seq_len: 5
15 |
16 | dataset:
17 | test:
18 | module: datasets.kitti.kitti_odometry_evaluation
19 | name: KittiOdometryEvaluation
20 | args:
21 | data_dir: ../dataset/kitti_raw
22 | test_files: ./data/odometry_val.txt
23 | gt_dir: ./kitti_eval/pose_data/ground_truth/09
24 | seq_list: 9
25 | # data_dir: /home/user/.chainer/dataset/pfnet/chainer/raw
26 | seq_len: 5
27 |
28 | updater:
29 | name: StandardUpdater
30 |
31 | iterator:
32 | name: SerialIterator
33 | train_batchsize: 4
34 | test_batchsize: 4
35 | # args:
36 | # n_processes: 4
37 | # n_prefetch: 2
38 | # shared_mem: 60000000
39 |
--------------------------------------------------------------------------------
/experiments/sfm_learner_v1_ssim.yml:
--------------------------------------------------------------------------------
1 | end_trigger: [20, "epoch"]
2 | results: results/ssim_015_sm01_lr1
3 | gpus: [2]
4 | mode: Train
5 | seed: 1
6 |
7 | model:
8 | module: models.base_model
9 | name: SFMLearner
10 | pretrained_model:
11 | path: #
12 | download: # https://0000_model.npz
13 | architecture:
14 | smooth_reg: 0.1
15 | exp_reg: 0
16 | seq_len: 3
17 | ssim_rate: 0.15
18 |
19 | dataset:
20 | train:
21 | module: datasets.kitti.kitti_raw_transformed
22 | name: KittiRawTransformed
23 | args:
24 | data_dir: ../dataset/KITTI_formatted
25 | # data_dir: /mnt/sakuradata10-striped/datasets/KITTI_formatted
26 | split: train
27 | seq_len: 3
28 | n_scale: 4
29 |
30 | # valid:
31 | # module: datasets.kitti.kitti_raw_dataset
32 | # name: KittiRawDataset
33 | # args:
34 | # data_dir: ../dataset/KITTI_formatted
35 | # # data_dir: /mnt/sakuradata10-striped/datasets/KITTI_formatted
36 | # split: val
37 | # seq_len: 3
38 |
39 | updater:
40 | name: StandardUpdater
41 |
42 | iterator:
43 | name: MultiprocessIterator
44 | train_batchsize: 4
45 | test_batchsize: 4
46 | args:
47 | n_processes: 4
48 | n_prefetch: 2
49 | shared_mem: 60000000
50 |
51 | optimizer:
52 | name: Adam
53 | args:
54 | alpha: 0.0001
55 | beta1: 0.9
56 | beta2: 0.999
57 | hook:
58 | WeightDecay: 0.0002
59 |
60 | extension:
61 | #Evaluator:
62 | # name: Evaluator
63 | # trigger: [1, 'epoch']
64 | snapshot_object:
65 | trigger: [5000, "iteration"]
66 | LogReport:
67 | trigger: [1000, "iteration"]
68 | PrintReport:
69 | name:
70 | epoch
71 | iteration
72 | main/total_loss
73 | main/pixel_loss
74 | main/smooth_loss
75 | main/exp_loss
76 | main/ssim_loss
77 | trigger: [1000, "iteration"]
78 | ProgressBar:
79 | update_interval: 10
80 | observe_lr:
81 |
--------------------------------------------------------------------------------
/experiments/sfm_learner_v1_test.yml:
--------------------------------------------------------------------------------
1 | gpus: []
2 | mode: Test
3 | seed: 1
4 |
5 | model:
6 | module: models.base_model
7 | name: SFMLearner
8 | pretrained_model:
9 | path: ./weights/exp02smooth01/model.npz
10 | download: # https://0000_model.npz
11 | architecture:
12 | smooth_reg: 0.
13 | exp_reg: 0.
14 | seq_len: 3
15 |
16 | dataset:
17 | test:
18 | module: datasets.kitti.kitti_raw_transformed
19 | name: KittiRawTransformed
20 | args:
21 | data_dir: ../dataset/KITTI_formatted
22 | # data_dir: /mnt/sakuradata10-striped/datasets/KITTI_formatted
23 | split: train
24 | seq_len: 3
25 |
26 | updater:
27 | name: StandardUpdater
28 |
29 | iterator:
30 | name: SerialIterator
31 | train_batchsize: 4
32 | test_batchsize: 4
33 | # args:
34 | # n_processes: 4
35 | # n_prefetch: 2
36 | # shared_mem: 60000000
37 |
--------------------------------------------------------------------------------
/imgs/odom_10.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pfnet/sfm-learner-chainer/03f09adc776041210747ab633270af5191028400/imgs/odom_10.png
--------------------------------------------------------------------------------
/imgs/output_1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pfnet/sfm-learner-chainer/03f09adc776041210747ab633270af5191028400/imgs/output_1.png
--------------------------------------------------------------------------------
/imgs/output_2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pfnet/sfm-learner-chainer/03f09adc776041210747ab633270af5191028400/imgs/output_2.png
--------------------------------------------------------------------------------
/imgs/output_3.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pfnet/sfm-learner-chainer/03f09adc776041210747ab633270af5191028400/imgs/output_3.png
--------------------------------------------------------------------------------
/imgs/result_10.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pfnet/sfm-learner-chainer/03f09adc776041210747ab633270af5191028400/imgs/result_10.png
--------------------------------------------------------------------------------
/imgs/result_9.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pfnet/sfm-learner-chainer/03f09adc776041210747ab633270af5191028400/imgs/result_9.png
--------------------------------------------------------------------------------
/inference.py:
--------------------------------------------------------------------------------
1 | #!/usr/env/bin python3
2 | # -*- coding: utf-8 -*-
3 |
4 | import argparse
5 | import numpy as np
6 | import sys
7 | import subprocess
8 | import os
9 | import yaml
10 |
11 | import chainer
12 | from chainer import cuda, optimizers, serializers
13 | from chainer import training
14 | from chainer import functions as F
15 |
16 | import cv2
17 | from scipy.misc import imread
18 | from kitti_eval.odom_util import convert_trajectory
19 |
20 | from config_utils import *
21 | import matplotlib as mpl
22 | mpl.rcParams['axes.xmargin'] = 0
23 | mpl.rcParams['axes.ymargin'] = 0
24 | import matplotlib.pyplot as plt
25 |
26 | chainer.cuda.set_max_workspace_size(1024 * 1024 * 1024)
27 | os.environ["CHAINER_TYPE_CHECK"] = "0"
28 |
29 | from collections import OrderedDict
30 | yaml.add_constructor(yaml.resolver.BaseResolver.DEFAULT_MAPPING_TAG,
31 | lambda loader, node: OrderedDict(loader.construct_pairs(node)))
32 |
33 | def normalize_depth_for_display(disp, pc=95, crop_percent=0, normalizer=None,
34 | cmap='gray'):
35 | """Convert disparity images to depth images."""
36 | depth = 1./(disp + 1e-6)
37 | if normalizer is not None:
38 | depth = depth/normalizer
39 | else:
40 | depth = depth/(np.percentile(depth, pc) + 1e-6)
41 | depth = np.clip(depth, 0, 1)
42 | depth = gray2rgb(depth, cmap=cmap)
43 | keep_H = int(depth.shape[0] * (1-crop_percent))
44 | depth = depth[:keep_H]
45 | depth = depth
46 | return depth
47 |
48 | def gray2rgb(im, cmap='gray'):
49 | cmap = plt.get_cmap(cmap)
50 | rgba_img = cmap(im.astype(np.float32))
51 | rgb_img = np.delete(rgba_img, 3, 2)
52 | return rgb_img
53 |
54 | def demo_depth_by_image(model, args, gpu_id):
55 | print("Inference for specified image")
56 | input_img = imread(args.img_path).astype(np.float32)
57 | input_img = cv2.resize(input_img, (args.width, args.height))
58 | img = input_img / (255. * 0.5) - 1
59 | img = img.transpose(2, 0, 1)[None, :]
60 | if gpu_id is not None:
61 | img = chainer.cuda.to_gpu(img, device=gpu_id)
62 | pred_depth, _, _ = model.inference(img, None, None, None,
63 | is_depth=True, is_pose=False)
64 | depth = chainer.cuda.to_cpu(pred_depth.data[0, 0])
65 | depth = normalize_depth_for_display(depth)
66 | fig, axes = plt.subplots(1, 2)
67 | axes[0].imshow(input_img / 255)
68 | axes[1].imshow(depth)
69 | axes[0].axis('off')
70 | axes[1].axis('off')
71 | if args.save != -1:
72 | plt.savefig("output_{}.png".format(args.save),
73 | bbox_inches="tight", pad_inches=0.0, transparent=True)
74 | plt.show()
75 | # cv2.imwrite("input.png", (input_img))
76 | # cv2.imwrite("depth.png", depth * 255 )
77 | print("Complete")
78 |
79 | def demo_depth_by_dataset(model, config, gpu_id):
80 | test_data = load_dataset_test(config["dataset"])
81 | test_iter = create_iterator_test(test_data,
82 | config['iterator'])
83 | index = 0
84 | for batch in test_iter:
85 | input_img = batch[0][0].transpose(1, 2, 0)
86 | batch = chainer.dataset.concat_examples(batch, gpu_id)
87 | pred_depth, pred_pose, pred_mask = model.inference(*batch)
88 | depth = chainer.cuda.to_cpu(pred_depth.data[0, 0])
89 | depth = normalize_depth_for_display(depth)
90 | mask = chainer.cuda.to_cpu(pred_mask[0].data[0, 0])
91 | cv2.imwrite("input_{}.png".format(index), (input_img + 1) / 2 * 255)
92 | cv2.imwrite("depth_{}.png".format(index), depth * 255 )
93 | per = np.percentile(mask, 99)
94 | mask = mask * (mask < per)
95 | mask_min = mask.min()
96 | mask_max = mask.max()
97 | mask = (1 - (mask - mask_min) / mask_max) * 255
98 | cv2.imwrite("exp_{}.png".format(index), mask)
99 | print(index)
100 | index += 1
101 |
102 | def demo_odom_by_dataset(model, config, gpu_id):
103 | test_data = load_dataset_test(config["dataset"])
104 | test_iter = create_iterator_test(test_data,
105 | config['iterator'])
106 | index = 0
107 | num_data = len(test_iter.dataset)
108 | print("Start inference")
109 | base_pose = None
110 | scale_list = []
111 | for i, batch in enumerate(test_iter):
112 | if i % 4 != 0:
113 | continue
114 | batch = chainer.dataset.concat_examples(batch, gpu_id)
115 | tgt_img, ref_imgs, _, gt_pose = batch
116 | _, pred_pose, _ = model.inference(tgt_img, ref_imgs,
117 | None, None, is_depth=False,
118 | is_pose=True, is_exp=False)
119 | pred_pose = chainer.cuda.to_cpu(F.concat(pred_pose, axis=0).data)
120 | pred_pose = np.insert(pred_pose, 2, np.zeros((1, 6)), axis=0)
121 | gt_pose = chainer.cuda.to_cpu(gt_pose[0])
122 | pred_pose, orig_pose, base_pose = convert_trajectory(
123 | pred_pose, gt_pose,
124 | base_pose=base_pose)
125 | if i == 0:
126 | all_trajectory = pred_pose
127 | continue
128 | all_trajectory = np.concatenate((all_trajectory, pred_pose[1:, :]), axis=0)
129 | np.savetxt('test.txt', all_trajectory, delimiter=' ')
130 |
131 | def visualize_odom(args, gt_file=None, pred_file=None):
132 | data_dict = {'gt_label': gt_file, 'pred_label': pred_file}
133 | for label, file_name in data_dict.items():
134 | if file_name:
135 | x = []
136 | z = []
137 | with open(file_name, 'r') as f:
138 | data = f.readlines()
139 | for d in data:
140 | d = d.split(" ")
141 | xyz = d[1:4]
142 | x.append(xyz[0])
143 | z.append(xyz[2])
144 | plt.plot(x, z, label=label)
145 | plt.legend()
146 | if args.save != -1:
147 | plt.savefig("result_{}.png".format(args.save))
148 | plt.show()
149 |
150 | def demo_sfm_learner():
151 | """Demo sfm_learner."""
152 | config, args = parse_args()
153 | model = get_model(config["model"])
154 | devices = parse_devices(config['gpus'], config['updater']['name'])
155 | gpu_id = None if devices is None else devices['main']
156 | if devices:
157 | chainer.cuda.get_device_from_id(gpu_id).use()
158 | model.to_gpu(gpu_id)
159 |
160 | if args.mode == "depth":
161 | if args.img_path:
162 | demo_depth_by_image(model, args, gpu_id)
163 | else:
164 | demo_depth_by_dataset(model, config, gpu_id)
165 | elif args.mode == "odom":
166 | if args.gt_file or args.pred_file:
167 | visualize_odom(args, args.gt_file, args.pred_file)
168 | else:
169 | demo_odom_by_dataset(model, config, gpu_id)
170 |
171 | def main():
172 | demo_sfm_learner()
173 |
174 | if __name__ == '__main__':
175 | main()
176 |
--------------------------------------------------------------------------------
/kitti_eval/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pfnet/sfm-learner-chainer/03f09adc776041210747ab633270af5191028400/kitti_eval/__init__.py
--------------------------------------------------------------------------------
/kitti_eval/depth_util.py:
--------------------------------------------------------------------------------
1 | #!/usr/env/bin python3
2 | # -*- coding: utf-8 -*-
3 |
4 | import numpy as np
5 |
6 | def compute_depth_errors(gt, pred):
7 | thresh = np.maximum((gt / pred), (pred / gt))
8 | a1 = (thresh < 1.25 ).mean()
9 | a2 = (thresh < 1.25 ** 2).mean()
10 | a3 = (thresh < 1.25 ** 3).mean()
11 |
12 | rmse = (gt - pred) ** 2
13 | rmse = np.sqrt(rmse.mean())
14 |
15 | rmse_log = (np.log(gt) - np.log(pred)) ** 2
16 | rmse_log = np.sqrt(rmse_log.mean())
17 |
18 | abs_rel = np.mean(np.abs(gt - pred) / gt)
19 |
20 | sq_rel = np.mean(((gt - pred)**2) / gt)
21 |
22 | return np.array([abs_rel, sq_rel, rmse, rmse_log, a1, a2, a3], dtype='f')
23 |
24 | def print_depth_stats(sum_errors):
25 | error_names = ['abs_rel','sq_rel','rms','log_rms','a1','a2','a3']
26 | print("Results with scale factor determined by GT/prediction ratio (like the original paper) : ")
27 | print("{:>10}, {:>10}, {:>10}, {:>10}, {:>10}, {:>10}, {:>10}".format(*error_names))
28 | print("{:10.4f}, {:10.4f}, {:10.4f}, {:10.4f}, {:10.4f}, {:10.4f}, {:10.4f}".format(*sum_errors))
29 |
--------------------------------------------------------------------------------
/kitti_eval/odom_util.py:
--------------------------------------------------------------------------------
1 | # Some of the code are from the TUM evaluation toolkit:
2 | # https://vision.in.tum.de/data/datasets/rgbd-dataset/tools#absolute_trajectory_error_ate
3 |
4 | import math
5 | import numpy as np
6 |
7 |
8 | # print("Predictions dir: %s" % args.pred_dir)
9 | # print("ATE mean: %.4f, std: %.4f" % (np.mean(ate_all), np.std(ate_all)))
10 |
11 | def print_odom_stats(sum_errors):
12 | error_names = ['ATE mean','std']
13 | print("Results with scale factor determined by GT/prediction ratio (like the original paper) : ")
14 | print("{:>10}, {:>10}".format(*error_names))
15 | print("{:10.4f}, {:10.4f}".format(np.mean(sum_errors), np.std(sum_errors)))
16 |
17 | def compute_odom_errors(pred_pose, gt_pose):
18 | gt_xyz = gt_pose[:, 1:4]
19 | pred_xyz = pred_pose[:, 1:4]
20 | # Make sure that the first matched frames align (no need for rotational alignment as
21 | # all the predicted/ground-truth snippets have been converted to use the same coordinate
22 | # system with the first frame of the snippet being the origin).
23 | offset = gt_xyz[0] - pred_xyz[0]
24 | pred_xyz += offset[None,:]
25 |
26 | # Optimize the scaling factor
27 | scale = np.sum(gt_xyz * pred_xyz) / np.sum(pred_xyz ** 2)
28 | alignment_error = pred_xyz * scale - gt_xyz
29 | rmse = np.sqrt(np.sum(alignment_error ** 2)) / len(pred_xyz)
30 | return rmse
31 |
32 | def rot2quat(R):
33 | rz, ry, rx = mat2euler(R)
34 | qw, qx, qy, qz = euler2quat(rz, ry, rx)
35 | return qw, qx, qy, qz
36 |
37 | def quat2mat(q):
38 | ''' Calculate rotation matrix corresponding to quaternion
39 | https://afni.nimh.nih.gov/pub/dist/src/pkundu/meica.libs/nibabel/quaternions.py
40 | Parameters
41 | ----------
42 | q : 4 element array-like
43 |
44 | Returns
45 | -------
46 | M : (3,3) array
47 | Rotation matrix corresponding to input quaternion *q*
48 |
49 | Notes
50 | -----
51 | Rotation matrix applies to column vectors, and is applied to the
52 | left of coordinate vectors. The algorithm here allows non-unit
53 | quaternions.
54 |
55 | References
56 | ----------
57 | Algorithm from
58 | http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion
59 |
60 | Examples
61 | --------
62 | >>> import numpy as np
63 | >>> M = quat2mat([1, 0, 0, 0]) # Identity quaternion
64 | >>> np.allclose(M, np.eye(3))
65 | True
66 | >>> M = quat2mat([0, 1, 0, 0]) # 180 degree rotn around axis 0
67 | >>> np.allclose(M, np.diag([1, -1, -1]))
68 | True
69 | '''
70 | w, x, y, z = q
71 | Nq = w*w + x*x + y*y + z*z
72 | if Nq < 1e-8:
73 | return np.eye(3)
74 | s = 2.0/Nq
75 | X = x*s
76 | Y = y*s
77 | Z = z*s
78 | wX = w*X; wY = w*Y; wZ = w*Z
79 | xX = x*X; xY = x*Y; xZ = x*Z
80 | yY = y*Y; yZ = y*Z; zZ = z*Z
81 | return np.array(
82 | [[ 1.0-(yY+zZ), xY-wZ, xZ+wY ],
83 | [ xY+wZ, 1.0-(xX+zZ), yZ-wX ],
84 | [ xZ-wY, yZ+wX, 1.0-(xX+yY) ]])
85 |
86 | def mat2euler(M, cy_thresh=None, seq='zyx'):
87 | '''
88 | Taken From: http://afni.nimh.nih.gov/pub/dist/src/pkundu/meica.libs/nibabel/eulerangles.py
89 | Discover Euler angle vector from 3x3 matrix
90 | Uses the conventions above.
91 | Parameters
92 | ----------
93 | M : array-like, shape (3,3)
94 | cy_thresh : None or scalar, optional
95 | threshold below which to give up on straightforward arctan for
96 | estimating x rotation. If None (default), estimate from
97 | precision of input.
98 | Returns
99 | -------
100 | z : scalar
101 | y : scalar
102 | x : scalar
103 | Rotations in radians around z, y, x axes, respectively
104 | Notes
105 | -----
106 | If there was no numerical error, the routine could be derived using
107 | Sympy expression for z then y then x rotation matrix, which is::
108 | [ cos(y)*cos(z), -cos(y)*sin(z), sin(y)],
109 | [cos(x)*sin(z) + cos(z)*sin(x)*sin(y), cos(x)*cos(z) - sin(x)*sin(y)*sin(z), -cos(y)*sin(x)],
110 | [sin(x)*sin(z) - cos(x)*cos(z)*sin(y), cos(z)*sin(x) + cos(x)*sin(y)*sin(z), cos(x)*cos(y)]
111 | with the obvious derivations for z, y, and x
112 | z = atan2(-r12, r11)
113 | y = asin(r13)
114 | x = atan2(-r23, r33)
115 | for x,y,z order
116 | y = asin(-r31)
117 | x = atan2(r32, r33)
118 | z = atan2(r21, r11)
119 | Problems arise when cos(y) is close to zero, because both of::
120 | z = atan2(cos(y)*sin(z), cos(y)*cos(z))
121 | x = atan2(cos(y)*sin(x), cos(x)*cos(y))
122 | will be close to atan2(0, 0), and highly unstable.
123 | The ``cy`` fix for numerical instability below is from: *Graphics
124 | Gems IV*, Paul Heckbert (editor), Academic Press, 1994, ISBN:
125 | 0123361559. Specifically it comes from EulerAngles.c by Ken
126 | Shoemake, and deals with the case where cos(y) is close to zero:
127 | See: http://www.graphicsgems.org/
128 | The code appears to be licensed (from the website) as "can be used
129 | without restrictions".
130 | '''
131 | M = np.asarray(M)
132 | if cy_thresh is None:
133 | try:
134 | cy_thresh = np.finfo(M.dtype).eps * 4
135 | except ValueError:
136 | cy_thresh = _FLOAT_EPS_4
137 | r11, r12, r13, r21, r22, r23, r31, r32, r33 = M.flat
138 | # cy: sqrt((cos(y)*cos(z))**2 + (cos(x)*cos(y))**2)
139 | cy = math.sqrt(r33*r33 + r23*r23)
140 | if seq=='zyx':
141 | if cy > cy_thresh: # cos(y) not close to zero, standard form
142 | z = math.atan2(-r12, r11) # atan2(cos(y)*sin(z), cos(y)*cos(z))
143 | y = math.atan2(r13, cy) # atan2(sin(y), cy)
144 | x = math.atan2(-r23, r33) # atan2(cos(y)*sin(x), cos(x)*cos(y))
145 | else: # cos(y) (close to) zero, so x -> 0.0 (see above)
146 | # so r21 -> sin(z), r22 -> cos(z) and
147 | z = math.atan2(r21, r22)
148 | y = math.atan2(r13, cy) # atan2(sin(y), cy)
149 | x = 0.0
150 | elif seq=='xyz':
151 | if cy > cy_thresh:
152 | y = math.atan2(-r31, cy)
153 | x = math.atan2(r32, r33)
154 | z = math.atan2(r21, r11)
155 | else:
156 | z = 0.0
157 | if r31 < 0:
158 | y = np.pi/2
159 | x = atan2(r12, r13)
160 | else:
161 | y = -np.pi/2
162 | else:
163 | raise Exception('Sequence not recognized')
164 | return z, y, x
165 |
166 | import functools
167 | def euler2mat(z=0, y=0, x=0, isRadian=True):
168 | if not isRadian:
169 | z = ((np.pi)/180.) * z
170 | y = ((np.pi)/180.) * y
171 | x = ((np.pi)/180.) * x
172 | assert z>=(-np.pi) and z < np.pi, 'Inapprorpriate z: %f' % z
173 | assert y>=(-np.pi) and y < np.pi, 'Inapprorpriate y: %f' % y
174 | assert x>=(-np.pi) and x < np.pi, 'Inapprorpriate x: %f' % x
175 |
176 | Ms = []
177 | if z:
178 | cosz = math.cos(z)
179 | sinz = math.sin(z)
180 | Ms.append(np.array(
181 | [[cosz, -sinz, 0],
182 | [sinz, cosz, 0],
183 | [0, 0, 1]]))
184 | if y:
185 | cosy = math.cos(y)
186 | siny = math.sin(y)
187 | Ms.append(np.array(
188 | [[cosy, 0, siny],
189 | [0, 1, 0],
190 | [-siny, 0, cosy]]))
191 | if x:
192 | cosx = math.cos(x)
193 | sinx = math.sin(x)
194 | Ms.append(np.array(
195 | [[1, 0, 0],
196 | [0, cosx, -sinx],
197 | [0, sinx, cosx]]))
198 | if Ms:
199 | return functools.reduce(np.dot, Ms[::-1])
200 | return np.eye(3)
201 |
202 | def euler2quat(z=0, y=0, x=0, isRadian=True):
203 | ''' Return quaternion corresponding to these Euler angles
204 | Uses the z, then y, then x convention above
205 | Parameters
206 | ----------
207 | z : scalar
208 | Rotation angle in radians around z-axis (performed first)
209 | y : scalar
210 | Rotation angle in radians around y-axis
211 | x : scalar
212 | Rotation angle in radians around x-axis (performed last)
213 | Returns
214 | -------
215 | quat : array shape (4,)
216 | Quaternion in w, x, y z (real, then vector) format
217 | Notes
218 | -----
219 | We can derive this formula in Sympy using:
220 | 1. Formula giving quaternion corresponding to rotation of theta radians
221 | about arbitrary axis:
222 | http://mathworld.wolfram.com/EulerParameters.html
223 | 2. Generated formulae from 1.) for quaternions corresponding to
224 | theta radians rotations about ``x, y, z`` axes
225 | 3. Apply quaternion multiplication formula -
226 | http://en.wikipedia.org/wiki/Quaternions#Hamilton_product - to
227 | formulae from 2.) to give formula for combined rotations.
228 | '''
229 |
230 | if not isRadian:
231 | z = ((np.pi)/180.) * z
232 | y = ((np.pi)/180.) * y
233 | x = ((np.pi)/180.) * x
234 | z = z/2.0
235 | y = y/2.0
236 | x = x/2.0
237 | cz = math.cos(z)
238 | sz = math.sin(z)
239 | cy = math.cos(y)
240 | sy = math.sin(y)
241 | cx = math.cos(x)
242 | sx = math.sin(x)
243 | return np.array([
244 | cx*cy*cz - sx*sy*sz,
245 | cx*sy*sz + cy*cz*sx,
246 | cx*cz*sy - sx*cy*sz,
247 | cx*cy*sz + sx*cz*sy])
248 |
249 | def pose_vec_to_mat(vec):
250 | tx = vec[3]
251 | ty = vec[4]
252 | tz = vec[5]
253 | trans = np.array([tx, ty, tz]).reshape((3,1))
254 | rot = euler2mat(z=vec[2], y=vec[1], x=vec[0])
255 | Tmat = np.concatenate((rot, trans), axis=1)
256 | hfiller = np.array([0, 0, 0, 1]).reshape((1,4))
257 | Tmat = np.concatenate((Tmat, hfiller), axis=0)
258 | return Tmat
259 |
260 | def convert_eval_format(pred_pose, gt_pose):
261 | pred_data = []
262 | first_pose = pose_vec_to_mat(pred_pose[0])
263 | for p in range(len(gt_pose)):
264 | this_pose = pose_vec_to_mat(pred_pose[p])
265 | this_pose = np.dot(first_pose, np.linalg.inv(this_pose))
266 | tx = this_pose[0, 3]
267 | ty = this_pose[1, 3]
268 | tz = this_pose[2, 3]
269 | rot = this_pose[:3, :3]
270 | qw, qx, qy, qz = rot2quat(rot)
271 | pred_data.append([gt_pose[p][0], tx, ty, tz, qx, qy, qz, qw])
272 | return np.array(pred_data, dtype='f')
273 |
274 |
275 | def mat2eval_format(this_pose, gt_pose):
276 | tx = this_pose[0, 3]
277 | ty = this_pose[1, 3]
278 | tz = this_pose[2, 3]
279 | rot = this_pose[:3, :3]
280 | qw, qx, qy, qz = rot2quat(rot)
281 | return [gt_pose[0], tx, ty, tz, qx, qy, qz, qw]
282 |
283 | def convert_trajectory(pred_pose, gt_pose, base_pose=None):
284 | pred_data = []
285 | orig_data = []
286 |
287 | # Convert base from center to first element
288 | first_pose = pose_vec_to_mat(pred_pose[0])
289 | for p in range(len(gt_pose)):
290 | this_pose = pose_vec_to_mat(pred_pose[p])
291 | this_pose = np.dot(first_pose, np.linalg.inv(this_pose))
292 | orig_data.append(mat2eval_format(this_pose, gt_pose[p]))
293 |
294 | # Rescale
295 | orig_data = np.array(orig_data, dtype='f')
296 | scale = np.sum(np.abs(gt_pose[:, 1:4] * orig_data[:, 1:4])) / np.sum(orig_data[:, 1:4] ** 2)
297 | orig_data[:, 1:4] *= scale
298 |
299 | # Convert base from first element to root(base_pose)
300 | for p in range(len(gt_pose)):
301 | data = orig_data[p]
302 | tx, ty, tz, qx, qy, qz, qw = data[1:]
303 | this_pose = np.zeros((4, 4), dtype="f")
304 | this_pose[0, 3] = tx
305 | this_pose[1, 3] = ty
306 | this_pose[2, 3] = tz
307 | this_pose[3, 3] = 1
308 | this_pose[:3, :3] = quat2mat([qw, qx, qy, qz])
309 | if base_pose is not None:
310 | this_pose = np.dot(base_pose, this_pose)
311 | pred_data.append(mat2eval_format(this_pose, gt_pose[p]))
312 |
313 | base_pose = this_pose
314 | pred_data = np.array(pred_data, dtype='f')
315 | orig_data = np.array(orig_data, dtype='f')
316 | return pred_data, orig_data, base_pose
317 |
--------------------------------------------------------------------------------
/kitti_eval/test_files_eigen.txt:
--------------------------------------------------------------------------------
1 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000069.png
2 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000054.png
3 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000042.png
4 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000057.png
5 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000030.png
6 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000027.png
7 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000012.png
8 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000075.png
9 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000036.png
10 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000033.png
11 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000015.png
12 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000072.png
13 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000003.png
14 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000039.png
15 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000009.png
16 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000051.png
17 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000060.png
18 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000021.png
19 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000000.png
20 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000024.png
21 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000045.png
22 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000018.png
23 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000048.png
24 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000006.png
25 | 2011_09_26/2011_09_26_drive_0002_sync/image_02/data/0000000063.png
26 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000000.png
27 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000016.png
28 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000032.png
29 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000048.png
30 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000064.png
31 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000080.png
32 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000096.png
33 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000112.png
34 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000128.png
35 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000144.png
36 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000160.png
37 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000176.png
38 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000196.png
39 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000212.png
40 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000228.png
41 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000244.png
42 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000260.png
43 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000276.png
44 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000292.png
45 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000308.png
46 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000324.png
47 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000340.png
48 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000356.png
49 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000372.png
50 | 2011_09_26/2011_09_26_drive_0009_sync/image_02/data/0000000388.png
51 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000090.png
52 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000050.png
53 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000110.png
54 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000115.png
55 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000060.png
56 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000105.png
57 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000125.png
58 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000020.png
59 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000140.png
60 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000085.png
61 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000070.png
62 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000080.png
63 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000065.png
64 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000095.png
65 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000130.png
66 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000100.png
67 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000010.png
68 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000030.png
69 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000000.png
70 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000135.png
71 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000040.png
72 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000005.png
73 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000120.png
74 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000045.png
75 | 2011_09_26/2011_09_26_drive_0013_sync/image_02/data/0000000035.png
76 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000003.png
77 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000069.png
78 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000057.png
79 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000012.png
80 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000072.png
81 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000018.png
82 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000063.png
83 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000000.png
84 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000084.png
85 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000015.png
86 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000066.png
87 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000006.png
88 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000048.png
89 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000060.png
90 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000009.png
91 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000033.png
92 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000021.png
93 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000075.png
94 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000027.png
95 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000045.png
96 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000078.png
97 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000036.png
98 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000051.png
99 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000054.png
100 | 2011_09_26/2011_09_26_drive_0020_sync/image_02/data/0000000042.png
101 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000018.png
102 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000090.png
103 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000126.png
104 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000378.png
105 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000036.png
106 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000288.png
107 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000198.png
108 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000450.png
109 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000144.png
110 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000072.png
111 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000252.png
112 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000180.png
113 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000432.png
114 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000396.png
115 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000054.png
116 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000468.png
117 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000306.png
118 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000108.png
119 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000162.png
120 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000342.png
121 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000270.png
122 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000414.png
123 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000216.png
124 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000360.png
125 | 2011_09_26/2011_09_26_drive_0023_sync/image_02/data/0000000324.png
126 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000077.png
127 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000035.png
128 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000091.png
129 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000112.png
130 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000007.png
131 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000175.png
132 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000042.png
133 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000098.png
134 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000133.png
135 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000161.png
136 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000014.png
137 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000126.png
138 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000168.png
139 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000070.png
140 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000084.png
141 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000140.png
142 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000049.png
143 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000000.png
144 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000182.png
145 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000147.png
146 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000056.png
147 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000063.png
148 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000021.png
149 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000119.png
150 | 2011_09_26/2011_09_26_drive_0027_sync/image_02/data/0000000028.png
151 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000380.png
152 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000394.png
153 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000324.png
154 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000000.png
155 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000268.png
156 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000366.png
157 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000296.png
158 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000014.png
159 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000028.png
160 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000182.png
161 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000168.png
162 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000196.png
163 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000140.png
164 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000084.png
165 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000056.png
166 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000112.png
167 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000352.png
168 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000126.png
169 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000070.png
170 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000310.png
171 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000154.png
172 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000098.png
173 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000408.png
174 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000042.png
175 | 2011_09_26/2011_09_26_drive_0029_sync/image_02/data/0000000338.png
176 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000000.png
177 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000128.png
178 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000192.png
179 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000032.png
180 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000352.png
181 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000608.png
182 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000224.png
183 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000576.png
184 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000672.png
185 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000064.png
186 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000448.png
187 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000704.png
188 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000640.png
189 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000512.png
190 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000768.png
191 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000160.png
192 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000416.png
193 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000480.png
194 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000800.png
195 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000288.png
196 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000544.png
197 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000096.png
198 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000384.png
199 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000256.png
200 | 2011_09_26/2011_09_26_drive_0036_sync/image_02/data/0000000320.png
201 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000000.png
202 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000005.png
203 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000010.png
204 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000015.png
205 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000020.png
206 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000025.png
207 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000030.png
208 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000035.png
209 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000040.png
210 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000045.png
211 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000050.png
212 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000055.png
213 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000060.png
214 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000065.png
215 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000070.png
216 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000075.png
217 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000080.png
218 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000085.png
219 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000090.png
220 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000095.png
221 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000100.png
222 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000105.png
223 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000110.png
224 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000115.png
225 | 2011_09_26/2011_09_26_drive_0046_sync/image_02/data/0000000120.png
226 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000000.png
227 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000001.png
228 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000002.png
229 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000003.png
230 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000004.png
231 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000005.png
232 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000006.png
233 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000007.png
234 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000008.png
235 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000009.png
236 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000010.png
237 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000011.png
238 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000012.png
239 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000013.png
240 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000014.png
241 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000015.png
242 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000016.png
243 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000017.png
244 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000018.png
245 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000019.png
246 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000020.png
247 | 2011_09_26/2011_09_26_drive_0048_sync/image_02/data/0000000021.png
248 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000046.png
249 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000014.png
250 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000036.png
251 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000028.png
252 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000026.png
253 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000050.png
254 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000040.png
255 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000008.png
256 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000016.png
257 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000044.png
258 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000018.png
259 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000032.png
260 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000042.png
261 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000010.png
262 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000020.png
263 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000048.png
264 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000052.png
265 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000006.png
266 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000030.png
267 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000012.png
268 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000038.png
269 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000000.png
270 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000002.png
271 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000004.png
272 | 2011_09_26/2011_09_26_drive_0052_sync/image_02/data/0000000022.png
273 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000011.png
274 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000033.png
275 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000242.png
276 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000253.png
277 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000286.png
278 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000154.png
279 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000099.png
280 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000220.png
281 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000022.png
282 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000077.png
283 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000187.png
284 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000143.png
285 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000066.png
286 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000176.png
287 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000110.png
288 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000275.png
289 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000264.png
290 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000198.png
291 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000055.png
292 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000088.png
293 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000121.png
294 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000209.png
295 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000165.png
296 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000231.png
297 | 2011_09_26/2011_09_26_drive_0056_sync/image_02/data/0000000044.png
298 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000056.png
299 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000000.png
300 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000344.png
301 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000358.png
302 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000316.png
303 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000238.png
304 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000098.png
305 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000112.png
306 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000028.png
307 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000014.png
308 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000330.png
309 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000154.png
310 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000042.png
311 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000302.png
312 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000182.png
313 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000288.png
314 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000140.png
315 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000274.png
316 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000224.png
317 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000372.png
318 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000196.png
319 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000126.png
320 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000084.png
321 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000210.png
322 | 2011_09_26/2011_09_26_drive_0059_sync/image_02/data/0000000070.png
323 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000528.png
324 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000308.png
325 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000044.png
326 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000352.png
327 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000066.png
328 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000000.png
329 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000506.png
330 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000176.png
331 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000022.png
332 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000242.png
333 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000462.png
334 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000418.png
335 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000110.png
336 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000440.png
337 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000396.png
338 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000154.png
339 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000374.png
340 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000088.png
341 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000286.png
342 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000550.png
343 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000264.png
344 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000220.png
345 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000330.png
346 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000484.png
347 | 2011_09_26/2011_09_26_drive_0064_sync/image_02/data/0000000198.png
348 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000283.png
349 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000361.png
350 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000270.png
351 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000127.png
352 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000205.png
353 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000218.png
354 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000153.png
355 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000335.png
356 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000192.png
357 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000348.png
358 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000101.png
359 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000049.png
360 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000179.png
361 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000140.png
362 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000374.png
363 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000322.png
364 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000309.png
365 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000244.png
366 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000062.png
367 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000257.png
368 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000088.png
369 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000114.png
370 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000075.png
371 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000296.png
372 | 2011_09_26/2011_09_26_drive_0084_sync/image_02/data/0000000231.png
373 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000007.png
374 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000196.png
375 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000439.png
376 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000169.png
377 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000115.png
378 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000034.png
379 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000304.png
380 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000331.png
381 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000277.png
382 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000520.png
383 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000682.png
384 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000628.png
385 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000088.png
386 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000601.png
387 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000574.png
388 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000223.png
389 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000655.png
390 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000358.png
391 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000412.png
392 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000142.png
393 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000385.png
394 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000061.png
395 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000493.png
396 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000466.png
397 | 2011_09_26/2011_09_26_drive_0086_sync/image_02/data/0000000250.png
398 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000000.png
399 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000016.png
400 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000032.png
401 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000048.png
402 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000064.png
403 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000080.png
404 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000096.png
405 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000112.png
406 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000128.png
407 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000144.png
408 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000160.png
409 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000176.png
410 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000192.png
411 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000208.png
412 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000224.png
413 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000240.png
414 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000256.png
415 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000305.png
416 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000321.png
417 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000337.png
418 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000353.png
419 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000369.png
420 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000385.png
421 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000401.png
422 | 2011_09_26/2011_09_26_drive_0093_sync/image_02/data/0000000417.png
423 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000000.png
424 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000019.png
425 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000038.png
426 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000057.png
427 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000076.png
428 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000095.png
429 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000114.png
430 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000133.png
431 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000152.png
432 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000171.png
433 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000190.png
434 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000209.png
435 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000228.png
436 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000247.png
437 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000266.png
438 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000285.png
439 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000304.png
440 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000323.png
441 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000342.png
442 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000361.png
443 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000380.png
444 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000399.png
445 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000418.png
446 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000437.png
447 | 2011_09_26/2011_09_26_drive_0096_sync/image_02/data/0000000456.png
448 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000692.png
449 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000930.png
450 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000760.png
451 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000896.png
452 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000284.png
453 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000148.png
454 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000522.png
455 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000794.png
456 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000624.png
457 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000726.png
458 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000216.png
459 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000318.png
460 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000488.png
461 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000590.png
462 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000454.png
463 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000862.png
464 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000386.png
465 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000352.png
466 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000420.png
467 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000658.png
468 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000828.png
469 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000556.png
470 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000114.png
471 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000182.png
472 | 2011_09_26/2011_09_26_drive_0101_sync/image_02/data/0000000080.png
473 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000015.png
474 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000035.png
475 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000043.png
476 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000051.png
477 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000059.png
478 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000067.png
479 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000075.png
480 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000083.png
481 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000091.png
482 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000099.png
483 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000107.png
484 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000115.png
485 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000123.png
486 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000131.png
487 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000139.png
488 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000147.png
489 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000155.png
490 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000163.png
491 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000171.png
492 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000179.png
493 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000187.png
494 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000195.png
495 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000203.png
496 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000211.png
497 | 2011_09_26/2011_09_26_drive_0106_sync/image_02/data/0000000219.png
498 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000312.png
499 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000494.png
500 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000104.png
501 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000130.png
502 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000156.png
503 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000182.png
504 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000598.png
505 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000416.png
506 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000364.png
507 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000026.png
508 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000078.png
509 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000572.png
510 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000468.png
511 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000260.png
512 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000624.png
513 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000234.png
514 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000442.png
515 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000390.png
516 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000546.png
517 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000286.png
518 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000000.png
519 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000338.png
520 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000208.png
521 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000650.png
522 | 2011_09_26/2011_09_26_drive_0117_sync/image_02/data/0000000052.png
523 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000024.png
524 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000021.png
525 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000036.png
526 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000000.png
527 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000051.png
528 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000018.png
529 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000033.png
530 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000090.png
531 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000045.png
532 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000054.png
533 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000012.png
534 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000039.png
535 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000009.png
536 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000003.png
537 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000030.png
538 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000078.png
539 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000060.png
540 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000048.png
541 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000084.png
542 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000081.png
543 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000006.png
544 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000057.png
545 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000072.png
546 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000087.png
547 | 2011_09_28/2011_09_28_drive_0002_sync/image_02/data/0000000063.png
548 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000252.png
549 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000540.png
550 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000001054.png
551 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000036.png
552 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000360.png
553 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000807.png
554 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000879.png
555 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000288.png
556 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000771.png
557 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000000.png
558 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000216.png
559 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000951.png
560 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000324.png
561 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000432.png
562 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000504.png
563 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000576.png
564 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000108.png
565 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000180.png
566 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000072.png
567 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000612.png
568 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000915.png
569 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000735.png
570 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000144.png
571 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000396.png
572 | 2011_09_29/2011_09_29_drive_0071_sync/image_02/data/0000000468.png
573 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000132.png
574 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000011.png
575 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000154.png
576 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000022.png
577 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000242.png
578 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000198.png
579 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000176.png
580 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000231.png
581 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000275.png
582 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000220.png
583 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000088.png
584 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000143.png
585 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000055.png
586 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000033.png
587 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000187.png
588 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000110.png
589 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000044.png
590 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000077.png
591 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000066.png
592 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000000.png
593 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000165.png
594 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000264.png
595 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000253.png
596 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000209.png
597 | 2011_09_30/2011_09_30_drive_0016_sync/image_02/data/0000000121.png
598 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000000107.png
599 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000002247.png
600 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000001391.png
601 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000000535.png
602 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000001819.png
603 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000001177.png
604 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000000428.png
605 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000001926.png
606 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000000749.png
607 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000001284.png
608 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000002140.png
609 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000001605.png
610 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000001498.png
611 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000000642.png
612 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000002740.png
613 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000002419.png
614 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000000856.png
615 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000002526.png
616 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000001712.png
617 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000001070.png
618 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000000000.png
619 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000002033.png
620 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000000214.png
621 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000000963.png
622 | 2011_09_30/2011_09_30_drive_0018_sync/image_02/data/0000002633.png
623 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000533.png
624 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000001040.png
625 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000082.png
626 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000205.png
627 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000835.png
628 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000451.png
629 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000164.png
630 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000794.png
631 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000328.png
632 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000615.png
633 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000917.png
634 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000369.png
635 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000287.png
636 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000123.png
637 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000876.png
638 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000410.png
639 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000492.png
640 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000958.png
641 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000656.png
642 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000000.png
643 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000753.png
644 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000574.png
645 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000001081.png
646 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000041.png
647 | 2011_09_30/2011_09_30_drive_0027_sync/image_02/data/0000000246.png
648 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000002906.png
649 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000002544.png
650 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000000362.png
651 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000004535.png
652 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000000734.png
653 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000001096.png
654 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000004173.png
655 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000000543.png
656 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000001277.png
657 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000004354.png
658 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000001458.png
659 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000001820.png
660 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000003449.png
661 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000003268.png
662 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000000915.png
663 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000002363.png
664 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000002725.png
665 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000000181.png
666 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000001639.png
667 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000003992.png
668 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000003087.png
669 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000002001.png
670 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000003811.png
671 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000003630.png
672 | 2011_10_03/2011_10_03_drive_0027_sync/image_02/data/0000000000.png
673 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000096.png
674 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000800.png
675 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000320.png
676 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000576.png
677 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000000.png
678 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000480.png
679 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000640.png
680 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000032.png
681 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000384.png
682 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000160.png
683 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000704.png
684 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000736.png
685 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000672.png
686 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000064.png
687 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000288.png
688 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000352.png
689 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000512.png
690 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000544.png
691 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000608.png
692 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000128.png
693 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000224.png
694 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000416.png
695 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000192.png
696 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000448.png
697 | 2011_10_03/2011_10_03_drive_0047_sync/image_02/data/0000000768.png
698 |
--------------------------------------------------------------------------------
/models/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/pfnet/sfm-learner-chainer/03f09adc776041210747ab633270af5191028400/models/__init__.py
--------------------------------------------------------------------------------
/models/base_model.py:
--------------------------------------------------------------------------------
1 | #/usr/bin/env python3
2 | # -*- coding: utf-8 -*-
3 |
4 | import functools
5 | import numpy as np
6 | import os
7 | import sys
8 | import subprocess
9 | import time
10 | try:
11 | import matplotlib.pyplot as plt
12 | except:
13 | pass
14 | import cv2
15 | import chainer
16 | import chainer.functions as F
17 | import chainer.links as L
18 | from chainer import Variable
19 | from models.transform import projective_inverse_warp
20 | from models.pose_net import PoseNet
21 | from models.disp_net import DispNet
22 | from models.utils import *
23 |
24 | def parse_dict(dic, key, value=None):
25 | return value if dic is None or not key in dic else dic[key]
26 |
27 |
28 | class SFMLearner(chainer.Chain):
29 |
30 | """Sfm Learner original Implementation"""
31 |
32 | def __init__(self, config, pretrained_model=None):
33 | super(SFMLearner, self).__init__(
34 | pose_net = PoseNet(n_sources=config['seq_len'] - 1),
35 | disp_net = DispNet())
36 |
37 | self.smooth_reg = config['smooth_reg']
38 | self.exp_reg = config['exp_reg']
39 | self.ssim_rate = parse_dict(config, 'ssim_rate', 0.0)
40 |
41 | if pretrained_model['download']:
42 | if not os.path.exists(pretrained_model['download'].split("/")[-1]):
43 | subprocess.call(['wget', pretrained_model['download']])
44 |
45 | if pretrained_model['path']:
46 | chainer.serializers.load_npz(pretrained_model['path'], self)
47 |
48 | def __call__(self, tgt_img, src_imgs, intrinsics, inv_intrinsics):
49 | """
50 | Args:
51 | tgt_img: target image. Shape is (Batch, 3, H, W)
52 | src_imgs: source images. Shape is (Batch, ?, 3, H, W)
53 | intrinsics: Shape is (Batch, ?, 3, 3)
54 | Return:
55 | loss (Variable).
56 | """
57 | batchsize, n_sources, _, H, W = src_imgs.shape
58 | stacked_src_imgs = self.xp.reshape(src_imgs, (batchsize, -1, H, W))
59 | pred_disps = self.disp_net(tgt_img)
60 | pred_depthes = [1. / d for d in pred_disps]
61 | do_exp = self.exp_reg is not None and self.exp_reg > 0
62 | pred_poses, pred_maskes = self.pose_net(tgt_img, stacked_src_imgs,
63 | do_exp=do_exp)
64 | smooth_loss, exp_loss, pixel_loss = 0, 0, 0
65 | ssim_loss = 0
66 | n_scales = len(pred_depthes)
67 | start, stop = create_timer()
68 | sum_time = 0
69 | for ns in range(n_scales):
70 | curr_img_size = (H // (2 ** ns), W // (2 ** ns))
71 | curr_tgt_img = F.resize_images(tgt_img, curr_img_size).data
72 | curr_src_imgs = F.resize_images(stacked_src_imgs, curr_img_size).data
73 |
74 | # Smoothness regularization
75 | if self.smooth_reg:
76 | smooth_loss += (self.smooth_reg / (2 ** ns)) * \
77 | self.compute_smooth_loss(pred_disps[ns])
78 | # smooth_loss += (self.smooth_reg / (2 ** ns)) * \
79 | # self.compute_disp_smooth(curr_tgt_img,
80 | # pred_disps[ns])
81 | curr_pred_depthes = pred_depthes[ns]
82 | curr_pred_depthes = F.reshape(curr_pred_depthes, (batchsize, 1, -1))
83 | curr_pred_depthes = F.broadcast_to(curr_pred_depthes,
84 | (batchsize, 3, curr_pred_depthes.shape[2]))
85 | curr_intrinsics = intrinsics[:, ns]
86 | if self.exp_reg:
87 | curr_pred_mask = pred_maskes[ns]
88 | for i in range(n_sources):
89 | # Inverse warp the source image to the target image frame
90 | curr_proj_img = projective_inverse_warp(
91 | curr_src_imgs[:, i*3:(i+1)*3],
92 | curr_pred_depthes,
93 | pred_poses[i],
94 | curr_intrinsics)
95 | curr_proj_error = F.absolute(curr_proj_img - curr_tgt_img)
96 | mask = (curr_proj_img.data == 0).prod(1, keepdims=True).astype('bool')
97 | mask = self.xp.broadcast_to(mask, curr_proj_error.shape)
98 | curr_proj_error = F.where(mask,
99 | self.xp.zeros(curr_proj_error.shape, dtype='f'),
100 | curr_proj_error)
101 | # curr_proj_error *= (1 - (curr_proj_img.data == 0).prod(1, keepdims=True))
102 | # explainability regularization
103 | if self.exp_reg:
104 | pred_exp_logits = curr_pred_mask[:, i:i+1, :, :]
105 | exp_loss += self.exp_reg * \
106 | self.compute_exp_reg_loss(pred_exp_logits)
107 | pred_exp = F.sigmoid(pred_exp_logits)
108 | pred_exp = F.broadcast_to(pred_exp, (batchsize, 3, curr_img_size[0], curr_img_size[1]))
109 | pixel_loss += F.mean(curr_proj_error * pred_exp)
110 | else:
111 | pixel_loss += F.mean(curr_proj_error)
112 | if self.ssim_rate:
113 | ssim_error = self.compute_ssim(curr_proj_img, curr_tgt_img)
114 | ssim_error *= (1 - mask)
115 | ssim_loss += F.mean(ssim_error)
116 |
117 | total_loss = (1 - self.ssim_rate) * pixel_loss + self.ssim_rate * ssim_loss + \
118 | smooth_loss + exp_loss
119 | chainer.report({'total_loss': total_loss}, self)
120 | chainer.report({'pixel_loss': pixel_loss}, self)
121 | chainer.report({'smooth_loss': smooth_loss}, self)
122 | chainer.report({'exp_loss': exp_loss}, self)
123 | chainer.report({'ssim_loss': ssim_loss}, self)
124 | return total_loss
125 |
126 | def compute_ssim(self, x, y):
127 | c1 = 0.01 ** 2
128 | c2 = 0.03 ** 2
129 |
130 | mu_x = F.average_pooling_2d(x, 3, 1, 1)
131 | mu_y = F.average_pooling_2d(y, 3, 1, 1).data
132 |
133 | sigma_x = F.average_pooling_2d(x ** 2, 3, 1, 1) - mu_x ** 2
134 | sigma_y = F.average_pooling_2d(y ** 2, 3, 1, 1).data - mu_y ** 2
135 | sigma_xy = F.average_pooling_2d(x * y, 3, 1, 1) - mu_x * mu_y
136 |
137 | SSIM_n = (2 * mu_x * mu_y + c1) * (2 * sigma_xy + c2)
138 | SSIM_d = (mu_x ** 2 + mu_y ** 2 + c1) * (sigma_x + sigma_y + c2)
139 |
140 | SSIM = SSIM_n / SSIM_d
141 |
142 | return F.clip((1 - SSIM) / 2, 0., 1.)
143 |
144 | def compute_disp_smooth(self, img, pred_disp):
145 | def gradient(input_img):
146 | D_dy = input_img[:, :, 1:] - input_img[:, :, :-1]
147 | D_dx = input_img[:, :, :, 1:] - input_img[:, :, :, :-1]
148 | return D_dx, D_dy
149 |
150 | i_dx, i_dy = gradient(img)
151 | i_dx = F.mean(i_dx, axis=1, keepdims=True)
152 | i_dy = F.mean(i_dy, axis=1, keepdims=True)
153 | d_dx, d_dy = gradient(pred_disp)
154 | return F.mean(F.absolute(d_dx) * F.exp(-F.absolute(i_dx))) \
155 | + F.mean(F.absolute(d_dy) * F.exp(-F.absolute(i_dy)))
156 |
157 | def compute_exp_reg_loss(self, pred):
158 | """Compute expalanation loss.
159 |
160 | Args:
161 | pred: Shape is (Batch, 2, H, W)
162 | """
163 | p_shape = pred.shape
164 | label = self.xp.ones((p_shape[0] * p_shape[2] * p_shape[3],), dtype='i')
165 | l = F.sigmoid_cross_entropy(
166 | F.reshape(pred, (-1, )), label, reduce='no')
167 | return F.mean(l)
168 |
169 | def compute_smooth_loss(self, pred_disp):
170 | """Compute smoothness loss for the predicted dpeth maps.
171 | L1 norm of the second-order gradients.
172 |
173 | Args:
174 | pred_disp: Shape is (Batch, 1, H, W)
175 | """
176 | def gradient(pred):
177 | D_dy = pred[:, :, 1:] - pred[:, :, :-1]
178 | D_dx = pred[:, :, :, 1:] - pred[:, :, :, :-1]
179 | return D_dx, D_dy
180 |
181 | dx, dy = gradient(pred_disp)
182 | dx2, dxdy = gradient(dx)
183 | dydx, dy2 = gradient(dy)
184 | return F.mean(F.absolute(dx2)) + F.mean(F.absolute(dxdy)) \
185 | + F.mean(F.absolute(dydx)) + F.mean(F.absolute(dy2))
186 |
187 | def inference(self, tgt_img, src_imgs, intrinsics, inv_intrinsics,
188 | is_depth=True, is_pose=True, is_exp=True):
189 | pred_depth, pred_pose, pred_mask = 0, 0, 0
190 | with chainer.using_config('train', False), \
191 | chainer.function.no_backprop_mode():
192 | # #start, stop = create_timer()
193 | if is_depth:
194 | pred_depth = 1 / self.disp_net(tgt_img)[0]
195 |
196 | if is_pose:
197 | batchsize, n_sources, _, H, W = src_imgs.shape
198 | stacked_src_imgs = self.xp.reshape(src_imgs,
199 | (batchsize, -1, H, W))
200 | pred_pose, pred_mask = self.pose_net(tgt_img,
201 | stacked_src_imgs,
202 | do_exp=is_exp)
203 | # #print_timer(#start, stop, sentence="Inference Time")
204 | return pred_depth, pred_pose, pred_mask
205 |
--------------------------------------------------------------------------------
/models/disp_net.py:
--------------------------------------------------------------------------------
1 | # Original implementation with TF : https://github.com/tinghuiz/SfMLearner/blob/master/nets.py
2 | import chainer
3 | from chainer import functions as F
4 | from chainer import links as L
5 |
6 | # Range of disparity/inverse depth values
7 | DISP_SCALING = 10
8 | MIN_DISP = 0.01
9 |
10 |
11 | def resize_like(inputs, ref):
12 | if inputs.shape[2] == ref.shape[2] and inputs.shape[3] == ref.shape[3]:
13 | return inputs
14 | return F.resize_images(inputs, ref.shape[2:])
15 |
16 |
17 | class DispNet(chainer.Chain):
18 | def __init__(self, activation=F.relu):
19 | super(DispNet, self).__init__()
20 | self.activation = activation
21 | with self.init_scope():
22 | self.c1 = L.Convolution2D(None, 32, ksize=7, stride=2, pad=3)
23 | self.c1b = L.Convolution2D(None, 32, ksize=7, stride=1, pad=3)
24 | self.c2 = L.Convolution2D(None, 64, ksize=5, stride=2, pad=2)
25 | self.c2b = L.Convolution2D(None, 64, ksize=5, stride=1, pad=2)
26 | self.c3 = L.Convolution2D(None, 128, ksize=3, stride=2, pad=1)
27 | self.c3b = L.Convolution2D(None, 128, ksize=3, stride=1, pad=1)
28 | self.c4 = L.Convolution2D(None, 256, ksize=3, stride=2, pad=1)
29 | self.c4b = L.Convolution2D(None, 256, ksize=3, stride=1, pad=1)
30 | self.c5 = L.Convolution2D(None, 512, ksize=3, stride=2, pad=1)
31 | self.c5b = L.Convolution2D(None, 512, ksize=3, stride=1, pad=1)
32 | self.c6 = L.Convolution2D(None, 512, ksize=3, stride=2, pad=1)
33 | self.c6b = L.Convolution2D(None, 512, ksize=3, stride=1, pad=1)
34 | self.c7 = L.Convolution2D(None, 512, ksize=3, stride=2, pad=1)
35 | self.c7b = L.Convolution2D(None, 512, ksize=3, stride=1, pad=1)
36 |
37 | self.dc7 = L.Deconvolution2D(None, 512, ksize=4, stride=2, pad=1)
38 | self.idc7 = L.Convolution2D(None, 512, ksize=3, stride=1, pad=1)
39 | self.dc6 = L.Deconvolution2D(None, 512, ksize=4, stride=2, pad=1)
40 | self.idc6 = L.Convolution2D(None, 512, ksize=3, stride=1, pad=1)
41 | self.dc5 = L.Deconvolution2D(None, 256, ksize=4, stride=2, pad=1)
42 | self.idc5 = L.Convolution2D(None, 256, ksize=3, stride=1, pad=1)
43 | self.dc4 = L.Deconvolution2D(None, 128, ksize=4, stride=2, pad=1)
44 | self.idc4 = L.Convolution2D(None, 128, ksize=3, stride=1, pad=1)
45 | self.dispout4 = L.Convolution2D(None, 1, ksize=3, stride=1, pad=1)
46 | self.dc3 = L.Deconvolution2D(None, 64, ksize=4, stride=2, pad=1)
47 | self.idc3 = L.Convolution2D(None, 64, ksize=3, stride=1, pad=1)
48 | self.dispout3 = L.Convolution2D(None, 1, ksize=3, stride=1, pad=1)
49 | self.dc2 = L.Deconvolution2D(None, 32, ksize=4, stride=2, pad=1)
50 | self.idc2 = L.Convolution2D(None, 32, ksize=3, stride=1, pad=1)
51 | self.dispout2 = L.Convolution2D(None, 1, ksize=3, stride=1, pad=1)
52 | self.dc1 = L.Deconvolution2D(None, 16, ksize=4, stride=2, pad=1)
53 | self.idc1 = L.Convolution2D(None, 16, ksize=3, stride=1, pad=1)
54 | self.dispout1 = L.Convolution2D(None, 1, ksize=3, stride=1, pad=1)
55 |
56 | def __call__(self, x_target):
57 | # x_target: chainer.Variable of shape = [N, 3, H, W]
58 | # There might be dimension mismatch due to uneven down/up-sampling
59 | H, W = x_target.shape[2:]
60 | normalizer = lambda z: z
61 | h = x_target
62 | h = self.activation(normalizer(self.c1(h)))
63 | h = self.activation(normalizer(self.c1b(h)))
64 | h_c1b = h
65 | h = self.activation(normalizer(self.c2(h)))
66 | h = self.activation(normalizer(self.c2b(h)))
67 | h_c2b = h
68 | h = self.activation(normalizer(self.c3(h)))
69 | h = self.activation(normalizer(self.c3b(h)))
70 | h_c3b = h
71 | h = self.activation(normalizer(self.c4(h)))
72 | h = self.activation(normalizer(self.c4b(h)))
73 | h_c4b = h
74 | h = self.activation(normalizer(self.c5(h)))
75 | h = self.activation(normalizer(self.c5b(h)))
76 | h_c5b = h
77 | h = self.activation(normalizer(self.c6(h)))
78 | h = self.activation(normalizer(self.c6b(h)))
79 | h_c6b = h
80 | h = self.activation(normalizer(self.c7(h)))
81 | h = self.activation(normalizer(self.c7b(h)))
82 |
83 | h = self.activation(normalizer(self.dc7(h)))
84 | # There might be dimension mismatch due to uneven down/up-sampling
85 | # Resize by bilinear interpolation.
86 | # (by nearest neighbor sampling in the original implemntation.)
87 | h = resize_like(h, h_c6b)
88 | h = F.concat([h, h_c6b], axis=1)
89 | h = self.activation(normalizer(self.idc7(h)))
90 |
91 | h = self.activation(normalizer(self.dc6(h)))
92 | h = resize_like(h, h_c5b)
93 | h = F.concat([h, h_c5b], axis=1)
94 | h = self.activation(normalizer(self.idc6(h)))
95 |
96 | h = self.activation(normalizer(self.dc5(h)))
97 | h = resize_like(h, h_c4b)
98 | h = F.concat([h, h_c4b], axis=1)
99 | h = self.activation(normalizer(self.idc5(h)))
100 |
101 | h = self.activation(normalizer(self.dc4(h)))
102 | h = F.concat([h, h_c3b], axis=1)
103 | h = self.activation(normalizer(self.idc4(h)))
104 | disp4 = DISP_SCALING * F.sigmoid(self.dispout4(h)) + MIN_DISP
105 | disp4_up = F.resize_images(disp4, (H // 4, W // 4))
106 |
107 | h = self.activation(normalizer(self.dc3(h)))
108 | h = F.concat([h, h_c2b, disp4_up], axis=1)
109 | h = self.activation(normalizer(self.idc3(h)))
110 | disp3 = DISP_SCALING * F.sigmoid(self.dispout3(h)) + MIN_DISP
111 | disp3_up = F.resize_images(disp3, (H // 2, W // 2))
112 |
113 | h = self.activation(normalizer(self.dc2(h)))
114 | h = F.concat([h, h_c1b, disp3_up], axis=1)
115 | h = self.activation(normalizer(self.idc2(h)))
116 | disp2 = DISP_SCALING * F.sigmoid(self.dispout2(h)) + MIN_DISP
117 | disp2_up = F.resize_images(disp2, (H, W))
118 |
119 | h = self.activation(normalizer(self.dc1(h)))
120 | h = F.concat([h, disp2_up], axis=1)
121 | h = self.activation(normalizer(self.idc1(h)))
122 | disp1 = DISP_SCALING * F.sigmoid(self.dispout1(h)) + MIN_DISP
123 |
124 | return [disp1, disp2, disp3, disp4]
125 |
--------------------------------------------------------------------------------
/models/pose_net.py:
--------------------------------------------------------------------------------
1 | # Original implementation with TF : https://github.com/tinghuiz/SfMLearner/blob/master/nets.py
2 | import chainer
3 | from chainer import functions as F
4 | from chainer import links as L
5 |
6 |
7 | class PoseNet(chainer.Chain):
8 | def __init__(self, n_sources=2, activtion=F.relu):
9 | super(PoseNet, self, ).__init__()
10 | self.activation = activtion
11 | self.n_sources = n_sources
12 | with self.init_scope():
13 | self.c1 = L.Convolution2D(None, 16, ksize=7, stride=2, pad=3)
14 | self.c2 = L.Convolution2D(None, 32, ksize=5, stride=2, pad=2)
15 | self.c3 = L.Convolution2D(None, 64, ksize=3, stride=2, pad=1)
16 | self.c4 = L.Convolution2D(None, 128, ksize=3, stride=2, pad=1)
17 | self.c5 = L.Convolution2D(None, 256, ksize=3, stride=2, pad=1)
18 |
19 | self.pose1 = L.Convolution2D(None, 256, ksize=3, stride=2, pad=1)
20 | self.pose2 = L.Convolution2D(None, 256, ksize=3, stride=2, pad=1)
21 | self.poseout = L.Convolution2D(None, self.n_sources * 6, ksize=1,
22 | stride=1, pad=0)
23 |
24 | self.exp5 = L.Deconvolution2D(None, 256, ksize=4, stride=2, pad=1)
25 | self.exp4 = L.Deconvolution2D(None, 128, ksize=4, stride=2, pad=1)
26 | self.expout4 = L.Convolution2D(None, self.n_sources, ksize=3,
27 | pad=1)
28 | self.exp3 = L.Deconvolution2D(None, 64, ksize=4, stride=2, pad=1)
29 | self.expout3 = L.Convolution2D(None, self.n_sources, ksize=3,
30 | pad=1)
31 | self.exp2 = L.Deconvolution2D(None, 32, ksize=6, stride=2, pad=2)
32 | self.expout2 = L.Convolution2D(None, self.n_sources, ksize=5,
33 | pad=2, stride=1)
34 | self.exp1 = L.Deconvolution2D(None, 16, ksize=6, stride=2, pad=2)
35 | self.expout1 = L.Convolution2D(None, self.n_sources, ksize=7,
36 | pad=3, stride=1)
37 |
38 | def encode(self, x):
39 | h = self.activation(self.c1(x))
40 | h = self.activation(self.c2(h))
41 | h = self.activation(self.c3(h))
42 | h = self.activation(self.c4(h))
43 | h = self.activation(self.c5(h))
44 | return h
45 |
46 | def pred_pose(self, x):
47 | h = self.activation(self.pose1(x))
48 | h = self.activation(self.pose2(h))
49 | h = self.poseout(h)
50 | # Empirically the authors found that scaling by a small constant
51 | # facilitates training.
52 | h = 0.01 * F.mean(h, (2, 3)) # Batch, 6 * n_sources
53 | h = F.split_axis(h, self.n_sources, 1)
54 | return h
55 |
56 | def pred_expalanation(self, x):
57 | h = x
58 | h = self.activation(self.exp5(h))
59 | h = self.activation(self.exp4(h))
60 | mask4 = self.expout4(h)
61 | h = self.activation(self.exp3(h))
62 | mask3 = self.expout3(h)
63 | h = self.activation(self.exp2(h))
64 | mask2 = self.expout2(h)
65 | h = self.activation(self.exp1(h))
66 | mask1 = self.expout1(h)
67 | return [mask1, mask2, mask3, mask4]
68 |
69 | def __call__(self, x_target, x_sources, do_exp=True):
70 | """
71 | x_target: chainer.Variable of shape [N, 3, H, W]
72 | x_sources: chainer.Variable of shape [N, 3 * Source, H, W]
73 | """
74 | h = F.concat([x_target, x_sources], axis=1)
75 | h = self.encode(h)
76 | poses = self.pred_pose(h)
77 | if do_exp:
78 | masks = self.pred_expalanation(h)
79 | return poses, masks
80 | else:
81 | return poses, None
82 |
--------------------------------------------------------------------------------
/models/spational_transformer_sampler_interp.py:
--------------------------------------------------------------------------------
1 | import numpy
2 |
3 | import chainer
4 | from chainer import function
5 | from chainer.utils import argument
6 | from chainer.utils import type_check
7 | from chainer import cuda
8 |
9 | class SpatialTransformerSamplerInterp(function.Function):
10 |
11 | def check_type_forward(self, in_types):
12 | n_in = in_types.size()
13 | type_check.expect(2 == n_in)
14 |
15 | x_type = in_types[0]
16 | grid_type = in_types[1]
17 | type_check.expect(
18 | x_type.dtype.char == 'f',
19 | grid_type.dtype.char == 'f',
20 | x_type.ndim == 4,
21 | grid_type.ndim == 4,
22 | grid_type.shape[1] == 2,
23 | x_type.shape[0] == grid_type.shape[0],
24 | )
25 |
26 | def forward_cpu(self, inputs):
27 | return self._forward(inputs)
28 |
29 | def forward_gpu(self, inputs):
30 | return self._forward(inputs)
31 |
32 | def _forward(self, inputs):
33 | x, grid = inputs
34 | xp = cuda.get_array_module(x)
35 | B, C, H, W = x.shape
36 | _, _, out_H, out_W = grid.shape
37 |
38 | u = grid[:, 0].reshape(-1)
39 | v = grid[:, 1].reshape(-1)
40 |
41 | u0 = xp.floor(u)
42 | u1 = u0 + 1
43 | v0 = xp.floor(v)
44 | v1 = v0 + 1
45 |
46 | u0 = u0.clip(0, W - 1)
47 | v0 = v0.clip(0, H - 1)
48 | u1 = u1.clip(0, W - 1)
49 | v1 = v1.clip(0, H - 1)
50 |
51 | # weights
52 | wt_x0 = u1 - u
53 | wt_x1 = u - u0
54 | wt_y0 = v1 - v
55 | wt_y1 = v - v0
56 |
57 | w1 = wt_x0 * wt_y0
58 | w2 = wt_x1 * wt_y0
59 | w3 = wt_x0 * wt_y1
60 | w4 = wt_x1 * wt_y1
61 | w1 = w1.astype(x.dtype)
62 | w2 = w2.astype(x.dtype)
63 | w3 = w3.astype(x.dtype)
64 | w4 = w4.astype(x.dtype)
65 |
66 | u0 = u0.astype(numpy.int32)
67 | v0 = v0.astype(numpy.int32)
68 | u1 = u1.astype(numpy.int32)
69 | v1 = v1.astype(numpy.int32)
70 |
71 | batch_index = xp.repeat(xp.arange(B), out_H * out_W)
72 | y = w1[:, None] * x[batch_index, :, v0, u0]
73 | y += w2[:, None] * x[batch_index, :, v0, u1]
74 | y += w3[:, None] * x[batch_index, :, v1, u0]
75 | y += w4[:, None] * x[batch_index, :, v1, u1]
76 |
77 | y = y.reshape(B, out_H, out_W, C).transpose(0, 3, 1, 2)
78 | return y,
79 |
80 | def backward_cpu(self, inputs, grad_outputs):
81 | return self._backward(inputs, grad_outputs)
82 |
83 | def backward_gpu(self, inputs, grad_outputs):
84 | return self._backward(inputs, grad_outputs)
85 |
86 | def _backward(self, inputs, grad_outputs):
87 | x, grid = inputs
88 | xp = cuda.get_array_module(x)
89 | gy, = grad_outputs
90 | B, C, H, W = x.shape
91 | _, _, out_H, out_W = grid.shape
92 |
93 | u = grid[:, 0].reshape(-1)
94 | v = grid[:, 1].reshape(-1)
95 |
96 | # indices of the 2x2 pixel neighborhood surrounding the coordinates
97 | u0 = xp.floor(u)
98 | u1 = u0 + 1
99 | v0 = xp.floor(v)
100 | v1 = v0 + 1
101 |
102 | u0 = u0.clip(0, W - 1)
103 | v0 = v0.clip(0, H - 1)
104 | u1 = u1.clip(0, W - 1)
105 | v1 = v1.clip(0, H - 1)
106 |
107 | # weights
108 | wt_x0 = u1 - u
109 | wt_x1 = u - u0
110 | wt_y0 = v1 - v
111 | wt_y1 = v - v0
112 |
113 | wt_x0 = wt_x0.astype(gy.dtype)
114 | wt_x1 = wt_x1.astype(gy.dtype)
115 | wt_y0 = wt_y0.astype(gy.dtype)
116 | wt_y1 = wt_y1.astype(gy.dtype)
117 |
118 | u0 = u0.astype(numpy.int32)
119 | v0 = v0.astype(numpy.int32)
120 | u1 = u1.astype(numpy.int32)
121 | v1 = v1.astype(numpy.int32)
122 |
123 | batch_index = xp.repeat(xp.arange(B), out_H * out_W)
124 | x_indexed_1 = x[batch_index, :, v0, u0]
125 | x_indexed_2 = x[batch_index, :, v0, u1]
126 | x_indexed_3 = x[batch_index, :, v1, u0]
127 | x_indexed_4 = x[batch_index, :, v1, u1]
128 |
129 | gu = -wt_y0[:, None] * x_indexed_1
130 | gu += wt_y0[:, None] * x_indexed_2
131 | gu -= wt_y1[:, None] * x_indexed_3
132 | gu += wt_y1[:, None] * x_indexed_4
133 |
134 | gv = -wt_x0[:, None] * x_indexed_1
135 | gv -= wt_x1[:, None] * x_indexed_2
136 | gv += wt_x0[:, None] * x_indexed_3
137 | gv += wt_x1[:, None] * x_indexed_4
138 |
139 | gu = gu.reshape(B, out_H, out_W, C).transpose(0, 3, 1, 2)
140 | gv = gv.reshape(B, out_H, out_W, C).transpose(0, 3, 1, 2)
141 |
142 | gu *= gy
143 | gv *= gy
144 | gu = xp.sum(gu, axis=1)
145 | gv = xp.sum(gv, axis=1)
146 | # Offsets scaling of the coordinates and clip gradients.
147 | ggrid = xp.concatenate((gu[:, None], gv[:, None]), axis=1)
148 | gx = xp.zeros_like(x)
149 | return gx, ggrid
150 |
151 |
152 | def spatial_transformer_sampler_interp(x, grid, **kwargs):
153 | argument.check_unexpected_kwargs(
154 | kwargs, use_cudnn="The argument \"use_cudnn\" is not "
155 | "supported anymore. "
156 | "Use chainer.using_config('use_cudnn', value) "
157 | "context where value can be `always`, `never`, or `auto`.")
158 | argument.assert_kwargs_empty(kwargs)
159 | return SpatialTransformerSamplerInterp()(x, grid)
160 |
--------------------------------------------------------------------------------
/models/transform.py:
--------------------------------------------------------------------------------
1 | # Transformation from target view to source
2 | # See https://github.com/tinghuiz/SfMLearner/blob/master/utils.py
3 | import chainer
4 | from chainer import functions as F
5 | from chainer import links as L
6 | from chainer import cuda
7 | import numpy as np
8 | from models import utils
9 | from models.utils import *
10 |
11 | def euler2mat(r, xp=np):
12 | """Converts euler angles to rotation matrix
13 |
14 | Args:
15 | r: rotation angle(x, y, z). Shape is (N, 3).
16 | Returns:
17 | Rotation matrix corresponding to the euler angles. Shape is (N, 3, 3).
18 | """
19 | batchsize = r.shape[0]
20 | # start, stop = create_timer()
21 | zeros = xp.zeros((batchsize), dtype='f')
22 | ones = xp.ones((batchsize), dtype='f')
23 | r = F.clip(r, -np.pi, np.pi)
24 | cos_r = F.cos(r)
25 | sin_r = F.sin(r)
26 |
27 | zmat = F.stack([cos_r[:, 2], -sin_r[:, 2], zeros,
28 | sin_r[:, 2], cos_r[:, 2], zeros,
29 | zeros, zeros, ones], axis=1).reshape(batchsize, 3, 3)
30 |
31 | ymat = F.stack([cos_r[:, 1], zeros, sin_r[:, 1],
32 | zeros, ones, zeros,
33 | -sin_r[:, 1], zeros, cos_r[:, 1]], axis=1).reshape(batchsize, 3, 3)
34 |
35 | xmat = F.stack([ones, zeros, zeros,
36 | zeros, cos_r[:, 0], -sin_r[:, 0],
37 | zeros, sin_r[:, 0], cos_r[:, 0]], axis=1).reshape(batchsize, 3, 3)
38 | # #print_timer(start, stop, 'create matrix')
39 | rotMat = F.batch_matmul(F.batch_matmul(xmat, ymat), zmat)
40 | return rotMat
41 |
42 |
43 | def pose_vec2mat(vec, filler, xp=np):
44 | """Converts 6DoF parameters to transformation matrix
45 |
46 | Args:
47 | vec: 6DoF parameters in the order of rx, ry, rz, tx, ty, tz -- [N, 6]
48 | Returns:
49 | A transformation matrix -- [N, 4, 4]
50 | """
51 | # start, stop = create_timer()
52 | r, t = vec[:, :3], vec[:, 3:]
53 | rot_mat = euler2mat(r, xp=xp)
54 | # print_timer(start, stop, 'euler2mat')
55 | batch_size = rot_mat.shape[0]
56 | t = t.reshape(batch_size, 3, 1)
57 | transform_mat = F.dstack((rot_mat, t))
58 | transform_mat = F.hstack((transform_mat, filler))
59 | return transform_mat
60 |
61 |
62 | filler = None
63 |
64 | def proj_tgt_to_src(vec, K, N, xp=np, use_cpu=True):
65 | """Projection matrix from target to sources.
66 |
67 | Args:
68 | vec(Variable): Shape is (N, 6).
69 | K(array): Shape is (N, 3, 3).
70 | N(int): Batch size.
71 |
72 | Returns:
73 | Variable: Projection matrix.
74 | """
75 | is_transfer = False
76 | if xp != np and use_cpu:
77 | vec = gpu2cpu(vec)
78 | K = chainer.cuda.to_cpu(K)
79 | xp = np
80 | is_transfer = True
81 |
82 | global filler
83 | if filler is None or filler.shape[0] != N:
84 | filler = xp.tile(xp.asarray([0.0, 0.0, 0.0, 1.0], 'f').reshape(1, 1, 4),
85 | [N, 1, 1])
86 | K_ = F.concat([F.concat([K, xp.zeros([N, 3, 1], 'f')], axis=2), filler], axis=1)
87 | poses = pose_vec2mat(vec, filler, xp)
88 | proj_tgt_cam_to_src_pixel = F.batch_matmul(K_, poses)
89 | if is_transfer:
90 | proj_tgt_cam_to_src_pixel = cpu2gpu(proj_tgt_cam_to_src_pixel)
91 | return proj_tgt_cam_to_src_pixel
92 |
93 |
94 | def pixel2cam(depthes, pixel_coords, intrinsics, im_shape, xp=np):
95 | """Converter from pixel coordinates to camera coordinates.
96 |
97 | Args:
98 | depthes(Variable): Shape is (N, 3, H*W)
99 | pixel_coords(array): Shape is (N, 3, H*W)
100 | intrinsics(array): Shape is (N, 3, 3)
101 | Returns:
102 | cam_coords(Variable): Shape is (N, 3, H*W)
103 | """
104 | N, _, H, W = im_shape
105 | cam_coords = F.batch_matmul(F.batch_inv(intrinsics),
106 | pixel_coords)
107 | cam_coords = depthes * cam_coords
108 | cam_coords = F.concat((cam_coords, xp.ones((N, 1, H*W), 'f')), axis=1)
109 | return cam_coords
110 |
111 | def cam2pixel(cam_coords, proj, im_shape, xp=np):
112 | """Conveter from camera coordinates to pixel coordinates.
113 |
114 | Args:
115 | cam_coords(Variable): Shape is (N, 4, H*W).
116 | proj(Variable): Shape is (N, 4, 4).
117 |
118 | Returns:
119 | Variable: Pixel coordinates.
120 | """
121 | N, _, H, W = im_shape
122 | unnormalized_pixel_coords = F.batch_matmul(proj, cam_coords)
123 | z = unnormalized_pixel_coords[:, 2:3, :] + 1e-10
124 | p_s_x = (unnormalized_pixel_coords[:, 0:1] / z) / ((W - 1) / 2.) - 1
125 | p_s_y = (unnormalized_pixel_coords[:, 1:2] / z) / ((H - 1) / 2.) - 1
126 | p_s_xy = F.concat((p_s_x, p_s_y), axis=1)
127 |
128 | mask = xp.ones_like(p_s_xy.data)
129 | is_inside = (p_s_xy.data > -1) * (p_s_xy.data < 1)
130 | mask[~is_inside] = 2
131 | p_s_xy *= mask
132 | p_s_xy = F.reshape(p_s_xy, (N, 2, H, W))
133 | return p_s_xy
134 |
135 | meshgrid = None
136 |
137 | def generate_2dmeshgrid(H, W, N, xp=np):
138 | """Generate 2d meshgrid.
139 |
140 | Returns:
141 | Array: Shape is (N, 3, H*W)
142 | """
143 | global meshgrid
144 | if meshgrid is None or meshgrid.shape[2] != H*W:
145 | ys, xs = xp.meshgrid(
146 | xp.arange(0, H, dtype=np.float32),
147 | xp.arange(0, W, dtype=np.float32), indexing='ij',
148 | copy=False
149 | )
150 | meshgrid = xp.concatenate(
151 | [xs[None], ys[None], xp.ones((1, H, W), dtype=np.float32)],
152 | axis=0).reshape(1, 3, H*W)
153 | meshgrid = F.broadcast_to(meshgrid, (N, 3, H*W))
154 | return meshgrid
155 |
156 | def projective_inverse_warp(imgs, depthes, poses, K):
157 | """
158 | Args:
159 | imgs(Variable): Source images. Shape is [N, 3, H, W]
160 | depthes(Variable): Predicted depthes. Shape is [N, 3, H*W]
161 | poses(Variable): Predicted poses. Shape is [N, 6]
162 | K(array): [N, 3, 3]
163 | Return:
164 | transformed images of shape [N, 3, H, W]
165 | """
166 | xp = cuda.get_array_module(imgs)
167 | im_shape = imgs.shape
168 | N, _, H, W = im_shape
169 |
170 | # start, stop = create_timer()
171 | proj_tgt_cam_to_src_pixel = proj_tgt_to_src(poses, K, N, xp=xp,
172 | use_cpu=True)
173 | # print_timer(start, stop, 'proj')
174 |
175 | # start, stop = create_timer()
176 | pixel_coords = generate_2dmeshgrid(H, W, N, xp)
177 | # print_timer(start, stop, 'mesh grid')
178 |
179 | # start, stop = create_timer()
180 | cam_coords = pixel2cam(depthes, pixel_coords, K, im_shape, xp=xp)
181 | # print_timer(start, stop, 'pixel2cam')
182 |
183 | # start, stop = create_timer()
184 | src_pixel_coords = cam2pixel(cam_coords, proj_tgt_cam_to_src_pixel,
185 | im_shape, xp=xp)
186 | # print_timer(start, stop, 'cam2pixel')
187 |
188 | # start, stop = create_timer()
189 | transformed_img = F.spatial_transformer_sampler(imgs, src_pixel_coords)
190 | # transformed_img = spatial_transformer_sampler(imgs, src_pixel_coords)
191 | # transformed_img = spatial_transformer_sampler_interp(imgs, src_pixel_coords)
192 | # print_timer(start, stop, 'spatial transformer')
193 | return transformed_img
194 |
--------------------------------------------------------------------------------
/models/utils.py:
--------------------------------------------------------------------------------
1 | import numpy
2 | import six
3 |
4 | import chainer
5 | from chainer import configuration
6 | from chainer import cuda
7 | try:
8 | from chainer import function_node
9 | ParentClass = function_node.FunctionNode
10 | except:
11 | from chainer import function
12 | ParentClass = function.Function
13 | from chainer.utils import argument
14 | from chainer.utils import type_check
15 |
16 | def create_timer():
17 | start = chainer.cuda.Event()
18 | stop = chainer.cuda.Event()
19 | start.synchronize()
20 | start.record()
21 | return start, stop
22 |
23 | def print_timer(start, stop, sentence=None):
24 | stop.record()
25 | stop.synchronize()
26 | elapsed_time = chainer.cuda.cupy.cuda.get_elapsed_time(
27 | start, stop) / 1000
28 | if sentence is not None:
29 | print(sentence, elapsed_time)
30 | return elapsed_time
31 |
32 |
33 | class CPU2GPU(ParentClass):
34 |
35 | """CPU2GPU"""
36 |
37 | def __init__(self):
38 | pass
39 |
40 | def check_type_forward(self, in_types):
41 | pass
42 |
43 | def forward(self, inputs):
44 | x = inputs[0]
45 | x = chainer.cuda.to_gpu(x)
46 | return x,
47 |
48 | def backward(self, x, gy):
49 | out = gy[0]
50 | out.to_cpu()
51 | return out,
52 | # return gy[0][self.batch_indexes, :, self.d_indexes, self.h_indexes, self.w_indexes],
53 |
54 |
55 | def cpu2gpu(x):
56 | """CPU to GPU function."""
57 | return CPU2GPU().apply((x,))[0]
58 |
59 |
60 | class GPU2CPU(ParentClass):
61 |
62 | """GPU2CPU"""
63 |
64 | def __init__(self):
65 | pass
66 |
67 | def check_type_forward(self, in_types):
68 | pass
69 |
70 | def forward(self, inputs):
71 | x = inputs[0]
72 | x = chainer.cuda.to_cpu(x)
73 | return x,
74 |
75 | def backward(self, x, gy):
76 | out = gy[0]
77 | out.to_gpu()
78 | return out,
79 | # return gy[0][self.batch_indexes, :, self.d_indexes, self.h_indexes, self.w_indexes],
80 |
81 |
82 | def gpu2cpu(x):
83 | """GPU to CPU function."""
84 | return GPU2CPU().apply((x,))[0]
85 |
--------------------------------------------------------------------------------
/requirements.txt:
--------------------------------------------------------------------------------
1 | chainer==4.0.0b1
2 | chainercv==0.8.0
3 | cupy==4.0.0b1
4 | Cython==0.23.4
5 | matplotlib==1.5.1
6 | numpy==1.13.3
7 | opencv-python==3.2.0.8
8 | pandas==0.20.3
9 | pep8==1.7.0
10 |
--------------------------------------------------------------------------------
/train.py:
--------------------------------------------------------------------------------
1 | #!/usr/env/bin python3
2 | # -*- coding: utf-8 -*-
3 |
4 | import argparse
5 | import numpy as np
6 | import sys
7 | import subprocess
8 | import os
9 | import yaml
10 |
11 | import chainer
12 | from chainer import cuda, optimizers, serializers
13 | from chainer import training
14 |
15 | from config_utils import *
16 |
17 | chainer.cuda.set_max_workspace_size(1024 * 1024 * 1024)
18 | os.environ["CHAINER_TYPE_CHECK"] = "0"
19 |
20 | from collections import OrderedDict
21 | yaml.add_constructor(yaml.resolver.BaseResolver.DEFAULT_MAPPING_TAG,
22 | lambda loader, node: OrderedDict(loader.construct_pairs(node)))
23 |
24 | def train_sfm_learner():
25 | """Training VoxelNet."""
26 | config = parse_args()
27 | model = get_model(config["model"])
28 | devices = parse_devices(config['gpus'], config['updater']['name'])
29 | train_data, test_data = load_dataset(config["dataset"])
30 | train_iter, test_iter = create_iterator(train_data, test_data,
31 | config['iterator'], devices,
32 | config['updater']['name'])
33 |
34 | optimizer = create_optimizer(config['optimizer'], model)
35 | updater = create_updater(train_iter, optimizer, config['updater'], devices)
36 | trainer = training.Trainer(updater, config['end_trigger'], out=config['results'])
37 | trainer = create_extension(trainer, test_iter, model,
38 | config['extension'], devices=devices)
39 | trainer.run()
40 | chainer.serializers.save_npz(os.path.join(config['results'], 'model.npz'),
41 | model)
42 |
43 | def main():
44 | train_sfm_learner()
45 |
46 | if __name__ == '__main__':
47 | main()
48 |
--------------------------------------------------------------------------------