<jupyter_start><jupyter_text>**General**  Mount<jupyter_code>from google.colab import drive 
drive.mount('/content/gdrive')<jupyter_output><empty_output><jupyter_text>Import Packages<jupyter_code>%tensorflow_version 1.x
from keras.models import Sequential
from keras.layers.core import Flatten, Dense, Dropout, Reshape, Lambda
from keras.layers.convolutional import Conv2D, Convolution2D, MaxPooling2D, ZeroPadding2D
from keras.optimizers import Adam
import tensorflow as tf
from keras import backend as K
from IPython.display import SVG
from keras.utils.vis_utils import model_to_dot
from keras.layers.advanced_activations import LeakyReLU
from keras.layers import Input, Dense
from keras.models import Model
import matplotlib.pyplot as plt
from keras.callbacks import TensorBoard, EarlyStopping, ModelCheckpoint
import copy
import cv2, os
import numpy as np
from random import shuffle 
from keras.applications.vgg16 import VGG16 
from keras.applications.mobilenet_v2 import MobileNetV2
import pickle


#From the source - ignore 
get_ipython().magic(u'matplotlib inline')<jupyter_output><empty_output><jupyter_text>You may have to run this for using the MobileNetV2 for 3D network's feature extractor on Google Colab. You may also need  to restart session. (not sure)<jupyter_code>pip install 'h5py==2.10.0' --force-reinstall<jupyter_output><empty_output><jupyter_text>Download the Dataset  Download images<jupyter_code>!wget https://s3.eu-central-1.amazonaws.com/avg-kitti/data_object_image_2.zip
!unzip data_object_image_2.zip
!ls -l training/image_2 | wc -l<jupyter_output><empty_output><jupyter_text>Download labels<jupyter_code>!wget https://s3.eu-central-1.amazonaws.com/avg-kitti/data_object_label_2.zip
! unzip data_object_label_2.zip
!ls -l training/label_2 | wc -l<jupyter_output><empty_output><jupyter_text>Delete the zip files to save space on Colab<jupyter_code>rm -rf data_object_image_2.zip data_object_label_2.zip<jupyter_output><empty_output><jupyter_text>Setup Tensorboard You may want to change the logdir depending on your training setup<jupyter_code>%load_ext tensorboard
%tensorboard --logdir /content/output_eval

# If you want to kill tensorboard session:
# !kill [PID]<jupyter_output><empty_output><jupyter_text>Global Parameters  The Global parameters used for the 3D part<jupyter_code># This is for the orientation estimation.
BIN, OVERLAP = 6, 0.1

W = 1.
# The crop shapes for the input 3D network.
NORM_H, NORM_W = 224, 224

# Maximum bounding box jitter for data augmentation while training 3D network
MAX_JIT=5

# Classes to be considered and extracted from kitti dataset, while training the 3D network
VEHICLES = ['Car', 'Truck', 'Van']

# These are used to filter out useless and bad objects from dataset (for training)
MAX_OCCLUSION =1 # anything <= will be included.
MAX_TRUNCATION = 0.8 # anything < will be included.

# Path to the images and labels dir.
image_dir = 'training/image_2/'
label_dir = 'training/label_2/'

FEATURE_EXTRACTOR_3D = 'mobilenetv2' #OR vgg16<jupyter_output><empty_output><jupyter_text>Utils (compute_anchors, prepare_input_and_output, data_gen l2_normalize, orientation loss)<jupyter_code>def compute_anchors(angle):
    anchors = []
    
    wedge = 2.*np.pi/BIN  #Length of each bin in Radian.

    # Each angle will lie somewhere in a bin. But we need to find the closest. That is, given a bin, we want to know which bound is closer to our angle.
    # For instance if our bin is (pi/2, pi), we want to know whether the angle is easier to reference from pi/2 or pi.
    # So we keep both lower and upper bound using l_index and r_index, respectively.
    l_index = int(angle/wedge)
    r_index = l_index + 1
    
    # Now we check if our angle is more closer to current bin's lower bound, or its upper bound.

    # If close enough to the lower bound, consider current index..
    if (angle - l_index*wedge) < wedge/2 * (1+OVERLAP/2):
        anchors.append([l_index, angle - l_index*wedge])
    # If close enough to upper bound,
    if (r_index*wedge - angle) < wedge/2 * (1+OVERLAP/2):
        anchors.append([r_index%BIN, angle - r_index*wedge])
        
    return anchors

def prepare_input_and_output(train_inst):

    # Read the image.
    img = cv2.imread(image_dir + train_inst['image'])

    # Read AND jitter each bounding box
    xmin = train_inst['xmin'] + np.random.randint(-MAX_JIT, MAX_JIT+1)
    ymin = train_inst['ymin'] + np.random.randint(-MAX_JIT, MAX_JIT+1)
    xmax = train_inst['xmax'] + np.random.randint(-MAX_JIT, MAX_JIT+1)
    ymax = train_inst['ymax'] + np.random.randint(-MAX_JIT, MAX_JIT+1)

    # Ensure that your coordinates are withing ranges after jittering.
    shape = img.shape
    xmin = max(min(xmin, shape[1]-1),0)
    xmax = max(min(xmax, shape[1]-1),0)
    ymin = max(min(ymin, shape[0]-1),0)
    ymax = max(min(ymax, shape[0]-1),0)
    
    # Crop the frame.
    img = copy.deepcopy(img[ymin:ymax+1,xmin:xmax+1]).astype(np.float32)

    # Randomly decide whether to flip or not.
    flip = np.random.binomial(1, .5)
    if flip > 0.5: img = cv2.flip(img, 1)
        
    # resize the patch to standard size
    img = cv2.resize(img, (NORM_H, NORM_W))

    # Make pixel values [-0.5 to 0.5].
    img = img / 255.0 - 0.5
    
    #Return data but take care of returining proper orientation (will change if image is flipped).
    if flip > 0.5:
        return img, train_inst['dims'], train_inst['orient_flipped'], train_inst['conf_flipped']
    else:
        return img, train_inst['dims'], train_inst['orient'], train_inst['conf']

def data_gen(all_objs, batch_size):

    # Total objects (patches) that we have
    num_obj = len(all_objs)
    
    keys = range(num_obj)
    np.random.shuffle(list(keys))
    
    # For each batch, we will have indices of [ l_bound ...... r_bound ) (not including r_bound itself).
    # Usually r_bound - l_bound  should be equal to batch size.
    l_bound = 0
    r_bound = batch_size if batch_size < num_obj else num_obj
    
    while True:
        if l_bound == r_bound:
            l_bound  = 0
            r_bound = batch_size if batch_size < num_obj else num_obj
            np.random.shuffle(list(keys))
        
        currt_inst = 0
        x_batch = np.zeros((r_bound - l_bound, 224, 224, 3)) # Image batch
        d_batch = np.zeros((r_bound - l_bound, 3)) # Dimension batch
        o_batch = np.zeros((r_bound - l_bound, BIN, 2)) # orientation batch
        c_batch = np.zeros((r_bound - l_bound, BIN)) # confidences batch
        
        # Iterate the batch
        for key in keys[l_bound:r_bound]:
            # Prepare the data for the current frame.
            image, dimension, orientation, confidence = prepare_input_and_output(all_objs[key])
            
            x_batch[currt_inst, :] = image
            d_batch[currt_inst, :] = dimension
            o_batch[currt_inst, :] = orientation
            c_batch[currt_inst, :] = confidence
            
            currt_inst += 1
                
        # Yield the prepared batch. 
        yield x_batch, [d_batch, o_batch, c_batch]

        # Go for the next batch
        l_bound  = r_bound
        r_bound = r_bound + batch_size

        # Limit the r_bount to max valid index.
        if r_bound > num_obj: r_bound = num_obj 

def l2_normalize(x):
    # Compute the second norm for each (sin, cos) pare and normalize the values.
    # So (sin, cos) will be normalized into ( sin/sqrt(sin^2+cos^2) , cos/sqrt(sin^2+cos^2)).
    # Thus if the network gives (a,b), we are always sure that a^2 + b^2 = 1 and we can use arctan with no worries.
    return tf.nn.l2_normalize(x, axis=2)

def orientation_loss(y_true, y_pred):
    # Here we have two 3D arrays with shape (batch_size, bin count, 2). the 2 is for the (sin, cos) vector.
    # The loss value, however, should be a scalar.
    
    # Make (sin, cos) into (sin^2, cos^2) and then sum them up into 1 scalar.
    # So the shape is now (batch_size, bin_count)
    anchors = tf.reduce_sum(tf.square(y_true), axis=2)

    # Not sure about this line. I believe it assigns true for every bin that
    # has enough overlap with the true angle. Because, in the "Process data" cell,
    # we inserted (0,0) for other bins and only the one or two bins with enough overlap received the (sin, cos)
    anchors = tf.greater(anchors, tf.constant(0.5))

    # Now for each row, sum the values. So the shape is now (batch size).
    anchors = tf.reduce_sum(tf.cast(anchors, tf.float32), 1)
    

    # We use cosine similarity for the loss. we compute  cos(alpha)= a.b / |a||b|. 
    # alpha is the angle between ground truth vector and the estimation.
    # The ideal value would be cos(0) = 1. But gradient decent tries to MINIMIZE the loss.
    # So we add a - behind it. Now the ideal loss value is -1 and the network gets trained in the right direction.
    loss = -(y_true[:,:,0]*y_pred[:,:,0] + y_true[:,:,1]*y_pred[:,:,1]) 

    # For each batch, sum all loss values for all bins. So the shape becomes (batch size)
    loss = tf.reduce_sum(loss, axis=1)

    # Now normalize the loss. If I've got it right, the anchors are an array of (bin count, bin count, ...)
    # So now the loss becomes (loss0/relevant_bin_count, loss1/relevant_bin_count, ...) with shape (batch size).
    loss = loss / anchors
    
    # Use mean to turn the vector loss into scalar.
    return tf.reduce_mean(loss)<jupyter_output><empty_output><jupyter_text>Parse annotation (Function)<jupyter_code>def parse_annotation(label_dir, image_dir):
    # Here we prepare all objects (patches), their attributes, and the
    # average dimension across the dataset.
    all_objs = []
    
    # The average vector and the object count used for averaging.
    dims_avg = {key:np.array([0, 0, 0]) for key in VEHICLES}
    dims_cnt = {key:0 for key in VEHICLES}
    
    # Iterate through label files.
    for label_file in os.listdir(label_dir):

        # If you are using another dataset, take care of this.
        image_file = label_file.replace('txt', 'png')

        # Iterate through lines in each label file
        for line in open(label_dir + label_file).readlines():
            line = line.strip().split(' ')
            # Each row will have this structure:
            # Class Truncated Occluded Theta(local) Xmin Ymin Xmax Ymax Dim Dim Dim T T T 
            truncated = np.abs(float(line[1]))
            occluded  = np.abs(float(line[2]))

            # Make sure it is a relevant class and with enough visibility (based on global parameters)
            if line[0] in VEHICLES and truncated < MAX_TRUNCATION and occluded <= MAX_OCCLUSION:

                # This is pretty confusing here. I will explain it somewhere in the documentation. Sorry :(
                new_alpha = -float(line[3]) + 3*np.pi/2
                new_alpha = new_alpha - np.floor(new_alpha / (2. * np.pi)) * (2. * np.pi)

                obj = {'name':line[0],
                       'image':image_file,
                       'xmin':int(float(line[4])),
                       'ymin':int(float(line[5])),
                       'xmax':int(float(line[6])),
                       'ymax':int(float(line[7])),
                       'dims':np.array([float(number) for number in line[8:11]]),
                       'new_alpha': new_alpha, 
                      # The next 3 are not used for training the network, but for benchmarking translation vector accuracy. 
                       'translation':np.array([float(number) for number in line[11:14]]),
                       'truncated': truncated,
                       'occluded': occluded
                      }
                
                # Update the average while reading the new object's dimensions
                dims_avg[obj['name']]  = dims_cnt[obj['name']]*dims_avg[obj['name']] + obj['dims']
                dims_cnt[obj['name']] += 1
                dims_avg[obj['name']] /= dims_cnt[obj['name']]

                # Add the object to the list
                all_objs.append(obj)
                
    # return the object list and the average dimensions.
    return all_objs, dims_avg<jupyter_output><empty_output><jupyter_text>Process data<jupyter_code>all_objs, dims_avg = parse_annotation(label_dir, image_dir)

print("number of objects: {}".format(len(all_objs)))
print(" ----- \n A sample object(patch):\n {} \n -----".format(all_objs[0]))
print("The average dimensions: {}".format(dims_avg))

for obj in all_objs:

    # Fix dimensions, compute the residual
    obj['dims'] = obj['dims'] - dims_avg[obj['name']]

    # Make all residuals 0
    orientation = np.zeros((BIN,2))
    # Make all bin confidences 0
    confidence = np.zeros(BIN)
    
    #turn angles into -> [bin# , residual]
    anchors = compute_anchors(obj['new_alpha'])
    
    for anchor in anchors:
        #compute the cosine and sine of the residual
        orientation[anchor[0]] = np.array([np.cos(anchor[1]), np.sin(anchor[1])])

        #make the confidence of relative bin# 1
        confidence[anchor[0]] = 1.
        
    #normalize confidences
    confidence = confidence / np.sum(confidence)
        
    #add the computed orientation and confidence to the obj
    obj['orient'] = orientation
    obj['conf'] = confidence
        
    # Fix orientation and confidence for flip
    orientation = np.zeros((BIN,2))
    confidence = np.zeros(BIN)
    
    anchors = compute_anchors(2.*np.pi - obj['new_alpha'])
    
    for anchor in anchors:
        orientation[anchor[0]] = np.array([np.cos(anchor[1]), np.sin(anchor[1])])
        confidence[anchor[0]] = 1
        
    confidence = confidence / np.sum(confidence)
        
    obj['orient_flipped'] = orientation
    obj['conf_flipped'] = confidence<jupyter_output><empty_output><jupyter_text>**Set up and Train 3D Model**  Build 3D Model(Function and usage)<jupyter_code># Construct the network
def build_model(input_shape=(224, 224, 3), weights=None, freeze=False, feature_extractor):

    if feature_extractor == 'mobilenetv2':
        feature_extractor_model = MobileNetV2(include_top=False, weights=weights, input_shape=input_shape)
    elif feature_extractor == 'vgg16':
        feature_extractor_model = VGG16(include_top=False, weights=weights, input_shape=input_shape)
    else:
        print("Requested a non-existing feature extractor model. Either choose from mobilenetv2 and vgg16 or add your own to the code")
        exit(-1)
        
    if freeze:
        for layer in feature_extractor_model.layers:
            layer.trainable = False

    x = Flatten()(feature_extractor_model.output)

    dimension   = Dense(512)(x)
    dimension   = LeakyReLU(alpha=0.1)(dimension)
    dimension   = Dropout(0.5)(dimension)
    dimension   = Dense(3)(dimension)
    dimension   = LeakyReLU(alpha=0.1, name='dimension')(dimension)

    orientation = Dense(256)(x)
    orientation = LeakyReLU(alpha=0.1)(orientation)
    orientation = Dropout(0.5)(orientation)
    orientation = Dense(BIN*2)(orientation)
    orientation = LeakyReLU(alpha=0.1)(orientation)
    orientation = Reshape((BIN,-1))(orientation)
    orientation = Lambda(l2_normalize, name='orientation')(orientation)

    confidence  = Dense(256)(x)
    confidence  = LeakyReLU(alpha=0.1)(confidence)
    confidence  = Dropout(0.5)(confidence)
    confidence  = Dense(BIN, activation='softmax', name='confidence')(confidence)

    model = Model(feature_extractor_model.input, outputs=[dimension, orientation, confidence])
    model.summary() 


    #load weights
    #model.load_weights('/content/gdrive/MyDrive/weights_new2.hdf5')
    return model<jupyter_output><empty_output><jupyter_text>Pre-training Setup<jupyter_code># You can replace 'mobilenet' with 'vgg16' to change the feature extractor implementation in build_model().
model = build_model(input_shape = (224, 224, 3), weights = 'imagenet', freeze=False, feature_extractor=FEATURE_EXTRACTOR_3D)

# Make sure to change the checkpoint path based on your setup.
checkpoint  = ModelCheckpoint('/content/gdrive/MyDrive/saves/saved_weights_mobilenet_transition.hdf5', monitor='val_loss', verbose=1, save_best_only=True, mode='min', period=1)
early_stop  = EarlyStopping(monitor='val_loss', min_delta=0.001, patience=10, mode='min', verbose=1)
tensorboard = TensorBoard(log_dir='../logs/', histogram_freq=0, write_graph=True, write_images=False)

all_exams  = len(all_objs)
trv_split  = int(0.85*all_exams)
batch_size = 32

# As I understood, Kitti's 2D Object dataset is already shuffeled. So there is no need to shuffle the dataset.
# Nevertheless, you can do so with next line: 
# np.random.shuffle(all_objs)

train_gen = data_gen(all_objs[:trv_split], batch_size)
valid_gen = data_gen(all_objs[trv_split:all_exams], batch_size)

train_num = int(np.ceil(trv_split/batch_size))
valid_num = int(np.ceil((all_exams - trv_split)/batch_size))

print("training configurations:")
print("all data:", all_exams)
print("training data:", trv_split)
print("batch size:", batch_size)
print("train_num", train_num)
print("valid_num", valid_num)


minimizer  = Adam(lr=1e-5)
model.compile(optimizer=minimizer, #minimizer,
              loss={'dimension': 'mean_squared_error', 'orientation': orientation_loss, 'confidence': 'categorical_crossentropy'},
                  loss_weights={'dimension': 2., 'orientation': 1., 'confidence': 4.}, metrics=['mse','mae'])<jupyter_output><empty_output><jupyter_text>Train 3D Model<jupyter_code>model.fit_generator(generator = train_gen, 
                    steps_per_epoch = train_num, 
                    epochs = 150, 
                    verbose = 1, 
                    callbacks = [early_stop, checkpoint, tensorboard],
                    validation_data = valid_gen, 
                    validation_steps = valid_num,
                    )<jupyter_output><empty_output><jupyter_text>Test 3D Model's Performance  Open a sample image and patch<jupyter_code># The testing/ dir, is a completely separate dir that is neither used nor annotated. Just a series of image files.
# So for using these samples, you must have a 2D detector at hand. Or you can use the training/ dir, but this dir  
# includes your training samples as well. So you can use training dir just for testing the code not the accuracy.
test_number = '001024'
test_image_path = 'testing/image_2/{test_number}.png'.format(test_number = test_number)
calib_path = 'testing/calib/{test_number}.txt'.format(test_number = test_number)

# You can also read from train dir (this includes both train and test instances)
# test_image_path = 'training/image_2/{test_number}.png'.format(test_number = test_number)
# calib_path = 'training/calib/{test_number}.txt'.format(test_number = test_number)
# test_label_path = 'training/label_2/{test_number}.txt'.format(test_number = test_number)

# Plot the sample
test_image = cv2.imread(test_image_path)
image_plot = test_image[:, :, ::-1]
plt.figure(figsize=(60,60))
plt.imshow(image_plot/255.)
plt.show()<jupyter_output><empty_output><jupyter_text>Get 2D boxes (3 options)  Option A: Manually select a bounding box<jupyter_code># If you are want to do a quick test, you can insert bounding box annotations manually.
xmin,ymin,xmax,ymax = (115, 170, 242, 235)
final_boxes = [[xmin,ymin,xmax,ymax]] #Only one box of course.

# Plot the patch
patch = plot_image[ymin:ymax, xmin:xmax]
plt.figure()
plt.imshow(patch/255.)
plt.show()<jupyter_output><empty_output><jupyter_text>Option B: Get bounding boxes from label file (if using training dir)<jupyter_code>patches = []
final_boxes = []
for line in open(test_label_path).readlines():
    line = line.strip().split(' ')

    xmin=int(float(line[4]))
    ymin=int(float(line[5]))
    xmax=int(float(line[6]))
    ymax=int(float(line[7]))

    patch = plot_image[ymin:ymax, xmin:xmax]
    patch = cv2.resize(patch, (224, 224))
    patches.append(patch)
    final_boxes.append((xmin, ymin, xmax, ymax))

num_patches = len(patches)
fig=plt.figure(figsize=(15, 15))

for index, patch in enumerate(patches):
    fig.add_subplot(1, num_patches, index+1)
    plt.imshow(patch)

plt.show()<jupyter_output><empty_output><jupyter_text>Choose only one of the above if you want.<jupyter_code># If you don't want all of the above boxes, choose only one.s
index = 0
patch = patches[index]
final_boxes = [final_boxes[index]]
plt.figure()
plt.imshow(patch)
plt.show()<jupyter_output><empty_output><jupyter_text>Option C: Use Tensorflow Object Detection API Step 1: Get the pretrained Faster RCNN model<jupyter_code>!wget http://download.tensorflow.org/models/object_detection/faster_rcnn_resnet101_kitti_2018_01_28.tar.gz
!tar -xvf faster_rcnn_resnet101_kitti_2018_01_28.tar.gz
# Clean up
rm -rf faster_rcnn_resnet101_kitti_2018_01_28.tar.gz<jupyter_output><empty_output><jupyter_text>Step 2: Load the model and detect cars in 2DKeep in mind that this model is trained to classify cars and pedestrians. But we want Cars, Vans, and Trucks.At the end of this notebook, there is a tutorial on re-training the model (or any other model) on these classes.But for now, we assume that detecting cars is sufficient.<jupyter_code>def load_graph(frozen_graph_filename):
    with tf.gfile.GFile(frozen_graph_filename, "rb") as f:
        graph_def = tf.GraphDef()
        graph_def.ParseFromString(f.read())

    with tf.Graph().as_default() as graph:
        tf.import_graph_def(graph_def, name="2D")

    return graph

# Make sure to change the path based on your setup
graph = load_graph('/content/faster_rcnn_resnet101_kitti_2018_01_28/frozen_inference_graph.pb')

# Input tensor
image_tensor = graph.get_tensor_by_name('2D/image_tensor:0')

# Output tensors
scores_tensor = graph.get_tensor_by_name('2D/detection_scores:0')
boxes_tensor = graph.get_tensor_by_name('2D/detection_boxes:0')
classes_tensor = graph.get_tensor_by_name('2D/detection_classes:0')
num_tensor = graph.get_tensor_by_name('2D/num_detections:0')

# We launch a Session
image_width = test_image.shape[1]
image_height = test_image.shape[0]
with tf.Session(graph=graph) as sess:
    (boxes, scores, classes, num) = sess.run(
        [boxes_tensor, scores_tensor, classes_tensor, num_tensor],
        feed_dict={image_tensor: [test_image]})

show= test_image.copy()
final_boxes=[]
for index in range(int(num[0])):
  # Apply confidence threshold
  if (scores[0][index] < 0.7): 
      continue
  box = boxes[0][index]
  real_box = (int(box[0]*image_height), int(box[1]*image_width), int(box[2]*image_height), int(box[3]*image_width))
  print(real_box, scores[0][index])
  show = cv2.rectangle(show, (real_box[1], real_box[0]), (real_box[3], real_box[2]), color=(0,0,255), thickness=2)
  final_boxes.append(real_box)

show_plot = show[:, :, ::-1]
plt.figure(figsize=(60,60))
plt.imshow(show_plot/255.)
plt.show()<jupyter_output><empty_output><jupyter_text>Prepare for 3D<jupyter_code>### Load Model Weights<jupyter_output><empty_output><jupyter_text>Make sure to change the checkpoint path based on your setup.model.load_weights('/content/gdrive/MyDrive/saves/saved_weights_mobilenet_new.hdf5')  Download Calibration Files<jupyter_code>!wget https://s3.eu-central-1.amazonaws.com/avg-kitti/data_object_calib.zip
!unzip data_object_calib.zip<jupyter_output><empty_output><jupyter_text>Read Projection Matrix<jupyter_code>with open(calib_path, "r") as f:
    c2c_file = f.readlines()

    for line in c2c_file:
          (key, val) = line.split(':', 1)
          if key == ('P2'):
                P_ = np.fromstring(val, sep=' ', dtype = 'float')
                P_ = P_.reshape(3, 4)
                
                calib_mat = P_
                break
print("Projection mat:")
print(calib_mat)

# #Just for quick tests
# calib_mat= [[7.215377e+02, 0.000000e+00, 6.095593e+02, 4.485728e+01],
#             [0.000000e+00, 7.215377e+02, 1.728540e+02, 2.163791e-01],
#             [0.000000e+00, 0.000000e+00, 1.000000e+00, 2.745884e-03]]<jupyter_output><empty_output><jupyter_text>Define and Prepare Projection utils<jupyter_code>def init_points3D(dims):
    points3D = np.zeros((8, 3))
    cnt = 0
    for i in [1, -1]:
        for j in [1, -1]:
            for k in [1, -1]:
                points3D[cnt] = dims[[1, 0, 2]].T / 2.0 * [i, k, j * i]
                cnt += 1
    return points3D

def gen_3D_box(yaw,dims,cam_to_img,box_2D):
    
    dims = dims.reshape((-1,1))
    box_2D = box_2D.reshape((-1,1))
    points3D = init_points3D(dims)

    # Here the rotation is done around the Y axis. Just a convention in the code.
    rot_M = np.asarray([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0], [-np.sin(yaw), 0, np.cos(yaw)]]) 
    center = compute_center(points3D, rot_M, cam_to_img, box_2D, inds)

    points2D = points3D_to_2D(points3D, center, rot_M, cam_to_img)

    return points2D

def compute_center(points3D,rot_M,cam_to_img,box_2D,inds):
    
    fx = cam_to_img[0][0]
    fy = cam_to_img[1][1]
    u0 = cam_to_img[0][2]
    v0 = cam_to_img[1][2]
    
    W = np.array([[fx, 0, u0 - box_2D[0]],
                  [fx, 0, u0 - box_2D[2]],
                  [0, fy, v0 - box_2D[1]],
                  [0, fy, v0 - box_2D[3]]], dtype = 'float')
    center =None
    error_min = 1e10

    for ind in inds:
        y = np.zeros((4, 1))
        for i in range(len(ind)):
            
            RP = np.dot(rot_M, (points3D[ind[i]]).reshape((-1, 1)))
            y[i] = box_2D[i] * cam_to_img[2, 3] - np.dot(W[i], RP) - cam_to_img[i // 2, 3]
            
        result = solve_least_squre(W, y)
        error = compute_error(points3D, result, rot_M, cam_to_img, box_2D)
        
        if error < error_min and result[2,0]>0:
            center = result
            error_min = error
            
    return center

def draw_3D_box(image,points):
    points = points.astype(np.int)

    for i in range(4):
        point_1_ = points[2 * i]
        point_2_ = points[2 * i + 1]
        cv2.line(image, (point_1_[0], point_1_[1]), (point_2_[0], point_2_[1]), (0, 255, 0), 1)

    # The red X at the front.
    cv2.line(image,tuple(points[0]),tuple(points[7]),(0, 0, 255), 2)
    cv2.line(image, tuple(points[1]), tuple(points[6]), (0, 0, 255), 2)

    for i in range(8):
        point_1_ = points[i]
        point_2_ = points[(i + 2) % 8]
        cv2.line(image, (point_1_[0], point_1_[1]), (point_2_[0], point_2_[1]), (0, 255, 0), 1)

    return image;

def solve_least_squre(W,y):
    U, Sigma, VT = np.linalg.svd(W)
    result = np.dot(np.dot(np.dot(VT.T, np.linalg.pinv(np.eye(4, 3) * Sigma)), U.T), y)
    return result

def points3D_to_2D(points3D,center,rot_M,cam_to_img):
    
    # General formula is: [2D] = K[R T][3D]
    # So for each 3D point, apply rotation, add translation (3D center)
    # and multiply K. At last, you will have a 3x1 vector, which you should normalize
    # by the third element (Homogenous coordinates).
    points2D = []
    for point3D in points3D:
        point3D = point3D.reshape((-1,1))
        point = center + np.dot(rot_M, point3D)
        point = np.append(point, 1)
        point = np.dot(cam_to_img, point)
        point2D = point[:2] / point[2]
        points2D.append(point2D)
    points2D = np.asarray(points2D)

    return points2D

def compute_error(points3D,center,rot_M, cam_to_img,box_2D):
    # Get all of 8 corners from 3D box projected on image.
    points2D = points3D_to_2D(points3D, center, rot_M, cam_to_img)

    # Get a new bounding box from the 8 projected coreners.
    new_box_2D = np.asarray([np.min(points2D[:,0]),
                             np.max(points2D[:,0]),
                             np.min(points2D[:,1]),
                             np.max(points2D[:,1])]).reshape((-1,1))

    # Sum the absolute difference of xmin, xmax, ymin, ymax for the 2D bbox,
    # and the new bbox from 8 projections.
    error = np.sum(np.abs(new_box_2D - box_2D))
    return error

# These will be used in the next cell
inds = []
indx = [1, 3, 5, 7]
indy = [0, 1, 2, 3]
for i in indx:
    for j in indx:
        for m in indy:
            for n in indy:
                inds.append([i, j, m, n])

# Dimension averages collected from the dataset
dims_avg = {'Car' : [1.52130159, 1.64441129, 3.85729945],
            'Truck': [ 3.07044968,  2.62877944, 11.17126338],
            'Van': [2.18560847, 1.91077601, 5.08042328],
            'Tram': [3.56005102,  2.4002551,  18.52173469] }<jupyter_output><empty_output><jupyter_text>2D to 3D<jupyter_code>final_image = test_image.copy()
for index, box in enumerate(final_boxes):
    ymin, xmin, ymax, xmax = box
    # Crop
    patch = test_image[ymin:ymax, xmin:xmax]
    # Resize
    patch = cv2.resize(patch, (224, 224))
    # Set the input pixels to be within (-0.5 , 0.5).
    patch = patch / 255.0 - 0.5

    patch = np.expand_dims(patch, axis = 0)
    prediction = model.predict(patch)

    # Get the (cos, sin) of the bin with highest probabilitys
    max_anc = np.argmax(prediction[2][0])
    anchors = prediction[1][0][max_anc]

    # anchors=(cos, sin)
    if anchors[1] > 0:
        angle_offset = np.arccos(anchors[0])
    else:
        angle_offset = -np.arccos(anchors[0])

    wedge = 2.*np.pi/BIN
    theta_loc = angle_offset + max_anc*wedge

    fx = calib_mat[0][0]
    u0 = calib_mat[0][2]
    v0 = calib_mat[1][2]

    # As suggested in the original paper, we estimate the raye to 2D box center instead of
    # object's 3D center. Of course, it is not entirely accurate, but it is sufficient.
    box2d_center_x= (xmin + xmax) / 2.0
    theta_ray = np.arctan(fx /(box2d_center_x - u0))
    
    # Arctan's output is (-pi/2, pi/2). But theta_ray should be (0, pi).
    # From 0 to pi/2 the values are ok but if the object's center is on the left half,
    # the arctan will result in a negative theta_ray. This can be fixed by adding pi.
    if theta_ray<0:
          theta_ray = theta_ray+np.pi

    # Final theta
    theta = theta_loc + theta_ray
    yaw = np.pi/2 - theta
                
    # Here we use average dims of cars. If you use a trained model for 2D detection with 
    # correct classes, you can simply replace this line. The training steps of 2D network is 
    # provided at the end of the notebook.
    dims = dims_avg['Car'] + prediction[0][0]

    box_2D = np.asarray([box[1], box[0], box[3], box[2]], dtype = np.float)
    points2D = gen_3D_box(yaw, dims, calib_mat, box_2D) #switched yaw -> theta    
    final_image = draw_3D_box(final_image, points2D)

final_image_show = final_image[:, :, ::-1]
plt.figure(figsize=(20,20))
plt.imshow(final_image_show/255.)
plt.show()<jupyter_output><empty_output><jupyter_text>Test Translation Vector AccuracySince this code is independent from testing 3D model's accuracy, I have redifned many functions so it remains independent.<jupyter_code>inds = []
indx = [1, 3, 5, 7]
indy = [0, 1, 2, 3]
for i in indx:
      for j in indx:
      for m in indy:
            for n in indy:
                  inds.append([i, j, m, n])

def get_t(yaw,dims,cam_to_img,box_2D, ind):
    dims = dims.reshape((-1,1))
    box_2D = box_2D.reshape((-1,1))
    points3D = init_points3D(dims)
    rot_M = np.asarray([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0], [-np.sin(yaw), 0, np.cos(yaw)]]) 
    center = compute_center(points3D, rot_M, cam_to_img, box_2D, inds)
    return center

def compare_t(estimated_t, true_t):
  estimated = np.asarray([float(estimated_t[0]), float(estimated_t[1]), float(estimated_t[2])])
  dist = np.linalg.norm(true_t - estimated)
  normalized_dist = np.linalg.norm(true_t - estimated) / np.linalg.norm(true_t)
  return dist, normalized_dist

avg_t_loss=0
avg_nt_loss=0
cnt = 0
for obj in all_objs[trv_split:all_exams]:
    truncated = obj['truncated']
    occluded = obj['occluded']
    height = int(obj['ymax'] - obj['ymin'])
    width = int(obj['xmax'] - obj['xmin'])
    # Filter based on occlusion and trunction.
    # Many objects' centers are not even in front of the camera
    if (truncated > 0.5 or occluded> 1 or height <=25 or width <=25):
      continue

    with open('/content/training/calib/'+obj['image'].split('.')[0]+'.txt', "r") as f:
      c2c_file = f.readlines()

    for line in c2c_file:
          (key, val) = line.split(':', 1)
          if key == ('P2'):
                P_ = np.fromstring(val, sep=' ', dtype = 'float')
                P_ = P_.reshape(3, 4)
                calib_mat = P_
                break

    xmin = int(obj['xmin'])
    ymin = int(obj['ymin'])
    xmax = int(obj['xmax'])
    ymax = int(obj['ymax'])

    test_image = cv2.imread('/content/training/image_2/'+obj['image'])
    patch = test_image[ymin:ymax, xmin:xmax]
    patch = cv2.resize(patch, (224, 224))
    patch = patch / 255.0 - 0.5

    patch = np.expand_dims(patch, axis = 0)

    prediction = model.predict(patch)
    max_anc = np.argmax(prediction[2][0])
    anchors = prediction[1][0][max_anc]

    if anchors[1] > 0:
        angle_offset = np.arccos(anchors[0])
    else:
        angle_offset = -np.arccos(anchors[0])
        
    wedge = 2.*np.pi/BIN
    theta_loc = angle_offset + max_anc*wedge

    fx = calib_mat[0][0]
    u0 = calib_mat[0][2]
    v0 = calib_mat[1][2]

    box2d_center_x= (xmin + xmax) / 2.0
    
    theta_ray = np.arctan(fx /(box2d_center_x - u0))
    if theta_ray<0:
          theta_ray = theta_ray+np.pi

    # Theta Yaw
    theta = theta_loc + theta_ray
    yaw = np.pi/2 - theta

    #DIMENSION
    dims_avg = {'Car' : [1.52130159, 1.64441129, 3.85729945],
                'Truck': [ 3.07044968,  2.62877944, 11.17126338],
                'Van': [2.18560847, 1.91077601, 5.08042328],
                'Tram': [3.56005102,  2.4002551,  18.52173469] }
    dims = dims_avg[obj['name']] + prediction[0][0]



    box_2D = np.asarray([xmin, ymin, xmax, ymax], dtype = np.float)
    points2D = gen_3D_box(yaw, dims, calib_mat, box_2D) #switched yaw -> theta    
    final_image = draw_3D_box(test_image, points2D)

  
    t = get_t(yaw, dims, calib_mat, box_2D, inds) #switched yaw -> theta    
    t_loss, nt_loss = compare_t(t, obj['translation'])
    cnt += 1
    avg_t_loss = ( (avg_t_loss * (cnt-1)) + t_loss )/cnt
    avg_nt_loss = ( (avg_nt_loss * (cnt-1)) + nt_loss )/cnt
    
print("Average loss is: ", avg_t_loss)
print("Average normalized loss is: ", avg_nt_loss)
print("Count: ", cnt)<jupyter_output><empty_output><jupyter_text>Train a 2D Network Yourself  Setup the environment<jupyter_code>!pip install protobuf protobuf-compiler

# Clone the repo
!git clone https://github.com/tensorflow/models.git

# Compile protos.
%cd models/research
!protoc object_detection/protos/*.proto --python_out=.

# Install TensorFlow Object Detection API.
%cp object_detection/packages/tf1/setup.py .
!python -m pip install .

# Test the installation
!python object_detection/builders/model_builder_tf1_test.py
%cd ../..<jupyter_output><empty_output><jupyter_text>Get FRCNN_Resnet_101<jupyter_code>%cd /content
!wget http://download.tensorflow.org/models/object_detection/faster_rcnn_resnet101_kitti_2018_01_28.tar.gz
!tar -xvf faster_rcnn_resnet101_kitti_2018_01_28.tar.gz<jupyter_output><empty_output><jupyter_text>Get Mobilenet_V2+SSD<jupyter_code>%cd /content
!wget http://download.tensorflow.org/models/object_detection/ssd_mobilenet_v2_coco_2018_03_29.tar.gz
!tar -xvf ssd_mobilenet_v2_coco_2018_03_29.tar.gz<jupyter_output><empty_output><jupyter_text>Generate TF record<jupyter_code># In case you are running this cell again:
%cd /content
!rm -rf data
!rm -rf ouptut
!rm -rf data_object_image_2


%mkdir data_object_image_2
%cd data_object_image_2
%mkdir training
%cd ..
%cp -r training/image_2 data_object_image_2/training/
%mkdir data
!mkdir output

# Create the labelmap
!touch data/kitti_label_map.pbtxt
!printf 'item {\n id: 1\n name: "car"\n}\nitem {\n id: 2\n name: "pedestrian"\n}\nitem {\n id: 3\n name: "van"\n}\nitem {\n id: 4\n name: "truck"\n}' >> data/kitti_label_map.pbtxt
print("MAKE SURE TO DOUBLE CHECK THE LABELMAP FILE' \n S" )

# Do not bother with this line
!rm -rf /content/data_object_image_2/training/image_2/.ipynb_checkpoints
# Generate TFRecord
!python /content/models/research/object_detection/dataset_tools/create_kitti_tf_record.py \
--data_dir=/content \
--output_path=/content/output/kitti.record \
--classes_to_use=car,pedestrian,van,truck \
--validation_set_side=500<jupyter_output><empty_output><jupyter_text>As you may suspect, we do not really need the Pedestrian class. However, in my experience, removing the pedestrain class from TFRecords and LabelMap will result in a model that detects nothing! I am not sure why, but I guess this has something to do with the transfer learning and weight re-initialization part. Again, not sure! If you have any idea on this, do send me a PR or open an issue.  Train the 2D model  Copy the **train.py** and **eval.py** to the main object_detection dir<jupyter_code>%cp models/research/object_detection/legacy/train.py models/research/object_detection
%cp /content/models/research/object_detection/legacy/eval.py /content/models/research/object_detection<jupyter_output><empty_output><jupyter_text>Train the modelA default **pipeline.config** is provided with the checkpoints you download from modelzoo and they can work with small modifications. But, due to the lack of documentation and out-dated configs, you may come across errors. Therefore, I suggest using the pipelines that I have provided in the repo. They are not the best config but at least they work. Make sure to do your best on optimizing the configuration. I would appreciate it if you share your config changes with PR or and issue.The next cell checks the output_train and starts with its latest checkpoint, and creates the first if there is none. So if you can stop the cell and run it again.<jupyter_code>%cd /content

# To clean the entire training story and re-train the model:
# !rm -rf /content/output_train

# Make sure to fix the paths according to your setup.
!python models/research/object_detection/train.py \
--logdir=/content/log \
--train_dir=/content/output_train \
--pipeline_config_path=/content/ssd_mobilenet_v2_coco_2018_03_29/pipeline.config<jupyter_output><empty_output><jupyter_text>Evaluate the model<jupyter_code>%rm -rf /content/output_eval
%cd /content/models/research

# Make sure to fix the paths according to your setup.
!python object_detection/eval.py \
        --logtostderr \
        --checkpoint_dir=/content/output_train \
        --eval_dir=/content/output_eval \
        --pipeline_config_path=/content/output_train/pipeline.config<jupyter_output><empty_output><jupyter_text>Export the frozen graphWith the frozen graph you can easily load the model to get the 2D boxes. The graph can be loaded in the same way I loaded the pre-trained frcnn graph. Only the class numbers can change based on your labelmap.<jupyter_code>%cd /content/models/research/
# Make sure to fix the paths according to your setup.
!python object_detection/export_inference_graph.py \
--input_type=image_tensor \
--pipeline_config_path=/content/gdrive/MyDrive/colab/new/25_May/output_train/pipeline.config \
--trained_checkpoint_prefix=/content/gdrive/MyDrive/colab/new/25_May/output_train/model.ckpt-25000 \
--output_directory=/content/gdrive/MyDrive/colab/new/25_May/Frozen<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.11759436980166346, 'fraction_numerical': 0.03129878438899552, 'mean_word_length': 3.419362135263515, 'pattern_counts': {'":': 0, '<': 151, '<?xml version=': 0, '>': 151, 'https://': 4, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 8, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '34', 'dir': 'jupyter-structured-clean-dedup', 'id': '633899', 'license': 'MIT', 'n_tokens_mistral': 14409, 'n_tokens_neox': 13458, 'n_words': 3542, 'path': 'AllinOne.ipynb', 'repo_name': 'm-parchami/3DVehicleDetection', 'size': '39075'}