[ { "path": "D3QN/D3QN_testing.py", "content": "from __future__ import print_function\nfrom RealWorld import RealWorld\n\nimport tensorflow as tf\nimport random\nimport numpy as np\nimport time\nimport rospy\nimport models\nimport cv2\n\nACTIONS = 7 # number of valid actions\nSPEED = 2 # DoF of speed\nGAMMA = 0.99 # decay rate of past observations\nOBSERVE = 10. # timesteps to observe before training\nEXPLORE = 20000. # frames over which to anneal epsilon\nFINAL_EPSILON = 0.0001 # final value of epsilon\nINITIAL_EPSILON = 0.001 # starting value of epsilon\nREPLAY_MEMORY = 50000 # number of previous transitions to remember\nBATCH = 8 # size of minibatch\nMAX_EPISODE = 20000\nDEPTH_IMAGE_WIDTH = 160\nDEPTH_IMAGE_HEIGHT = 128\nRGB_IMAGE_HEIGHT = 228\nRGB_IMAGE_WIDTH = 304\nCHANNEL = 3\nTAU = 0.001 # Rate to update target network toward primary network\nH_SIZE = 8*10*64\nIMAGE_HIST = 4\n\ndef variable_summaries(var):\n\t\"\"\"Attach a lot of summaries to a Tensor (for TensorBoard visualization).\"\"\"\n\twith tf.name_scope('summaries'):\n\t\tmean = tf.reduce_mean(var)\n\ttf.summary.scalar('mean', mean)\n\twith tf.name_scope('stddev'):\n\t\tstddev = tf.sqrt(tf.reduce_mean(tf.square(var - mean)))\n\ttf.summary.scalar('stddev', stddev)\n\ttf.summary.scalar('max', tf.reduce_max(var))\n\ttf.summary.scalar('min', tf.reduce_min(var))\n\ttf.summary.histogram('histogram', var)\n\t\t\ndef weight_variable(shape):\n\tinitial = tf.truncated_normal(shape, stddev = 0.01)\n\treturn tf.Variable(initial, name=\"weights\")\n\ndef bias_variable(shape):\n\tinitial = tf.constant(0., shape = shape)\n\treturn tf.Variable(initial, name=\"bias\")\n\ndef conv2d(x, W, stride_h, stride_w):\n\treturn tf.nn.conv2d(x, W, strides = [1, stride_h, stride_w, 1], padding = \"SAME\")\n\n\nclass QNetwork(object):\n\t\"\"\"docstring for ClassName\"\"\"\n\tdef __init__(self, sess, depth_predict):\n\t\t# network weights\n\t\t# input 128x160x1\n\t\twith tf.name_scope(\"Conv1\"):\n\t\t\tW_conv1 = weight_variable([10, 14, IMAGE_HIST, 32])\n\t\t\tvariable_summaries(W_conv1)\n\t\t\tb_conv1 = bias_variable([32])\n\t\t# 16x20x32\n\t\twith tf.name_scope(\"Conv2\"):\n\t\t\tW_conv2 = weight_variable([4, 4, 32, 64])\n\t\t\tvariable_summaries(W_conv2)\n\t\t\tb_conv2 = bias_variable([64])\n\t\t# 8x10x64\n\t\twith tf.name_scope(\"Conv3\"):\n\t\t\tW_conv3 = weight_variable([3, 3, 64, 64])\n\t\t\tvariable_summaries(W_conv3)\n\t\t\tb_conv3 = bias_variable([64])\n\t\t# 8x10x64\n\t\twith tf.name_scope(\"FCValue\"):\n\t\t\tW_value = weight_variable([H_SIZE, 512])\n\t\t\tvariable_summaries(W_value)\n\t\t\tb_value = bias_variable([512])\n\t\t\t# variable_summaries(b_ob_value)\n\n\t\twith tf.name_scope(\"FCAdv\"):\n\t\t\tW_adv = weight_variable([H_SIZE, 512])\n\t\t\tvariable_summaries(W_adv)\n\t\t\tb_adv = bias_variable([512])\n\t\t\t# variable_summaries(b_adv)\n\n\t\twith tf.name_scope(\"FCValueOut\"):\n\t\t\tW_value_out = weight_variable([512, 1])\n\t\t\tvariable_summaries(W_value_out)\n\t\t\tb_value_out = bias_variable([1])\n\t\t\t# variable_summaries(b_ob_value_out)\n\n\t\twith tf.name_scope(\"FCAdvOut\"):\n\t\t\tW_adv_out = weight_variable([512, ACTIONS])\n\t\t\tvariable_summaries(W_adv_out)\n\t\t\tb_adv_out = bias_variable([ACTIONS])\n\t\t\t# variable_summaries(b_ob_adv_out)\t\n\n\t\t# input layer\n\t\tself.state = tf.placeholder(\"float\", [None, DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, IMAGE_HIST])\n\t\t# Conv1 layer\n\t\th_conv1 = tf.nn.relu(conv2d(self.state, W_conv1, 8, 8) + b_conv1)\n\t\t# print('conv1:', h_conv1)\n\t\t# Conv2 layer\n\t\th_conv2 = tf.nn.relu(conv2d(h_conv1, W_conv2, 2, 2) + b_conv2)\n\t\t# print('conv2:', h_conv2)\n\t\t# Conv2 layer\n\t\th_conv3 = tf.nn.relu(conv2d(h_conv2, W_conv3, 1, 1) + b_conv3)\n\t\t# print('conv3:', h_conv3)\n\t\th_conv3_flat = tf.reshape(h_conv3, [-1, H_SIZE])\n\n\t\t# FC ob value layer\n\t\th_fc_value = tf.nn.relu(tf.matmul(h_conv3_flat, W_value) + b_value)\n\t\tvalue = tf.matmul(h_fc_value, W_value_out) + b_value_out\n\n\t\t# FC ob adv layer\n\t\th_fc_adv = tf.nn.relu(tf.matmul(h_conv3_flat, W_adv) + b_adv)\t\t\n\t\tadvantage = tf.matmul(h_fc_adv, W_adv_out) + b_adv_out\n\t\t\n\t\t# Q = value + (adv - advAvg)\n\t\tadvAvg = tf.expand_dims(tf.reduce_mean(advantage, axis=1), axis=1)\n\t\tadvIdentifiable = tf.subtract(advantage, advAvg)\n\t\tself.readout = tf.add(value, advIdentifiable)\n\n\t\t# define the ob cost function\n\t\tself.a = tf.placeholder(\"float\", [None, ACTIONS])\n\t\tself.y = tf.placeholder(\"float\", [None])\n\t\tself.readout_action = tf.reduce_sum(tf.multiply(self.readout, self.a), axis=1)\n\t\tself.td_error = tf.square(self.y - self.readout_action)\n\t\tself.cost = tf.reduce_mean(self.td_error)\n\t\tself.train_step = tf.train.AdamOptimizer(1e-4).minimize(self.cost)\n\ndef updateTargetGraph(tfVars,tau):\n\ttotal_vars = len(tfVars)\n\top_holder = []\n\tfor idx,var in enumerate(tfVars[0:total_vars/2]):\n\t\top_holder.append(tfVars[idx+total_vars/2].assign((var.value()*tau) + ((1-tau)*tfVars[idx+total_vars/2].value())))\n\treturn op_holder\n\ndef updateTarget(op_holder,sess):\n\tfor op in op_holder:\n\t\tsess.run(op)\n\ndef TestNetwork():\n\tsess = tf.InteractiveSession()\n\t# define depth_net \n\tdepth_state = tf.placeholder(\"float\", [None, RGB_IMAGE_HEIGHT, RGB_IMAGE_WIDTH, CHANNEL])\n\tdepth_net = models.ResNet50UpProj({'data': depth_state}, 1, 1, True)\n\tdepth_predict = tf.divide(depth_net.get_output(), 5.)\n\tdepth_net_var = tf.trainable_variables()\n\tprint(\" [*] printing DepthNet variables\")\n\tfor idx, v in enumerate(depth_net_var):\n\t\tprint(\" var {:3}: {:15} {}\".format(idx, str(v.get_shape()), v.name))\n\tdepth_net_saver = tf.train.Saver(depth_net_var, max_to_keep=1)\n\n\t# define q network\n\twith tf.device(\"/cpu:0\"):\n\t\twith tf.name_scope(\"OnlineNetwork\"):\n\t\t\tonline_net = QNetwork(sess, depth_predict)\n\t\twith tf.name_scope(\"TargetNetwork\"):\n\t\t\ttarget_net = QNetwork(sess, depth_predict)\n\trospy.sleep(1.)\n\n\treward_var = tf.Variable(0., trainable=False)\n\treward_epi = tf.summary.scalar('reward', reward_var)\n\t# define summary\n\tmerged_summary = tf.summary.merge_all()\n\tsummary_writer = tf.summary.FileWriter('./logs', sess.graph)\n\n\t# Initialize the World and variables\n\tenv = RealWorld()\n\tprint('Environment initialized')\n\t# init_op = tf.global_variables_initializer()\n\t# sess.run(init_op)\n\n\t# get the first state \n\trgb_img_t1 = env.GetRGBImageObservation()\n\tterminal = False\n\n\t# Loading depth network\n\tdepth_checkpoint = tf.train.get_checkpoint_state('saved_networks/depth')\n\tif depth_checkpoint and depth_checkpoint.model_checkpoint_path:\n\t\tprint('Loading from checkpoint:', depth_checkpoint)\n\t\tdepth_net_saver.restore(sess, depth_checkpoint.model_checkpoint_path)\n\t\tprint(\"Depth network model loaded:\", depth_checkpoint.model_checkpoint_path)\n\t\n\t# saving and loading Q networks\n\tepisode = 0\n\tq_net_params = []\n\n\t# Find variables of q network\n\tfor variable in tf.trainable_variables():\n\t\tvariable_name = variable.name\n\t\tif variable_name.find('OnlineNetwork') >= 0:\n\t\t\tq_net_params.append(variable)\n\tfor variable in tf.trainable_variables():\n\t\tvariable_name = variable.name\n\t\tif variable_name.find('TargetNetwork') >= 0:\n\t\t\tq_net_params.append(variable)\n\tprint(\" [*] printing QNetwork variables\")\n\tfor idx, v in enumerate(q_net_params):\n\t\tprint(\" var {:3}: {:15} {}\".format(idx, str(v.get_shape()), v.name))\n\tq_net_saver = tf.train.Saver(q_net_params, max_to_keep=1)\n\n\tcheckpoint = tf.train.get_checkpoint_state(\"saved_networks/Q\")\n\tprint('checkpoint:', checkpoint)\n\tif checkpoint and checkpoint.model_checkpoint_path:\n\t\tq_net_saver.restore(sess, checkpoint.model_checkpoint_path)\n\t\tprint(\"Q network successfully loaded:\", checkpoint.model_checkpoint_path)\n\telse:\n\t\tprint(\"Could not find old Q network weights\")\n\t\t\n\t# start training\n\tepsilon = INITIAL_EPSILON\n\tr_epi = 0.\n\tr_cache = []\n\trandom_flag = False\n\tt_max = 0\n\tt = 0\n\trate = rospy.Rate(5)\n\tloop_time = time.time()\n\tlast_loop_time = loop_time\n\twhile not rospy.is_shutdown():\n\t\tepisode += 1\n\t\tenv.ResetWorld()\n\t\tt = 0\n\t\tterminal = False\n\t\treset = False\n\t\taction_index = 0\n\t\t# first observation\n\t\trgb_img_t = env.GetRGBImageObservation()\n\t\tdepth_img_t1 = sess.run(depth_predict, feed_dict={depth_state: [rgb_img_t]})[0]\n\t\tdepth_img_t1 = np.reshape(depth_img_t1, (DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH))\n\t\tdepth_imgs_t1 = np.stack((depth_img_t1, depth_img_t1, depth_img_t1, depth_img_t1), axis=2)\n\t\twhile not rospy.is_shutdown():\n\t\t\trgb_img_t1 = env.GetRGBImageObservation()\n\t\t\treward_t, terminal, reset = env.GetRewardAndTerminate(t)\n\t\t\tdepth_imgs_t = depth_imgs_t1\n\t\t\trgb_img_t = rgb_img_t1\n\n\t\t\tdepth_img_t1 = sess.run(depth_predict, feed_dict={depth_state: [rgb_img_t1]})[0]\n\n\t\t\tenv.PublishDepthPrediction(depth_img_t1*5.)\n\n\t\t\tdepth_img_t1 = np.reshape(depth_img_t1, (DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, 1))\n\t\t\tdepth_imgs_t1 = np.append(depth_img_t1, depth_imgs_t1[:, :, :(IMAGE_HIST - 1)], axis=2)\n\n\t\t\t# choose an action epsilon greedily\n\t\t\ta = sess.run(online_net.readout, feed_dict = {online_net.state : [depth_imgs_t1]})\n\t\t\treadout_t = a[0]\n\t\t\ta_t = np.zeros([ACTIONS])\n\t\t\taction_index = np.argmax(readout_t)\n\t\t\ta_t[action_index] = 1\n\t\t\t# Control the agent\n\t\t\tenv.Control(action_index)\n\t\t\tprint('action:', action_index)\n\n\t\t\tt += 1\n\t\t\tlast_loop_time = loop_time\n\t\t\tloop_time = time.time()\n\t\t\trate.sleep()\n\n\ndef main():\n\tTestNetwork()\n\nif __name__ == \"__main__\":\n\tmain()\n\n" }, { "path": "D3QN/D3QN_training.py", "content": "from __future__ import print_function\nfrom GazeboWorld import GazeboWorld\n\nimport tensorflow as tf\nimport random\nimport numpy as np\nimport time\nimport rospy\n\nfrom collections import deque\n\nGAME = 'GazeboWorld'\nACTIONS = 7 # number of valid actions\nSPEED = 2 # DoF of speed\nGAMMA = 0.99 # decay rate of past observations\nOBSERVE = 10. # timesteps to observe before training\nEXPLORE = 20000. # frames over which to anneal epsilon\nFINAL_EPSILON = 0.0001 # final value of epsilon\nINITIAL_EPSILON = 0.1 # starting value of epsilon\nREPLAY_MEMORY = 50000 # number of previous transitions to remember\nBATCH = 8 # size of minibatch\nMAX_EPISODE = 20000\nMAX_T = 200\nDEPTH_IMAGE_WIDTH = 160\nDEPTH_IMAGE_HEIGHT = 128\nRGB_IMAGE_HEIGHT = 228\nRGB_IMAGE_WIDTH = 304\nCHANNEL = 3\nTAU = 0.001 # Rate to update target network toward primary network\nH_SIZE = 8*10*64\nIMAGE_HIST = 4\n\n\ndef variable_summaries(var):\n\t\"\"\"Attach a lot of summaries to a Tensor (for TensorBoard visualization).\"\"\"\n\twith tf.name_scope('summaries'):\n\t\tmean = tf.reduce_mean(var)\n\ttf.summary.scalar('mean', mean)\n\twith tf.name_scope('stddev'):\n\t\tstddev = tf.sqrt(tf.reduce_mean(tf.square(var - mean)))\n\ttf.summary.scalar('stddev', stddev)\n\ttf.summary.scalar('max', tf.reduce_max(var))\n\ttf.summary.scalar('min', tf.reduce_min(var))\n\ttf.summary.histogram('histogram', var)\n\t\t\ndef weight_variable(shape):\n\tinitial = tf.truncated_normal(shape, stddev = 0.01)\n\treturn tf.Variable(initial, name=\"weights\")\n\ndef bias_variable(shape):\n\tinitial = tf.constant(0., shape = shape)\n\treturn tf.Variable(initial, name=\"bias\")\n\ndef conv2d(x, W, stride_h, stride_w):\n\treturn tf.nn.conv2d(x, W, strides = [1, stride_h, stride_w, 1], padding = \"SAME\")\n\n\nclass QNetwork(object):\n\t\"\"\"docstring for ClassName\"\"\"\n\tdef __init__(self, sess):\n\t\t# network weights\n\t\t# input 128x160x1\n\t\twith tf.name_scope(\"Conv1\"):\n\t\t\tW_conv1 = weight_variable([10, 14, IMAGE_HIST, 32])\n\t\t\tvariable_summaries(W_conv1)\n\t\t\tb_conv1 = bias_variable([32])\n\t\t# 16x20x32\n\t\twith tf.name_scope(\"Conv2\"):\n\t\t\tW_conv2 = weight_variable([4, 4, 32, 64])\n\t\t\tvariable_summaries(W_conv2)\n\t\t\tb_conv2 = bias_variable([64])\n\t\t# 8x10x64\n\t\twith tf.name_scope(\"Conv3\"):\n\t\t\tW_conv3 = weight_variable([3, 3, 64, 64])\n\t\t\tvariable_summaries(W_conv3)\n\t\t\tb_conv3 = bias_variable([64])\n\t\t# 8x10x64\n\t\twith tf.name_scope(\"FCValue\"):\n\t\t\tW_value = weight_variable([H_SIZE, 512])\n\t\t\tvariable_summaries(W_value)\n\t\t\tb_value = bias_variable([512])\n\t\t\t# variable_summaries(b_ob_value)\n\n\t\twith tf.name_scope(\"FCAdv\"):\n\t\t\tW_adv = weight_variable([H_SIZE, 512])\n\t\t\tvariable_summaries(W_adv)\n\t\t\tb_adv = bias_variable([512])\n\t\t\t# variable_summaries(b_adv)\n\n\t\twith tf.name_scope(\"FCValueOut\"):\n\t\t\tW_value_out = weight_variable([512, 1])\n\t\t\tvariable_summaries(W_value_out)\n\t\t\tb_value_out = bias_variable([1])\n\t\t\t# variable_summaries(b_ob_value_out)\n\n\t\twith tf.name_scope(\"FCAdvOut\"):\n\t\t\tW_adv_out = weight_variable([512, ACTIONS])\n\t\t\tvariable_summaries(W_adv_out)\n\t\t\tb_adv_out = bias_variable([ACTIONS])\n\t\t\t# variable_summaries(b_ob_adv_out)\t\n\n\t\t# input layer\n\t\tself.state = tf.placeholder(\"float\", [None, DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, IMAGE_HIST])\n\t\t# Conv1 layer\n\t\th_conv1 = tf.nn.relu(conv2d(self.state, W_conv1, 8, 8) + b_conv1)\n\t\t# Conv2 layer\n\t\th_conv2 = tf.nn.relu(conv2d(h_conv1, W_conv2, 2, 2) + b_conv2)\n\t\t# Conv2 layer\n\t\th_conv3 = tf.nn.relu(conv2d(h_conv2, W_conv3, 1, 1) + b_conv3)\n\t\th_conv3_flat = tf.reshape(h_conv3, [-1, H_SIZE])\n\n\t\t# FC ob value layer\n\t\th_fc_value = tf.nn.relu(tf.matmul(h_conv3_flat, W_value) + b_value)\n\t\tvalue = tf.matmul(h_fc_value, W_value_out) + b_value_out\n\n\t\t# FC ob adv layer\n\t\th_fc_adv = tf.nn.relu(tf.matmul(h_conv3_flat, W_adv) + b_adv)\t\t\n\t\tadvantage = tf.matmul(h_fc_adv, W_adv_out) + b_adv_out\n\t\t\n\t\t# Q = value + (adv - advAvg)\n\t\tadvAvg = tf.expand_dims(tf.reduce_mean(advantage, axis=1), axis=1)\n\t\tadvIdentifiable = tf.subtract(advantage, advAvg)\n\t\tself.readout = tf.add(value, advIdentifiable)\n\n\t\t# define the ob cost function\n\t\tself.a = tf.placeholder(\"float\", [None, ACTIONS])\n\t\tself.y = tf.placeholder(\"float\", [None])\n\t\tself.readout_action = tf.reduce_sum(tf.multiply(self.readout, self.a), axis=1)\n\t\tself.td_error = tf.square(self.y - self.readout_action)\n\t\tself.cost = tf.reduce_mean(self.td_error)\n\t\tself.train_step = tf.train.AdamOptimizer(1e-4).minimize(self.cost)\n\ndef updateTargetGraph(tfVars,tau):\n\ttotal_vars = len(tfVars)\n\top_holder = []\n\tfor idx,var in enumerate(tfVars[0:total_vars/2]):\n\t\top_holder.append(tfVars[idx+total_vars/2].assign((var.value()*tau) + ((1-tau)*tfVars[idx+total_vars/2].value())))\n\treturn op_holder\n\ndef updateTarget(op_holder,sess):\n\tfor op in op_holder:\n\t\tsess.run(op)\n\ndef trainNetwork():\n\tsess = tf.InteractiveSession()\n\twith tf.name_scope(\"OnlineNetwork\"):\n\t\tonline_net = QNetwork(sess)\n\twith tf.name_scope(\"TargetNetwork\"):\n\t\ttarget_net = QNetwork(sess)\n\trospy.sleep(1.)\n\n\treward_var = tf.Variable(0., trainable=False)\n\treward_epi = tf.summary.scalar('reward', reward_var)\n\t# define summary\n\tmerged_summary = tf.summary.merge_all()\n\tsummary_writer = tf.summary.FileWriter('./logs', sess.graph)\n\n\t# Initialize the World\n\tenv = GazeboWorld()\n\tprint('Environment initialized')\n\n\t# Initialize the buffer\n\tD = deque()\n\n\t# get the first state \n\tdepth_img_t1 = env.GetDepthImageObservation()\n\tdepth_imgs_t1 = np.stack((depth_img_t1, depth_img_t1, depth_img_t1, depth_img_t1), axis=2)\n\tterminal = False\n\t\n\t# saving and loading networks\n\ttrainables = tf.trainable_variables()\n\ttrainable_saver = tf.train.Saver(trainables)\n\tsess.run(tf.global_variables_initializer())\n\tcheckpoint = tf.train.get_checkpoint_state(\"saved_networks/Q\")\n\tprint('checkpoint:', checkpoint)\n\tif checkpoint and checkpoint.model_checkpoint_path:\n\t\ttrainable_saver.restore(sess, checkpoint.model_checkpoint_path)\n\t\tprint(\"Successfully loaded:\", checkpoint.model_checkpoint_path)\n\telse:\n\t\tprint(\"Could not find old network weights\")\n\t\t\n\t# start training\n\tepisode = 0\n\tepsilon = INITIAL_EPSILON\n\tr_epi = 0.\n\tt = 0\n\tT = 0\n\trate = rospy.Rate(5)\n\tprint('Number of trainable variables:', len(trainables))\n\ttargetOps = updateTargetGraph(trainables,TAU)\n\tloop_time = time.time()\n\tlast_loop_time = loop_time\n\twhile episode < MAX_EPISODE and not rospy.is_shutdown():\n\t\tenv.ResetWorld()\n\t\tt = 0\n\t\tr_epi = 0.\n\t\tterminal = False\n\t\treset = False\n\t\tloop_time_buf = []\n\t\taction_index = 0\n\t\twhile not reset and not rospy.is_shutdown():\n\t\t\tdepth_img_t1 = env.GetDepthImageObservation()\n\t\t\tdepth_img_t1 = np.reshape(depth_img_t1, (DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, 1))\n\t\t\tdepth_imgs_t1 = np.append(depth_img_t1, depth_imgs_t1[:, :, :(IMAGE_HIST - 1)], axis=2)\n\t\t\treward_t, terminal, reset = env.GetRewardAndTerminate(t)\n\t\t\tif t > 0 :\n\t\t\t\tD.append((depth_imgs_t, a_t, reward_t, depth_imgs_t1, terminal))\n\t\t\t\tif len(D) > REPLAY_MEMORY:\n\t\t\t\t\tD.popleft()\n\t\t\tdepth_imgs_t = depth_imgs_t1\n\n\t\t\t# choose an action epsilon greedily\n\t\t\ta = sess.run(online_net.readout, feed_dict = {online_net.state : [depth_imgs_t1]})\n\t\t\treadout_t = a[0]\n\t\t\ta_t = np.zeros([ACTIONS])\n\t\t\tif episode <= OBSERVE:\n\t\t\t\taction_index = random.randrange(ACTIONS)\n\t\t\t\ta_t[action_index] = 1\n\t\t\telse:\n\t\t\t\tif random.random() <= epsilon:\n\t\t\t\t\tprint(\"----------Random Action----------\")\n\t\t\t\t\taction_index = random.randrange(ACTIONS)\n\t\t\t\t\ta_t[action_index] = 1\n\t\t\t\telse:\n\t\t\t\t\taction_index = np.argmax(readout_t)\n\t\t\t\t\ta_t[action_index] = 1\n\t\t\t# Control the agent\n\t\t\tenv.Control(action_index)\n\n\t\t\tif episode > OBSERVE :\n\t\t\t\t# # sample a minibatch to train on\n\t\t\t\tminibatch = random.sample(D, BATCH)\n\t\t\t\ty_batch = []\n\t\t\t\t# get the batch variables\n\t\t\t\tdepth_imgs_t_batch = [d[0] for d in minibatch]\n\t\t\t\ta_batch = [d[1] for d in minibatch]\n\t\t\t\tr_batch = [d[2] for d in minibatch]\n\t\t\t\tdepth_imgs_t1_batch = [d[3] for d in minibatch]\n\t\t\t\tQ1 = online_net.readout.eval(feed_dict = {online_net.state : depth_imgs_t1_batch})\n\t\t\t\tQ2 = target_net.readout.eval(feed_dict = {target_net.state : depth_imgs_t1_batch})\n\t\t\t\tfor i in range(0, len(minibatch)):\n\t\t\t\t\tterminal_batch = minibatch[i][4]\n\t\t\t\t\t# if terminal, only equals reward\n\t\t\t\t\tif terminal_batch:\n\t\t\t\t\t\ty_batch.append(r_batch[i])\n\t\t\t\t\telse:\n\t\t\t\t\t\ty_batch.append(r_batch[i] + GAMMA * Q2[i, np.argmax(Q1[i])])\n\n\t\t\t\t#Update the network with our target values.\n\t\t\t\tonline_net.train_step.run(feed_dict={online_net.y : y_batch,\n\t\t\t\t\t\t\t\t\t\t\t\t\tonline_net.a : a_batch,\n\t\t\t\t\t\t\t\t\t\t\t\t\tonline_net.state : depth_imgs_t_batch })\n\t\t\t\tupdateTarget(targetOps, sess) # Set the target network to be equal to the primary network.\n\n\t\t\tr_epi = r_epi + reward_t\n\t\t\tt += 1\n\t\t\tT += 1\n\t\t\tlast_loop_time = loop_time\n\t\t\tloop_time = time.time()\n\t\t\tloop_time_buf.append(loop_time - last_loop_time)\n\t\t\trate.sleep()\n\n\t\t\t# scale down epsilon\n\t\t\tif epsilon > FINAL_EPSILON and episode > OBSERVE:\n\t\t\t\tepsilon -= (INITIAL_EPSILON - FINAL_EPSILON) / EXPLORE\n\n\t\t# write summaries\n\t\tif episode > OBSERVE:\n\t\t\tsummary_str = sess.run(merged_summary, feed_dict={reward_var: r_epi})\n\t\t\tsummary_writer.add_summary(summary_str, episode - OBSERVE)\n\n\t\t# save progress every 500 episodes\n\t\tif (episode+1) % 500 == 0 :\n\t\t\ttrainable_saver.save(sess, 'saved_networks/' + GAME + '-dqn', global_step = episode)\n\n\t\tif len(loop_time_buf) == 0:\n\t\t\tprint(\"EPISODE\", episode, \"/ REWARD\", r_epi, \"/ steps \", T)\n\t\telse:\n\t\t\tprint(\"EPISODE\", episode, \"/ REWARD\", r_epi, \"/ steps \", T,\n\t\t\t\t\"/ LoopTime:\", np.mean(loop_time_buf))\n\n\t\tepisode = episode + 1\t\n\ndef main():\n\ttrainNetwork()\n\nif __name__ == \"__main__\":\n\tmain()\n\n" }, { "path": "D3QN/GazeboWorld.py", "content": "import rospy\nimport math\nimport time\nimport numpy as np\nimport cv2\nimport copy\nimport tf\nimport random\n\nfrom geometry_msgs.msg import Twist\nfrom geometry_msgs.msg import Quaternion\nfrom gazebo_msgs.msg import ModelStates\nfrom gazebo_msgs.msg import ModelState\nfrom sensor_msgs.msg import Image\nfrom cv_bridge import CvBridge, CvBridgeError\nfrom sensor_msgs.msg import LaserScan\nfrom nav_msgs.msg import Odometry\nfrom kobuki_msgs.msg import BumperEvent\n\nclass GazeboWorld():\n\tdef __init__(self):\n\t\t # initiliaze\n\t\trospy.init_node('GazeboWorld', anonymous=False)\n\n\t\t#-----------Default Robot State-----------------------\n\t\tself.set_self_state = ModelState()\n\t\tself.set_self_state.model_name = 'mobile_base' \n\t\tself.set_self_state.pose.position.x = 0.5\n\t\tself.set_self_state.pose.position.y = 0.\n\t\tself.set_self_state.pose.position.z = 0.\n\t\tself.set_self_state.pose.orientation.x = 0.0\n\t\tself.set_self_state.pose.orientation.y = 0.0\n\t\tself.set_self_state.pose.orientation.z = 0.0\n\t\tself.set_self_state.pose.orientation.w = 1.0\n\t\tself.set_self_state.twist.linear.x = 0.\n\t\tself.set_self_state.twist.linear.y = 0.\n\t\tself.set_self_state.twist.linear.z = 0.\n\t\tself.set_self_state.twist.angular.x = 0.\n\t\tself.set_self_state.twist.angular.y = 0.\n\t\tself.set_self_state.twist.angular.z = 0.\n\t\tself.set_self_state.reference_frame = 'world'\n\n\t\t#------------Params--------------------\n\t\tself.depth_image_size = [160, 128]\n\t\tself.rgb_image_size = [304, 228]\n\t\tself.bridge = CvBridge()\n\n\t\tself.object_state = [0, 0, 0, 0]\n\t\tself.object_name = []\n\n\t\t# 0. | left 90/s | left 45/s | right 45/s | right 90/s | acc 1/s | slow down -1/s\n\t\tself.action_table = [0.4, 0.2, np.pi/6, np.pi/12, 0., -np.pi/12, -np.pi/6]\n\t\t\n\t\tself.self_speed = [.4, 0.0]\n\t\tself.default_states = None\n\t\t\n\t\tself.start_time = time.time()\n\t\tself.max_steps = 10000\n\n\t\tself.depth_image = None\n\t\tself.bump = False\n\n\t\t#-----------Publisher and Subscriber-------------\n\t\tself.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size = 10)\n\t\tself.set_state = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size = 10)\n\t\tself.resized_depth_img = rospy.Publisher('camera/depth/image_resized',Image, queue_size = 10)\n\t\tself.resized_rgb_img = rospy.Publisher('camera/rgb/image_resized',Image, queue_size = 10)\n\n\t\tself.object_state_sub = rospy.Subscriber('gazebo/model_states', ModelStates, self.ModelStateCallBack)\n\t\tself.depth_image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, self.RGBImageCallBack)\n\t\tself.rgb_image_sub = rospy.Subscriber('camera/depth/image_raw', Image, self.DepthImageCallBack)\n\t\tself.laser_sub = rospy.Subscriber('scan', LaserScan, self.LaserScanCallBack)\n\t\tself.odom_sub = rospy.Subscriber('odom', Odometry, self.OdometryCallBack)\n\t\tself.bumper_sub = rospy.Subscriber('mobile_base/events/bumper', BumperEvent, self.BumperCallBack)\n\n\t\trospy.sleep(2.)\n\n\t\t# What function to call when you ctrl + c \n\t\trospy.on_shutdown(self.shutdown)\n\n\n\tdef ModelStateCallBack(self, data):\n\t\t# self state\n\t\tidx = data.name.index(\"mobile_base\")\n\t\tquaternion = (data.pose[idx].orientation.x,\n\t\t\t\t\t data.pose[idx].orientation.y,\n\t\t\t\t\t data.pose[idx].orientation.z,\n\t\t\t\t\t data.pose[idx].orientation.w)\n\t\teuler = tf.transformations.euler_from_quaternion(quaternion)\n\t\troll = euler[0]\n\t\tpitch = euler[1]\n\t\tyaw = euler[2]\n\t\tself.self_state = [data.pose[idx].position.x, \n\t\t\t\t\t \t data.pose[idx].position.y,\n\t\t\t\t\t \t yaw,\n\t\t\t\t\t \t data.twist[idx].linear.x,\n\t\t\t\t\t\t data.twist[idx].linear.y,\n\t\t\t\t\t\t data.twist[idx].angular.z]\n\t\tfor lp in xrange(len(self.object_name)):\n\t\t\tidx = data.name.index(self.object_name[lp])\n\t\t\tquaternion = (data.pose[idx].orientation.x,\n\t\t\t\t\t\t data.pose[idx].orientation.y,\n\t\t\t\t\t\t data.pose[idx].orientation.z,\n\t\t\t\t\t\t data.pose[idx].orientation.w)\n\t\t\teuler = tf.transformations.euler_from_quaternion(quaternion)\n\t\t\troll = euler[0]\n\t\t\tpitch = euler[1]\n\t\t\tyaw = euler[2]\n\n\t\t\tself.object_state[lp] = [data.pose[idx].position.x, \n\t\t\t\t\t\t\t\t\t data.pose[idx].position.y,\n\t\t\t\t\t\t\t\t\t yaw]\n\n\t\tif self.default_states is None:\n\t\t\tself.default_states = copy.deepcopy(data)\n\n\n\tdef DepthImageCallBack(self, img):\n\t\tself.depth_image = img\n\n\tdef RGBImageCallBack(self, img):\n\t\tself.rgb_image = img\n\n\tdef LaserScanCallBack(self, scan):\n\t\tself.scan_param = [scan.angle_min, scan.angle_max, scan.angle_increment, scan.time_increment,\n\t\t\t\t\t\t scan.scan_time, scan.range_min, scan. range_max]\n\t\tself.scan = np.array(scan.ranges)\n\n\tdef OdometryCallBack(self, odometry):\n\t\tself.self_linear_x_speed = odometry.twist.twist.linear.x\n\t\tself.self_linear_y_speed = odometry.twist.twist.linear.y\n\t\tself.self_rotation_z_speed = odometry.twist.twist.angular.z\n\n\tdef BumperCallBack(self, bumper_data):\n\t\tif bumper_data.state == BumperEvent.PRESSED:\n\t\t\tself.bump = True\n\t\telse:\n\t\t\tself.bump = False\n\n\tdef GetDepthImageObservation(self):\n\t\t# ros image to cv2 image\n\n\t\ttry:\n\t\t\tcv_img = self.bridge.imgmsg_to_cv2(self.depth_image, \"32FC1\")\n\t\texcept Exception as e:\n\t\t\traise e\n\t\ttry:\n\t\t\tcv_rgb_img = self.bridge.imgmsg_to_cv2(self.rgb_image, \"bgr8\")\n\t\texcept Exception as e:\n\t\t\traise e\n\t\tcv_img = np.array(cv_img, dtype=np.float32)\n\t\t# resize\n\t\tdim = (self.depth_image_size[0], self.depth_image_size[1])\n\t\tcv_img = cv2.resize(cv_img, dim, interpolation = cv2.INTER_AREA)\n\n\t\tcv_img[np.isnan(cv_img)] = 0.\n\t\tcv_img[cv_img < 0.4] = 0.\n\t\tcv_img/=(10./255.)\n\n\t\t# cv_img/=(10000./255.)\n\t\t# print 'max:', np.amax(cv_img), 'min:', np.amin(cv_img)\n\t\t# cv_img[cv_img > 5.] = -1.\n\n\t\t# # inpainting\n\t\t# mask = copy.deepcopy(cv_img)\n\t\t# mask[mask == 0.] = 1.\n\t\t# mask[mask != 1.] = 0.\n\t\t# mask = np.uint8(mask)\n\t\t# cv_img = cv2.inpaint(np.uint8(cv_img), mask, 3, cv2.INPAINT_TELEA)\n\n\t\t# # guassian noise\n\t\t# gauss = np.random.normal(0., 0.5, dim)\n\t\t# gauss = gauss.reshape(dim[1], dim[0])\n\t\t# cv_img = np.array(cv_img, dtype=np.float32)\n\t\t# cv_img = cv_img + gauss\n\t\t# cv_img[cv_img<0.00001] = 0.\n\n\t\t# # smoothing\n\t\t# kernel = np.ones((4,4),np.float32)/16\n\t\t# cv_img = cv2.filter2D(cv_img,-1,kernel)\n\n\n\t\tcv_img = np.array(cv_img, dtype=np.float32)\n\t\tcv_img*=(10./255.)\n\n\t\t# cv2 image to ros image and publish\n\t\ttry:\n\t\t\tresized_img = self.bridge.cv2_to_imgmsg(cv_img, \"passthrough\")\n\t\texcept Exception as e:\n\t\t\traise e\n\t\tself.resized_depth_img.publish(resized_img)\n\t\treturn(cv_img/5.)\n\n\tdef GetRGBImageObservation(self):\n\t\t# ros image to cv2 image\n\t\ttry:\n\t\t\tcv_img = self.bridge.imgmsg_to_cv2(self.rgb_image, \"bgr8\")\n\t\texcept Exception as e:\n\t\t\traise e\n\t\t# resize\n\t\tdim = (self.rgb_image_size[0], self.rgb_image_size[1])\n\t\tcv_resized_img = cv2.resize(cv_img, dim, interpolation = cv2.INTER_AREA)\n\t\t# cv2 image to ros image and publish\n\t\ttry:\n\t\t\tresized_img = self.bridge.cv2_to_imgmsg(cv_resized_img, \"bgr8\")\n\t\texcept Exception as e:\n\t\t\traise e\n\t\tself.resized_rgb_img.publish(resized_img)\n\t\treturn(cv_resized_img)\n\n\tdef PublishDepthPrediction(self, depth_img):\n\t\t# cv2 image to ros image and publish\n\t\tcv_img = np.array(depth_img, dtype=np.float32)\n\t\ttry:\n\t\t\tresized_img = self.bridge.cv2_to_imgmsg(cv_img, \"passthrough\")\n\t\texcept Exception as e:\n\t\t\traise e\n\t\tself.resized_depth_img.publish(resized_img)\n\n\tdef GetLaserObservation(self):\n\t\tscan = copy.deepcopy(self.scan)\n\t\tscan[np.isnan(scan)] = 30.\n\t\treturn scan\n\n\tdef GetSelfState(self):\n\t\treturn self.self_state;\n\n\tdef GetSelfLinearXSpeed(self):\n\t\treturn self.self_linear_x_speed\n\n\tdef GetSelfOdomeSpeed(self):\n\t\tv = np.sqrt(self.self_linear_x_speed**2 + self.self_linear_y_speed**2)\n\t\treturn [v, self.self_rotation_z_speed]\n\n\tdef GetTargetState(self, name):\n\t\treturn self.object_state[self.TargetName.index(name)]\n\n\tdef GetSelfSpeed(self):\n\t\treturn np.array(self.self_speed)\n\n\tdef GetBump(self):\n\t\treturn self.bump\n\n\tdef SetObjectPose(self, name='mobile_base', random_flag=False):\n\t\tquaternion = tf.transformations.quaternion_from_euler(0., 0., np.random.uniform(-np.pi, np.pi))\n\t\tif name is 'mobile_base' :\n\t\t\tobject_state = copy.deepcopy(self.set_self_state)\n\t\t\tobject_state.pose.orientation.x = quaternion[0]\n\t\t\tobject_state.pose.orientation.y = quaternion[1]\n\t\t\tobject_state.pose.orientation.z = quaternion[2]\n\t\t\tobject_state.pose.orientation.w = quaternion[3]\n\t\telse:\n\t\t\tobject_state = self.States2State(self.default_states, name)\n\n\t\tself.set_state.publish(object_state)\n\n\tdef States2State(self, states, name):\n\t\tto_state = ModelState()\n\t\tfrom_states = copy.deepcopy(states)\n\t\tidx = from_states.name.index(name)\n\t\tto_state.model_name = name\n\t\tto_state.pose = from_states.pose[idx]\n\t\tto_state.twist = from_states.twist[idx]\n\t\tto_state.reference_frame = 'world'\n\t\treturn to_state\n\n\n\tdef ResetWorld(self):\n\t\tself.SetObjectPose() # reset robot\n\t\tfor x in xrange(len(self.object_name)): \n\t\t\tself.SetObjectPose(self.object_name[x]) # reset target\n\t\tself.self_speed = [.4, 0.0]\n\t\tself.step_target = [0., 0.]\n\t\tself.step_r_cnt = 0.\n\t\tself.start_time = time.time()\n\t\trospy.sleep(0.5)\n\n\tdef Control(self, action):\n\t\tif action <2:\n\t\t\tself.self_speed[0] = self.action_table[action]\n\t\t\t# self.self_speed[1] = 0.\n\t\telse:\n\t\t\tself.self_speed[1] = self.action_table[action]\n\t\tmove_cmd = Twist()\n\t\tmove_cmd.linear.x = self.self_speed[0]\n\t\tmove_cmd.linear.y = 0.\n\t\tmove_cmd.linear.z = 0.\n\t\tmove_cmd.angular.x = 0.\n\t\tmove_cmd.angular.y = 0.\n\t\tmove_cmd.angular.z = self.self_speed[1]\n\t\tself.cmd_vel.publish(move_cmd)\n\n\tdef shutdown(self):\n\t\t# stop turtlebot\n\t\trospy.loginfo(\"Stop Moving\")\n\t\tself.cmd_vel.publish(Twist())\n\t\trospy.sleep(1)\n\n\tdef GetRewardAndTerminate(self, t):\n\t\tterminate = False\n\t\treset = False\n\t\t[v, theta] = self.GetSelfOdomeSpeed()\n\t\tlaser = self.GetLaserObservation()\n\t\treward = v * np.cos(theta) * 0.2 - 0.01\n\n\t\tif self.GetBump() or np.amin(laser) < 0.46 or np.amin(laser) == 30.:\n\t\t\treward = -10.\n\t\t\tterminate = True\n\t\t\treset = True\n\t\tif t > 500:\n\t\t\treset = True\n\n\t\treturn reward, terminate, reset\t\n\t\n\t\t" }, { "path": "D3QN/RealWorld.py", "content": "import rospy\nimport math\nimport time\nimport numpy as np\nimport cv2\nimport copy\nimport tf\nimport random\n\nfrom geometry_msgs.msg import Twist\nfrom geometry_msgs.msg import Quaternion\nfrom gazebo_msgs.msg import ModelStates\nfrom gazebo_msgs.msg import ModelState\nfrom sensor_msgs.msg import Image\nfrom cv_bridge import CvBridge, CvBridgeError\nfrom sensor_msgs.msg import LaserScan\nfrom nav_msgs.msg import Odometry\nfrom kobuki_msgs.msg import BumperEvent\n\nclass RealWorld():\n\tdef __init__(self):\n\t\t # initiliaze\n\t\trospy.init_node('RealWorld', anonymous=False)\n\n\t\t#-----------Default Robot State-----------------------\n\t\tself.set_self_state = ModelState()\n\t\tself.set_self_state.model_name = 'mobile_base' \n\t\tself.set_self_state.pose.position.x = 0.5\n\t\tself.set_self_state.pose.position.y = 0.\n\t\tself.set_self_state.pose.position.z = 0.\n\t\tself.set_self_state.pose.orientation.x = 0.0\n\t\tself.set_self_state.pose.orientation.y = 0.0\n\t\tself.set_self_state.pose.orientation.z = 0.0\n\t\tself.set_self_state.pose.orientation.w = 1.0\n\t\tself.set_self_state.twist.linear.x = 0.\n\t\tself.set_self_state.twist.linear.y = 0.\n\t\tself.set_self_state.twist.linear.z = 0.\n\t\tself.set_self_state.twist.angular.x = 0.\n\t\tself.set_self_state.twist.angular.y = 0.\n\t\tself.set_self_state.twist.angular.z = 0.\n\t\tself.set_self_state.reference_frame = 'world'\n\n\t\t#------------Params--------------------\n\t\tself.depth_image_size = [160, 128]\n\t\tself.rgb_image_size = [304, 228]\n\t\tself.bridge = CvBridge()\n\n\t\tself.object_state = [0, 0, 0, 0]\n\t\tself.object_name = []\n\t\n\t\tself.action_table = [0.4, 0.2, np.pi/6, np.pi/12, 0., -np.pi/12, -np.pi/6]\n\t\tself.self_speed = [.4, 0.0]\n\t\tself.default_states = None\n\t\t\n\t\tself.start_time = time.time()\n\t\tself.max_steps = 10000\n\n\t\tself.depth_image = None\n\t\tself.laser_cb_num = 0\n\n\t\tself.rot_counter = 0\n\n\t\tself.now_phase = 1\n\t\tself.next_phase = 4\n\t\tself.step_target = [0., 0.]\n\t\tself.step_r_cnt = 0.\n\t\tself.bump = None\n\t\tself.action = 0\n\t\t\n\t\t#-----------Publisher and Subscriber-------------\n\t\tself.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size = 10)\n\t\tself.set_state = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size = 10)\n\t\tself.resized_depth_img = rospy.Publisher('camera/depth/image_resized',Image, queue_size = 10)\n\t\tself.resized_rgb_img = rospy.Publisher('camera/rgb/image_resized',Image, queue_size = 10)\n\n\t\tself.object_state_sub = rospy.Subscriber('gazebo/model_states', ModelStates, self.ModelStateCallBack)\n\t\tself.depth_image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, self.RGBImageCallBack)\n\t\tself.rgb_image_sub = rospy.Subscriber('camera/depth/image_raw', Image, self.DepthImageCallBack)\n\t\tself.laser_sub = rospy.Subscriber('scan', LaserScan, self.LaserScanCallBack)\n\t\tself.odom_sub = rospy.Subscriber('odom', Odometry, self.OdometryCallBack)\n\t\tself.bumper_sub = rospy.Subscriber('mobile_base/events/bumper', BumperEvent, self.BumperCallBack)\n\t\t# Wait until the first callback\n\t\t# while self.depth_image is None:\n\t\t# \tpass\n\t\trospy.sleep(2.)\n\t\t# What function to call when you ctrl + c \n\t\trospy.on_shutdown(self.shutdown)\n\n\n\tdef ModelStateCallBack(self, data):\n\t\t# self state\n\t\tidx = data.name.index(\"mobile_base\")\n\t\tquaternion = (data.pose[idx].orientation.x,\n\t\t\t\t\t data.pose[idx].orientation.y,\n\t\t\t\t\t data.pose[idx].orientation.z,\n\t\t\t\t\t data.pose[idx].orientation.w)\n\t\teuler = tf.transformations.euler_from_quaternion(quaternion)\n\t\troll = euler[0]\n\t\tpitch = euler[1]\n\t\tyaw = euler[2]\n\t\tself.self_state = [data.pose[idx].position.x, \n\t\t\t\t\t \t data.pose[idx].position.y,\n\t\t\t\t\t \t yaw,\n\t\t\t\t\t \t data.twist[idx].linear.x,\n\t\t\t\t\t\t data.twist[idx].linear.y,\n\t\t\t\t\t\t data.twist[idx].angular.z]\n\t\t\n\t\tfor lp in xrange(len(self.object_name)):\n\t\t\tidx = data.name.index(self.object_name[lp])\n\t\t\tquaternion = (data.pose[idx].orientation.x,\n\t\t\t\t\t\t data.pose[idx].orientation.y,\n\t\t\t\t\t\t data.pose[idx].orientation.z,\n\t\t\t\t\t\t data.pose[idx].orientation.w)\n\t\t\teuler = tf.transformations.euler_from_quaternion(quaternion)\n\t\t\troll = euler[0]\n\t\t\tpitch = euler[1]\n\t\t\tyaw = euler[2]\n\n\t\t\tself.object_state[lp] = [data.pose[idx].position.x, \n\t\t\t\t\t\t\t\t\t data.pose[idx].position.y,\n\t\t\t\t\t\t\t\t\t yaw]\n\n\t\tif self.default_states is None:\n\t\t\tself.default_states = copy.deepcopy(data)\n\n\n\tdef DepthImageCallBack(self, img):\n\t\tself.depth_image = img\n\n\tdef RGBImageCallBack(self, img):\n\t\tself.rgb_image = img\n\n\tdef LaserScanCallBack(self, scan):\n\t\tself.scan_param = [scan.angle_min, scan.angle_max, scan.angle_increment, scan.time_increment,\n\t\t\t\t\t\t scan.scan_time, scan.range_min, scan. range_max]\n\t\tself.scan = np.array(scan.ranges)\n\t\tself.laser_cb_num += 1\n\n\tdef OdometryCallBack(self, odometry):\n\t\tself.self_linear_x_speed = odometry.twist.twist.linear.x\n\t\tself.self_linear_y_speed = odometry.twist.twist.linear.y\n\t\tself.self_rotation_z_speed = odometry.twist.twist.angular.z\n\n\tdef BumperCallBack(self, bumper_data):\n\t\tif bumper_data.state == BumperEvent.PRESSED:\n\t\t\tself.bump = True\n\t\telse:\n\t\t\tself.bump = False\n\n\tdef sim_noise(self, depthFile, rgbFile):\n\t\timg=depthFile\n\t\timgcol=rgbFile\n\n\t\tedges = cv2.Canny(img,100,200,apertureSize = 3)\n\t\tedgescol = cv2.Canny(imgcol,100,200,apertureSize = 3)\n\t\tedges += edgescol\n\n\t\tmask=img.copy()\n\t\tmask.fill(0)\n\t\tminLineLength = 10\n\t\tmaxLineGap = 10\n\t\tlines = cv2.HoughLinesP(edges,1,np.pi/180,20,100,10)\n\t\tif lines is not None:\n\t\t\tfor line in lines:\n\t\t\t\tfor x1,y1,x2,y2 in line:\n\t\t\t\t\tcv2.line(mask,(x1,y1),(x2,y2),255,1)\n\n\t\t\tfor i in range(480):\n\t\t\t\tfor j in range(640):\n\t\t\t\t\tif mask[i][j]>0:\n\t\t\t\t\t\tcv2.circle(img,(j,i),2, (0,0,0), -1)\n\t\t\t\t\t\tif random.random()>0.8:\n\t\t\t\t\t\t\tcv2.circle(img,(j,i), random.randint(2,6), (0,0,0), -1)\n\t\treturn img\n\n\tdef GetDepthImageObservation(self):\n\t\t# ros image to cv2 image\n\t\ttry:\n\t\t\tcv_img = self.bridge.imgmsg_to_cv2(self.depth_image, \"32FC1\")\n\t\texcept Exception as e:\n\t\t\traise e\n\t\t# try:\n\t\t# \tcv_rgb_img = self.bridge.imgmsg_to_cv2(self.rgb_image, \"bgr8\")\n\t\t# except Exception as e:\n\t\t# \traise e\n\t\tcv_img = np.array(cv_img, dtype=np.float32)\n\n\t\tcv_img[np.isnan(cv_img)] = 0.\n\t\t# cv_img/=(10./255.)\n\t\tcv_img/=(10000./255.)\n\t\t# print 'max:', np.amax(cv_img), 'min:', np.amin(cv_img)\n\t\t# cv_img[cv_img > 5.] = -1.\n\t\t# cv_img[cv_img < 0.4] = 0.\n\n\t\t# inpainting\n\t\tmask = copy.deepcopy(cv_img)\n\t\tmask[mask == 0.] = 1.\n\t\tmask[mask != 1.] = 0.\n\t\t# print 'mask sum:', np.sum(mask)\n\t\tmask = np.uint8(mask)\n\t\tcv_img = cv2.inpaint(np.uint8(cv_img), mask, 3, cv2.INPAINT_TELEA)\n\n\t\tcv_img = np.array(cv_img, dtype=np.float32)\n\t\t# cv_img*=(10./255.)\n\t\tcv_img*=(10./255.)\n\t\t# resize\n\t\tdim = (self.depth_image_size[0], self.depth_image_size[1])\n\t\tcv_img = cv2.resize(cv_img, dim, interpolation = cv2.INTER_AREA)\n\n\t\t# cv2 image to ros image and publish\n\t\ttry:\n\t\t\tresized_img = self.bridge.cv2_to_imgmsg(cv_img, \"passthrough\")\n\t\texcept Exception as e:\n\t\t\traise e\n\t\tself.resized_depth_img.publish(resized_img)\n\t\treturn(cv_img/5.)\n\n\tdef GetRGBImageObservation(self):\n\t\t# ros image to cv2 image\n\t\ttry:\n\t\t\tcv_img = self.bridge.imgmsg_to_cv2(self.rgb_image, \"bgr8\")\n\t\texcept Exception as e:\n\t\t\traise e\n\t\t# resize\n\t\tdim = (self.rgb_image_size[0], self.rgb_image_size[1])\n\t\tcv_resized_img = cv2.resize(cv_img, dim, interpolation = cv2.INTER_AREA)\n\t\t# cv2 image to ros image and publish\n\t\ttry:\n\t\t\tresized_img = self.bridge.cv2_to_imgmsg(cv_resized_img, \"bgr8\")\n\t\texcept Exception as e:\n\t\t\traise e\n\t\tself.resized_rgb_img.publish(resized_img)\n\t\treturn(cv_resized_img)\n\n\tdef PublishDepthPrediction(self, depth_img):\n\t\t# cv2 image to ros image and publish\n\t\tcv_img = np.array(depth_img, dtype=np.float32)\n\t\ttry:\n\t\t\tresized_img = self.bridge.cv2_to_imgmsg(cv_img, \"passthrough\")\n\t\texcept Exception as e:\n\t\t\traise e\n\t\tself.resized_depth_img.publish(resized_img)\n\n\tdef GetLaserObservation(self):\n\t\tscan = copy.deepcopy(self.scan)\n\t\tscan[np.isnan(scan)] = 30.\n\t\treturn scan\n\n\tdef GetSelfState(self):\n\t\treturn self.self_state;\n\n\tdef GetSelfLinearXSpeed(self):\n\t\treturn self.self_linear_x_speed\n\n\tdef GetSelfOdomeSpeed(self):\n\t\tv = np.sqrt(self.self_linear_x_speed**2 + self.self_linear_y_speed**2)\n\t\treturn [v, self.self_rotation_z_speed]\n\n\tdef GetTargetState(self, name):\n\t\treturn self.object_state[self.TargetName.index(name)]\n\n\tdef GetSelfSpeed(self):\n\t\treturn np.array(self.self_speed)\n\n\tdef GetBump(self):\n\t\treturn self.bump\n\n\tdef SetObjectPose(self, name='mobile_base', random_flag=False):\n\t\tquaternion = tf.transformations.quaternion_from_euler(0., 0., np.random.uniform(-np.pi, np.pi))\n\t\tif name is 'mobile_base' :\n\t\t\tobject_state = copy.deepcopy(self.set_self_state)\n\t\t\tobject_state.pose.orientation.x = quaternion[0]\n\t\t\tobject_state.pose.orientation.y = quaternion[1]\n\t\t\tobject_state.pose.orientation.z = quaternion[2]\n\t\t\tobject_state.pose.orientation.w = quaternion[3]\n\t\telse:\n\t\t\tobject_state = self.States2State(self.default_states, name)\n\n\t\tself.set_state.publish(object_state)\n\n\tdef States2State(self, states, name):\n\t\tto_state = ModelState()\n\t\tfrom_states = copy.deepcopy(states)\n\t\tidx = from_states.name.index(name)\n\t\tto_state.model_name = name\n\t\tto_state.pose = from_states.pose[idx]\n\t\tto_state.twist = from_states.twist[idx]\n\t\tto_state.reference_frame = 'world'\n\t\treturn to_state\n\n\tdef ResetWorld(self):\n\t\tself.SetObjectPose() # reset robot\n\t\tfor x in xrange(len(self.object_name)): \n\t\t\tself.SetObjectPose(self.object_name[x]) # reset target\n\t\tself.self_speed = [.4, 0.0]\n\t\tself.step_target = [0., 0.]\n\t\tself.step_r_cnt = 0.\n\t\tself.start_time = time.time()\n\t\trospy.sleep(0.5)\n\n\tdef Control(self, action):\n\t\tif action <2:\n\t\t\tself.self_speed[0] = self.action_table[0]\n\t\telse:\n\t\t\tself.self_speed[1] = self.action_table[action]\n\t\tmove_cmd = Twist()\n\t\tmove_cmd.linear.x = self.self_speed[0]/2\n\t\tmove_cmd.linear.y = 0.\n\t\tmove_cmd.linear.z = 0.\n\t\tmove_cmd.angular.x = 0.\n\t\tmove_cmd.angular.y = 0.\n\t\tmove_cmd.angular.z = self.self_speed[1]/2\n\t\tself.cmd_vel.publish(move_cmd)\n\n\tdef shutdown(self):\n\t\t# stop turtlebot\n\t\trospy.loginfo(\"Stop Moving\")\n\t\tself.cmd_vel.publish(Twist())\n\t\trospy.sleep(1)\n\n\tdef GetRewardAndTerminate(self, t):\n\t\tterminate = False\n\t\treset = False\n\t\t[v, theta] = self.GetSelfOdomeSpeed()\n\t\t# laser = self.GetLaserObservation()\n\t\treward = v * np.cos(theta * 2) * 0.2 - 0.01\n\n\t\tif self.GetBump() :\n\t\t\treward = -10.\n\t\t\tterminate = True\n\t\t\treset = True\n\t\tif t > 500:\n\t\t\treset = True\n\n\t\treturn reward, terminate, reset\t\n\n\tdef GetTargetPoint(self):\n\t\t# r = random.uniform(8., 10.)\n\t\t# theta = random.uniform(-np.pi, np.py)\n\t\t# x = np.cos(theta) * r\n\t\t# y = np.sin(theta) * r\n\t\tx = random.uniform(-4.5, 4.5)\n\n\t\tif np.fabs(x) >= 3.5:\n\t\t\ty = random.uniform(-4.5, 4.5)\n\t\telse:\n\t\t\ty = random.uniform(3.5, 4.5)\n\t\t\tif random.uniform(-1., 1.) < 0.:\n\t\t\t\ty = -y\n\t\tself.target_point = [x, y]\n\t\tself.init_dist = np.sqrt(x**2 + y**2)\n\t\tself.last_distance = np.sqrt(x**2 + y**2)\n\t\treturn self.target_point\n" }, { "path": "D3QN/models/__init__.py", "content": "from .fcrn import ResNet50UpProj\n" }, { "path": "D3QN/models/fcrn.py", "content": "from .network import Network\n\nclass ResNet50UpProj(Network):\n def setup(self):\n # 1\n trainable_flag = True\n (self.feed('data')\n .conv(7, 7, 64, 2, 2, relu=False, name='conv1', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn_conv1', trainable=trainable_flag)\n .max_pool(3, 3, 2, 2, name='pool1')\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res2a_branch1', trainable=trainable_flag)\n .batch_normalization(name='bn2a_branch1', trainable=trainable_flag))\n # 2\n trainable_flag = True\n (self.feed('pool1')\n .conv(1, 1, 64, 1, 1, biased=False, relu=False, name='res2a_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn2a_branch2a', trainable=trainable_flag)\n .conv(3, 3, 64, 1, 1, biased=False, relu=False, name='res2a_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn2a_branch2b', trainable=trainable_flag)\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res2a_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn2a_branch2c', trainable=trainable_flag))\n # 3\n trainable_flag = True\n (self.feed('bn2a_branch1',\n 'bn2a_branch2c')\n .add(name='res2a')\n .relu(name='res2a_relu')\n .conv(1, 1, 64, 1, 1, biased=False, relu=False, name='res2b_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn2b_branch2a', trainable=trainable_flag)\n .conv(3, 3, 64, 1, 1, biased=False, relu=False, name='res2b_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn2b_branch2b', trainable=trainable_flag)\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res2b_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn2b_branch2c', trainable=trainable_flag))\n # 4\n trainable_flag = True\n (self.feed('res2a_relu',\n 'bn2b_branch2c')\n .add(name='res2b')\n .relu(name='res2b_relu')\n .conv(1, 1, 64, 1, 1, biased=False, relu=False, name='res2c_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn2c_branch2a', trainable=trainable_flag)\n .conv(3, 3, 64, 1, 1, biased=False, relu=False, name='res2c_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn2c_branch2b', trainable=trainable_flag)\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res2c_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn2c_branch2c', trainable=trainable_flag))\n # 5\n trainable_flag = True\n (self.feed('res2b_relu',\n 'bn2c_branch2c')\n .add(name='res2c')\n .relu(name='res2c_relu')\n .conv(1, 1, 512, 2, 2, biased=False, relu=False, name='res3a_branch1', trainable=trainable_flag)\n .batch_normalization(name='bn3a_branch1', trainable=trainable_flag))\n # 6\n trainable_flag = True\n (self.feed('res2c_relu')\n .conv(1, 1, 128, 2, 2, biased=False, relu=False, name='res3a_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3a_branch2a', trainable=trainable_flag)\n .conv(3, 3, 128, 1, 1, biased=False, relu=False, name='res3a_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3a_branch2b', trainable=trainable_flag)\n .conv(1, 1, 512, 1, 1, biased=False, relu=False, name='res3a_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn3a_branch2c', trainable=trainable_flag))\n # 7\n trainable_flag = True\n (self.feed('bn3a_branch1',\n 'bn3a_branch2c')\n .add(name='res3a')\n .relu(name='res3a_relu')\n .conv(1, 1, 128, 1, 1, biased=False, relu=False, name='res3b_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3b_branch2a', trainable=trainable_flag)\n .conv(3, 3, 128, 1, 1, biased=False, relu=False, name='res3b_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3b_branch2b', trainable=trainable_flag)\n .conv(1, 1, 512, 1, 1, biased=False, relu=False, name='res3b_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn3b_branch2c', trainable=trainable_flag))\n # 8\n trainable_flag = True\n (self.feed('res3a_relu',\n 'bn3b_branch2c')\n .add(name='res3b')\n .relu(name='res3b_relu')\n .conv(1, 1, 128, 1, 1, biased=False, relu=False, name='res3c_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3c_branch2a', trainable=trainable_flag)\n .conv(3, 3, 128, 1, 1, biased=False, relu=False, name='res3c_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3c_branch2b', trainable=trainable_flag)\n .conv(1, 1, 512, 1, 1, biased=False, relu=False, name='res3c_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn3c_branch2c', trainable=trainable_flag))\n # 9\n trainable_flag = True\n (self.feed('res3b_relu',\n 'bn3c_branch2c')\n .add(name='res3c')\n .relu(name='res3c_relu')\n .conv(1, 1, 128, 1, 1, biased=False, relu=False, name='res3d_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3d_branch2a', trainable=trainable_flag)\n .conv(3, 3, 128, 1, 1, biased=False, relu=False, name='res3d_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3d_branch2b', trainable=trainable_flag)\n .conv(1, 1, 512, 1, 1, biased=False, relu=False, name='res3d_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn3d_branch2c', trainable=trainable_flag))\n # 10\n trainable_flag = True\n (self.feed('res3c_relu',\n 'bn3d_branch2c')\n .add(name='res3d')\n .relu(name='res3d_relu')\n .conv(1, 1, 1024, 2, 2, biased=False, relu=False, name='res4a_branch1', trainable=trainable_flag)\n .batch_normalization(name='bn4a_branch1', trainable=trainable_flag))\n # 11\n trainable_flag = True\n (self.feed('res3d_relu')\n .conv(1, 1, 256, 2, 2, biased=False, relu=False, name='res4a_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4a_branch2a', trainable=trainable_flag)\n .conv(3, 3, 256, 1, 1, biased=False, relu=False, name='res4a_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4a_branch2b', trainable=trainable_flag)\n .conv(1, 1, 1024, 1, 1, biased=False, relu=False, name='res4a_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn4a_branch2c', trainable=trainable_flag))\n # 12 \n (self.feed('bn4a_branch1',\n 'bn4a_branch2c')\n .add(name='res4a')\n .relu(name='res4a_relu')\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res4b_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4b_branch2a', trainable=trainable_flag)\n .conv(3, 3, 256, 1, 1, biased=False, relu=False, name='res4b_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4b_branch2b', trainable=trainable_flag)\n .conv(1, 1, 1024, 1, 1, biased=False, relu=False, name='res4b_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn4b_branch2c', trainable=trainable_flag))\n # 13\n trainable_flag = True\n (self.feed('res4a_relu',\n 'bn4b_branch2c')\n .add(name='res4b')\n .relu(name='res4b_relu')\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res4c_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4c_branch2a', trainable=trainable_flag)\n .conv(3, 3, 256, 1, 1, biased=False, relu=False, name='res4c_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4c_branch2b', trainable=trainable_flag)\n .conv(1, 1, 1024, 1, 1, biased=False, relu=False, name='res4c_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn4c_branch2c', trainable=trainable_flag))\n # 14\n (self.feed('res4b_relu',\n 'bn4c_branch2c')\n .add(name='res4c')\n .relu(name='res4c_relu')\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res4d_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4d_branch2a', trainable=trainable_flag)\n .conv(3, 3, 256, 1, 1, biased=False, relu=False, name='res4d_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4d_branch2b', trainable=trainable_flag)\n .conv(1, 1, 1024, 1, 1, biased=False, relu=False, name='res4d_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn4d_branch2c', trainable=trainable_flag))\n # 15\n trainable_flag = True\n (self.feed('res4c_relu',\n 'bn4d_branch2c')\n .add(name='res4d')\n .relu(name='res4d_relu')\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res4e_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4e_branch2a', trainable=trainable_flag)\n .conv(3, 3, 256, 1, 1, biased=False, relu=False, name='res4e_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4e_branch2b', trainable=trainable_flag)\n .conv(1, 1, 1024, 1, 1, biased=False, relu=False, name='res4e_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn4e_branch2c', trainable=trainable_flag))\n # 16\n (self.feed('res4d_relu',\n 'bn4e_branch2c')\n .add(name='res4e')\n .relu(name='res4e_relu')\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res4f_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4f_branch2a', trainable=trainable_flag)\n .conv(3, 3, 256, 1, 1, biased=False, relu=False, name='res4f_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4f_branch2b', trainable=trainable_flag)\n .conv(1, 1, 1024, 1, 1, biased=False, relu=False, name='res4f_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn4f_branch2c', trainable=trainable_flag))\n # 17\n trainable_flag = True\n (self.feed('res4e_relu',\n 'bn4f_branch2c')\n .add(name='res4f')\n .relu(name='res4f_relu')\n .conv(1, 1, 2048, 2, 2, biased=False, relu=False, name='res5a_branch1', trainable=trainable_flag)\n .batch_normalization(name='bn5a_branch1', trainable=trainable_flag))\n # 18\n (self.feed('res4f_relu')\n .conv(1, 1, 512, 2, 2, biased=False, relu=False, name='res5a_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn5a_branch2a', trainable=trainable_flag)\n .conv(3, 3, 512, 1, 1, biased=False, relu=False, name='res5a_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn5a_branch2b', trainable=trainable_flag)\n .conv(1, 1, 2048, 1, 1, biased=False, relu=False, name='res5a_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn5a_branch2c', trainable=trainable_flag))\n # 19\n trainable_flag = True\n (self.feed('bn5a_branch1',\n 'bn5a_branch2c')\n .add(name='res5a')\n .relu(name='res5a_relu')\n .conv(1, 1, 512, 1, 1, biased=False, relu=False, name='res5b_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn5b_branch2a', trainable=trainable_flag)\n .conv(3, 3, 512, 1, 1, biased=False, relu=False, name='res5b_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn5b_branch2b', trainable=trainable_flag)\n .conv(1, 1, 2048, 1, 1, biased=False, relu=False, name='res5b_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn5b_branch2c', trainable=trainable_flag))\n # 20\n (self.feed('res5a_relu',\n 'bn5b_branch2c')\n .add(name='res5b')\n .relu(name='res5b_relu')\n .conv(1, 1, 512, 1, 1, biased=False, relu=False, name='res5c_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn5c_branch2a', trainable=trainable_flag)\n .conv(3, 3, 512, 1, 1, biased=False, relu=False, name='res5c_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn5c_branch2b', trainable=trainable_flag)\n .conv(1, 1, 2048, 1, 1, biased=False, relu=False, name='res5c_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn5c_branch2c', trainable=trainable_flag))\n # 21\n trainable_flag = True\n (self.feed('res5b_relu',\n 'bn5c_branch2c')\n .add(name='res5c')\n .relu(name='res5c_relu')\n .conv(1, 1, 1024, 1, 1, biased=True, relu=False, name='layer1', trainable=trainable_flag)\n .batch_normalization(relu=False, name='layer1_BN', trainable=trainable_flag)\n .up_project([3, 3, 1024, 512], id = '2x', stride = 1, BN=True, trainable=trainable_flag)\n .up_project([3, 3, 512, 256], id = '4x', stride = 1, BN=True, trainable=trainable_flag)\n .up_project([3, 3, 256, 128], id = '8x', stride = 1, BN=True, trainable=trainable_flag)\n .up_project([3, 3, 128, 64], id = '16x', stride = 1, BN=True, trainable=trainable_flag)\n .dropout(name = 'drop', keep_prob = 1.)\n .conv(3, 3, 1, 1, 1, name = 'ConvPred', trainable=trainable_flag))\n" }, { "path": "D3QN/models/network.py", "content": "import numpy as np\nimport tensorflow as tf\n\n# ----------------------------------------------------------------------------------\n# Commonly used layers and operations based on ethereon's implementation \n# https://github.com/ethereon/caffe-tensorflow\n# Slight modifications may apply. FCRN-specific operations have also been appended. \n# ----------------------------------------------------------------------------------\n# Thanks to *Helisa Dhamo* for the model conversion and integration into TensorFlow.\n# ----------------------------------------------------------------------------------\n\nDEFAULT_PADDING = 'SAME'\n\n\ndef get_incoming_shape(incoming):\n \"\"\" Returns the incoming data shape \"\"\"\n if isinstance(incoming, tf.Tensor):\n return incoming.get_shape().as_list()\n elif type(incoming) in [np.array, list, tuple]:\n return np.shape(incoming)\n else:\n raise Exception(\"Invalid incoming layer.\")\n\n\ndef interleave(tensors, axis):\n old_shape = get_incoming_shape(tensors[0])[1:]\n new_shape = [-1] + old_shape\n new_shape[axis] *= len(tensors)\n return tf.reshape(tf.stack(tensors, axis + 1), new_shape)\n\ndef layer(op):\n '''Decorator for composable network layers.'''\n\n def layer_decorated(self, *args, **kwargs):\n # Automatically set a name if not provided.\n name = kwargs.setdefault('name', self.get_unique_name(op.__name__))\n\n # Figure out the layer inputs.\n if len(self.terminals) == 0:\n raise RuntimeError('No input variables found for layer %s.' % name)\n elif len(self.terminals) == 1:\n layer_input = self.terminals[0]\n else:\n layer_input = list(self.terminals)\n # Perform the operation and get the output.\n layer_output = op(self, layer_input, *args, **kwargs)\n # Add to layer LUT.\n self.layers[name] = layer_output\n # This output is now the input for the next layer.\n self.feed(layer_output)\n # Return self for chained calls.\n return self\n\n return layer_decorated\n\n\nclass Network(object):\n\n def __init__(self, inputs, batch, keep_prob, is_training, trainable=False):\n # The input nodes for this network\n self.inputs = inputs\n # The current list of terminal nodes\n self.terminals = []\n # Mapping from layer names to layers\n self.layers = dict(inputs)\n # If true, the resulting variables are set as trainable\n self.trainable = trainable\n self.batch_size = batch\n self.keep_prob = keep_prob\n self.is_training = is_training\n self.setup()\n\n\n def setup(self):\n '''Construct the network. '''\n raise NotImplementedError('Must be implemented by the subclass.')\n\n def load(self, data_path, session, ignore_missing=False):\n '''Load network weights.\n data_path: The path to the numpy-serialized network weights\n session: The current TensorFlow session\n ignore_missing: If true, serialized weights for missing layers are ignored.\n '''\n data_dict = np.load(data_path, encoding='latin1').item()\n for op_name in data_dict: \n with tf.variable_scope(op_name, reuse=True):\n for param_name, data in iter(data_dict[op_name].items()): \n try:\n var = tf.get_variable(param_name)\n session.run(var.assign(data))\n\n except ValueError:\n if not ignore_missing:\n raise\n\n def feed(self, *args):\n '''Set the input(s) for the next operation by replacing the terminal nodes.\n The arguments can be either layer names or the actual layers.\n '''\n assert len(args) != 0\n self.terminals = []\n for fed_layer in args:\n if isinstance(fed_layer, str):\n try:\n fed_layer = self.layers[fed_layer]\n except KeyError:\n raise KeyError('Unknown layer name fed: %s' % fed_layer)\n self.terminals.append(fed_layer)\n return self\n\n def get_output(self):\n '''Returns the current network output.'''\n return self.terminals[-1]\n\n def get_layer_output(self, name):\n return self.layers[name]\n\n def get_unique_name(self, prefix):\n '''Returns an index-suffixed unique name for the given prefix.\n This is used for auto-generating layer names based on the type-prefix.\n '''\n ident = sum(t.startswith(prefix) for t, _ in self.layers.items()) + 1\n return '%s_%d' % (prefix, ident)\n\n def make_var(self, name, shape, trainable=False):\n '''Creates a new TensorFlow variable.'''\n return tf.get_variable(name, shape, dtype = 'float32', trainable=trainable)\n\n def validate_padding(self, padding):\n '''Verifies that the padding is one of the supported ones.'''\n assert padding in ('SAME', 'VALID')\n\n @layer\n def conv(self,\n input_data,\n k_h,\n k_w,\n c_o,\n s_h,\n s_w,\n name,\n relu=True,\n padding=DEFAULT_PADDING,\n group=1,\n biased=True,\n trainable=False):\n\n # Verify that the padding is acceptable\n self.validate_padding(padding)\n # Get the number of channels in the input\n c_i = input_data.get_shape()[-1]\n\n if (padding == 'SAME'):\n input_data = tf.pad(input_data, [[0, 0], [(k_h - 1)//2, (k_h - 1)//2], [(k_w - 1)//2, (k_w - 1)//2], [0, 0]], \"CONSTANT\")\n \n # Verify that the grouping parameter is valid\n assert c_i % group == 0\n assert c_o % group == 0\n # Convolution for a given input and kernel\n convolve = lambda i, k: tf.nn.conv2d(i, k, [1, s_h, s_w, 1], padding='VALID')\n \n with tf.variable_scope(name) as scope:\n kernel = self.make_var('weights', shape=[k_h, k_w, c_i // group, c_o], trainable=trainable)\n\n if group == 1:\n # This is the common-case. Convolve the input without any further complications.\n output = convolve(input_data, kernel)\n else:\n # Split the input into groups and then convolve each of them independently\n\n input_groups = tf.split(3, group, input_data)\n kernel_groups = tf.split(3, group, kernel)\n output_groups = [convolve(i, k) for i, k in zip(input_groups, kernel_groups)]\n # Concatenate the groups\n output = tf.concat(3, output_groups)\n\n # Add the biases\n if biased:\n biases = self.make_var('biases', [c_o], trainable=trainable)\n output = tf.nn.bias_add(output, biases)\n if relu:\n # ReLU non-linearity\n output = tf.nn.relu(output, name=scope.name)\n\n return output\n\n @layer\n def relu(self, input_data, name):\n return tf.nn.relu(input_data, name=name)\n\n @layer\n def max_pool(self, input_data, k_h, k_w, s_h, s_w, name, padding=DEFAULT_PADDING):\n self.validate_padding(padding)\n return tf.nn.max_pool(input_data,\n ksize=[1, k_h, k_w, 1],\n strides=[1, s_h, s_w, 1],\n padding=padding,\n name=name)\n\n @layer\n def avg_pool(self, input_data, k_h, k_w, s_h, s_w, name, padding=DEFAULT_PADDING):\n self.validate_padding(padding)\n return tf.nn.avg_pool(input_data,\n ksize=[1, k_h, k_w, 1],\n strides=[1, s_h, s_w, 1],\n padding=padding,\n name=name)\n\n @layer\n def lrn(self, input_data, radius, alpha, beta, name, bias=1.0):\n return tf.nn.local_response_normalization(input_data,\n depth_radius=radius,\n alpha=alpha,\n beta=beta,\n bias=bias,\n name=name)\n\n @layer\n def concat(self, inputs, axis, name):\n return tf.concat(concat_dim=axis, values=inputs, name=name)\n\n @layer\n def add(self, inputs, name):\n return tf.add_n(inputs, name=name)\n\n @layer\n def fc(self, input_data, num_out, name, relu=True):\n with tf.variable_scope(name) as scope:\n input_shape = input_data.get_shape()\n if input_shape.ndims == 4:\n # The input is spatial. Vectorize it first.\n dim = 1\n for d in input_shape[1:].as_list():\n dim *= d\n feed_in = tf.reshape(input_data, [-1, dim])\n else:\n feed_in, dim = (input_data, input_shape[-1].value)\n weights = self.make_var('weights', shape=[dim, num_out])\n biases = self.make_var('biases', [num_out])\n op = tf.nn.relu_layer if relu else tf.nn.xw_plus_b\n fc = op(feed_in, weights, biases, name=scope.name)\n return fc\n\n @layer\n def softmax(self, input_data, name):\n input_shape = map(lambda v: v.value, input_data.get_shape())\n if len(input_shape) > 2:\n # For certain models (like NiN), the singleton spatial dimensions\n # need to be explicitly squeezed, since they're not broadcast-able\n # in TensorFlow's NHWC ordering (unlike Caffe's NCHW).\n if input_shape[1] == 1 and input_shape[2] == 1:\n input_data = tf.squeeze(input_data, squeeze_dims=[1, 2])\n else:\n raise ValueError('Rank 2 tensor input expected for softmax!')\n return tf.nn.softmax(input_data, name)\n\n @layer\n def batch_normalization(self, input_data, name, scale_offset=True, relu=False, trainable=False):\n\n with tf.variable_scope(name) as scope:\n shape = [input_data.get_shape()[-1]]\n pop_mean = tf.get_variable(\"mean\", shape, initializer = tf.constant_initializer(0.0), trainable=trainable)\n pop_var = tf.get_variable(\"variance\", shape, initializer = tf.constant_initializer(1.0), trainable=trainable)\n epsilon = 1e-4\n decay = 0.999\n if scale_offset:\n scale = tf.get_variable(\"scale\", shape, initializer = tf.constant_initializer(1.0), trainable=trainable)\n offset = tf.get_variable(\"offset\", shape, initializer = tf.constant_initializer(0.0), trainable=trainable)\n else:\n scale, offset = (None, None)\n if self.is_training:\n batch_mean, batch_var = tf.nn.moments(input_data, [0, 1, 2])\n\n train_mean = tf.assign(pop_mean,\n pop_mean * decay + batch_mean * (1 - decay))\n train_var = tf.assign(pop_var,\n pop_var * decay + batch_var * (1 - decay))\n with tf.control_dependencies([train_mean, train_var]):\n output = tf.nn.batch_normalization(input_data,\n batch_mean, batch_var, offset, scale, epsilon, name = name)\n else:\n output = tf.nn.batch_normalization(input_data,\n pop_mean, pop_var, offset, scale, epsilon, name = name)\n\n if relu:\n output = tf.nn.relu(output)\n\n return output\n\n @layer\n def dropout(self, input_data, keep_prob, name):\n return tf.nn.dropout(input_data, keep_prob, name=name)\n \n\n def unpool_as_conv(self, size, input_data, id, stride = 1, ReLU = False, BN = True, trainable=False):\n\n\t\t# Model upconvolutions (unpooling + convolution) as interleaving feature\n\t\t# maps of four convolutions (A,B,C,D). Building block for up-projections. \n\n\n # Convolution A (3x3)\n # --------------------------------------------------\n layerName = \"layer%s_ConvA\" % (id)\n self.feed(input_data)\n self.conv( 3, 3, size[3], stride, stride, name = layerName, padding = 'SAME', relu = False, trainable=trainable)\n outputA = self.get_output()\n\n # Convolution B (2x3)\n # --------------------------------------------------\n layerName = \"layer%s_ConvB\" % (id)\n padded_input_B = tf.pad(input_data, [[0, 0], [1, 0], [1, 1], [0, 0]], \"CONSTANT\")\n self.feed(padded_input_B)\n self.conv(2, 3, size[3], stride, stride, name = layerName, padding = 'VALID', relu = False, trainable=trainable)\n outputB = self.get_output()\n\n # Convolution C (3x2)\n # --------------------------------------------------\n layerName = \"layer%s_ConvC\" % (id)\n padded_input_C = tf.pad(input_data, [[0, 0], [1, 1], [1, 0], [0, 0]], \"CONSTANT\")\n self.feed(padded_input_C)\n self.conv(3, 2, size[3], stride, stride, name = layerName, padding = 'VALID', relu = False, trainable=trainable)\n outputC = self.get_output()\n\n # Convolution D (2x2)\n # --------------------------------------------------\n layerName = \"layer%s_ConvD\" % (id)\n padded_input_D = tf.pad(input_data, [[0, 0], [1, 0], [1, 0], [0, 0]], \"CONSTANT\")\n self.feed(padded_input_D)\n self.conv(2, 2, size[3], stride, stride, name = layerName, padding = 'VALID', relu = False, trainable=trainable)\n outputD = self.get_output()\n\n # Interleaving elements of the four feature maps\n # --------------------------------------------------\n left = interleave([outputA, outputB], axis=1) # columns\n right = interleave([outputC, outputD], axis=1) # columns\n Y = interleave([left, right], axis=2) # rows\n \n if BN:\n layerName = \"layer%s_BN\" % (id)\n self.feed(Y)\n self.batch_normalization(name=layerName, scale_offset=True, relu=False, trainable=trainable)\n Y = self.get_output()\n\n if ReLU:\n Y = tf.nn.relu(Y, name = layerName)\n \n return Y\n\n\n def up_project(self, size, id, stride = 1, BN = True, trainable=False):\n \n # Create residual upsampling layer (UpProjection)\n\n input_data = self.get_output()\n\n # Branch 1\n id_br1 = \"%s_br1\" % (id)\n\n # Interleaving Convs of 1st branch\n out = self.unpool_as_conv(size, input_data, id_br1, stride, ReLU=True, BN=True, trainable=trainable)\n\n # Convolution following the upProjection on the 1st branch\n layerName = \"layer%s_Conv\" % (id)\n self.feed(out)\n self.conv(size[0], size[1], size[3], stride, stride, name = layerName, relu = False, trainable=trainable)\n\n if BN:\n layerName = \"layer%s_BN\" % (id)\n self.batch_normalization(name=layerName, scale_offset=True, relu=False, trainable=trainable)\n\n # Output of 1st branch\n branch1_output = self.get_output()\n\n \n # Branch 2\n id_br2 = \"%s_br2\" % (id)\n # Interleaving convolutions and output of 2nd branch\n branch2_output = self.unpool_as_conv(size, input_data, id_br2, stride, ReLU=False, trainable=trainable)\n\n \n # sum branches\n layerName = \"layer%s_Sum\" % (id)\n output = tf.add_n([branch1_output, branch2_output], name = layerName)\n # ReLU\n layerName = \"layer%s_ReLU\" % (id)\n output = tf.nn.relu(output, name=layerName)\n\n self.feed(output)\n return self\n" }, { "path": "D3QN/world/SquareWorld.world", "content": "\n \n \n 1\n 0 0 10 0 -0 0\n 0.8 0.8 0.8 1\n 0.2 0.2 0.2 1\n \n 1000\n 0.9\n 0.01\n 0.001\n \n 0.5 0.1 -0.9\n \n \n 0.4 0.4 0.4 1\n 0.7 0.7 0.7 1\n 1\n \n \n 0.01\n 1\n 100\n \n \n 1\n \n \n \n \n 0 0 1\n 100 100\n \n \n \n \n \n 100\n 50\n \n \n \n \n \n \n \n \n \n \n 10\n \n \n 0\n \n \n 0 0 1\n 100 100\n \n \n \n \n \n \n \n 0\n 0\n \n 0\n 0\n 1\n \n 0.497681 0 0 0 -0 0\n \n \n 17585 380000000\n 593 732830907\n 1494623668 929573495\n 58808\n \n -0.037988 0.034396 0 0 -0 0\n 1 1 1\n \n -2.18999 1.8914 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n -2.19299 1.7984 0 0 0 -1.5708\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n 2.68601 3.0634 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n 3.25351 2.6559 0 0 0 -1.54994\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n 2.71201 2.2484 0 0 -0 3.14159\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n 2.14451 2.6559 0 0 -0 1.61372\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n -2.93375 -2.45648 0 0 -0 1.09459\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n -2.00876 -2.45648 0 0 0 -1.01325\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n -2.42752 -3.25087 0 0 -0 3.14159\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n 2.35014 -2.44607 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n 3.16763 -1.25358 0 0 -0 1.5708\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n -0.037988 5.4594 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n 5.38701 0.034396 0 0 0 -1.5708\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n -0.037988 -5.3906 0 0 -0 3.14159\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n -5.46299 0.034396 0 0 -0 1.5708\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n \n 4.76137 -3.62422 0 0 -0 0\n 1 1 1\n \n 4.76137 -3.62422 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n \n 4.98173 5.02016 0 0 -0 0\n 1 1 1\n \n 4.98173 5.02016 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n \n -3.75684 3.84985 0 0 -0 0\n 1 1 1\n \n -3.75684 3.84985 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n \n 0.497681 0 0 0 -0 0\n 1 1 1\n \n 0.497681 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n \n -4.48069 -0.04466 0 0 -0 0\n 1 1 1\n \n -4.48069 -0.04466 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n 0 0 0 0 -0 0\n \n \n \n 0 0 10 0 -0 0\n \n \n \n \n -0.19454 -5.8041 19.5357 0 1.3098 1.55175\n orbit\n perspective\n \n \n 0 0 -9.8\n 6e-06 2.3e-05 -4.2e-05\n \n \n EARTH_WGS84\n 0\n 0\n 0\n 0\n \n \n -1.685 -2.396 0 0 -0 0\n \n \n \n \n 2.5 0.15 1\n \n \n 0 0 0.5 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 0.5 0 -0 0\n \n \n 2.5 0.15 1\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n -2.152 1.857 0 0 -0 0\n 0\n 0\n \n \n \n \n \n 2.75 0.15 1\n \n \n 0 0 0.5 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 0.5 0 -0 0\n \n \n 2.75 0.15 1\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n -2.155 1.764 0 0 0 -1.5708\n 0\n 0\n \n \n \n \n \n 1.2678 0.15 1\n \n \n 0 0 0.5 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 0.5 0 -0 0\n \n \n 1.2678 0.15 1\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n 2.724 3.029 0 0 -0 0\n 0\n 0\n \n \n \n \n \n 0.965005 0.15 1\n \n \n 0 0 0.5 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 0.5 0 -0 0\n \n \n 0.965005 0.15 1\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n 3.2915 2.6215 0 0 0 -1.54994\n 0\n 0\n \n \n \n \n \n 1.25 0.15 1\n \n \n 0 0 0.5 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 0.5 0 -0 0\n \n \n 1.25 0.15 1\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n 2.75 2.214 0 0 -0 3.14159\n 0\n 0\n \n \n \n \n \n 0.965005 0.15 1\n \n \n 0 0 0.5 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 0.5 0 -0 0\n \n \n 0.965005 0.15 1\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n 2.1825 2.6215 0 0 -0 1.61372\n 0\n 0\n \n \n \n \n \n 1.97704 0.15 1\n \n \n 0 0 0.5 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 0.5 0 -0 0\n \n \n 1.97704 0.15 1\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n -2.89576 -2.49088 0 0 -0 1.09459\n 0\n 0\n \n \n \n \n \n 2.06356 0.15 1\n \n \n 0 0 0.5 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 0.5 0 -0 0\n \n \n 2.06356 0.15 1\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n -1.97077 -2.49088 0 0 0 -1.01325\n 0\n 0\n \n \n \n \n \n 2 0.15 1\n \n \n 0 0 0.5 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 0.5 0 -0 0\n \n \n 2 0.15 1\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n -2.38953 -3.28527 0 0 -0 3.14159\n 0\n 0\n \n \n \n \n \n 1.75 0.15 1\n \n \n 0 0 0.5 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 0.5 0 -0 0\n \n \n 1.75 0.15 1\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n 2.38813 -2.48047 0 0 -0 0\n 0\n 0\n \n \n \n \n \n 2.5 0.15 1\n \n \n 0 0 0.5 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 0.5 0 -0 0\n \n \n 2.5 0.15 1\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n 3.20562 -1.28798 0 0 -0 1.5708\n 0\n 0\n \n \n \n \n \n 11 0.15 2\n \n \n 0 0 1 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 1 0 -0 0\n \n \n 11 0.15 2\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n 0 5.425 0 0 -0 0\n 0\n 0\n \n \n \n \n \n 11 0.15 2\n \n \n 0 0 1 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 1 0 -0 0\n \n \n 11 0.15 2\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n 5.425 0 0 0 0 -1.5708\n 0\n 0\n \n \n \n \n \n 11 0.15 2\n \n \n 0 0 1 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 1 0 -0 0\n \n \n 11 0.15 2\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n 0 -5.425 0 0 -0 3.14159\n 0\n 0\n \n \n \n \n \n 11 0.15 2\n \n \n 0 0 1 0 -0 0\n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 1 0 -0 0\n \n \n 11 0.15 2\n \n \n \n \n 1 1 1 1\n \n \n 0\n \n \n -5.425 0 0 0 -0 1.5708\n 0\n 0\n \n 1\n \n \n 1\n \n \n 0 0 -1 0 -0 0\n \n 2.05\n 0\n 0\n 2.05\n 0\n 2.05\n \n 25\n \n \n 0 0 0.01 0 -0 0\n \n \n 0.45 0.45 0.02\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 0.01 0 -0 0\n \n \n 0.45 0.45 0.02\n \n \n \n \n \n \n \n 0.235 0 0.51 0 -0 0\n \n \n 0.02 0.45 1.02\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0.235 0 0.51 0 -0 0\n \n \n 0.02 0.45 1.02\n \n \n \n \n \n \n \n 0 0.235 0.51 0 -0 0\n \n \n 0.45 0.02 1.02\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0.235 0.51 0 -0 0\n \n \n 0.45 0.02 1.02\n \n \n \n \n \n \n \n 0 0 0.51 0 -0 0\n \n \n 0.45 0.45 0.02\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 0.51 0 -0 0\n \n \n 0.45 0.45 0.02\n \n \n \n \n \n \n \n 0 -0.235 0.51 0 -0 0\n \n \n 0.45 0.02 1.02\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 -0.235 0.51 0 -0 0\n \n \n 0.45 0.02 1.02\n \n \n \n \n \n \n \n 0 0 1.01 0 -0 0\n \n \n 0.45 0.45 0.02\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0 1.01 0 -0 0\n \n \n 0.45 0.45 0.02\n \n \n \n \n \n \n 0\n 0\n \n 4.98173 5.02016 0 0 -0 0\n \n \n 1\n \n \n 1\n \n \n 0 0.005 0.6 0 -0 0\n \n \n 0.9 0.01 1.2\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 0.005 0.6 0 -0 0\n \n \n 0.9 0.01 1.2\n \n \n \n \n \n \n \n 0.45 -0.195 0.6 0 -0 0\n \n \n 0.02 0.4 1.2\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0.45 -0.195 0.6 0 -0 0\n \n \n 0.02 0.4 1.2\n \n \n \n \n \n \n \n -0.45 -0.195 0.6 0 -0 0\n \n \n 0.02 0.4 1.2\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n -0.45 -0.195 0.6 0 -0 0\n \n \n 0.02 0.4 1.2\n \n \n \n \n \n \n \n 0 -0.195 0.03 0 -0 0\n \n \n 0.88 0.4 0.06\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 -0.195 0.03 0 -0 0\n \n \n 0.88 0.4 0.06\n \n \n \n \n \n \n \n 0 -0.195 1.19 0 -0 0\n \n \n 0.88 0.4 0.02\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 -0.195 1.19 0 -0 0\n \n \n 0.88 0.4 0.02\n \n \n \n \n \n \n \n 0 -0.195 0.43 0 -0 0\n \n \n 0.88 0.4 0.02\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 -0.195 0.43 0 -0 0\n \n \n 0.88 0.4 0.02\n \n \n \n \n \n \n \n 0 -0.195 0.8 0 -0 0\n \n \n 0.88 0.4 0.02\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0 -0.195 0.8 0 -0 0\n \n \n 0.88 0.4 0.02\n \n \n \n \n \n \n 0\n 0\n \n 4.51394 -3.63417 0 0 -0 0\n \n \n 1\n \n \n 0 0 1 0 -0 0\n \n \n 1.5 0.8 0.03\n \n \n \n \n \n 0.6\n 0.6\n \n \n \n \n \n \n \n \n \n \n 10\n \n \n 0 0 1 0 -0 0\n \n \n 1.5 0.8 0.03\n \n \n \n \n \n \n \n 0.68 0.38 0.5 0 -0 0\n \n \n 0.02\n 1\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0.68 0.38 0.5 0 -0 0\n \n \n 0.02\n 1\n \n \n \n \n \n \n \n 0.68 -0.38 0.5 0 -0 0\n \n \n 0.02\n 1\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0.68 -0.38 0.5 0 -0 0\n \n \n 0.02\n 1\n \n \n \n \n \n \n \n -0.68 -0.38 0.5 0 -0 0\n \n \n 0.02\n 1\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n -0.68 -0.38 0.5 0 -0 0\n \n \n 0.02\n 1\n \n \n \n \n \n \n \n -0.68 0.38 0.5 0 -0 0\n \n \n 0.02\n 1\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n -0.68 0.38 0.5 0 -0 0\n \n \n 0.02\n 1\n \n \n \n \n \n \n 0\n 0\n \n -4.48069 -0.04466 0 0 -0 0\n \n \n 1\n \n \n \n \n model://fire_hydrant/meshes/fire_hydrant.dae\n 1 1 1\n \n \n 10\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n model://fire_hydrant/meshes/fire_hydrant.dae\n 1 1 1\n \n \n \n 0\n 0\n \n -3.75684 3.84985 0 0 -0 0\n \n \n\n" }, { "path": "Depth/DataPreprocess.py", "content": "import numpy as np \nimport cv2\nimport rospy\nimport random\nimport copy\nimport pickle\n\nfrom collections import deque\nfrom pathlib import Path\n\ndef ismember(A, B):\n return [ np.sum(a == B) for a in A ]\n\ndef rgb_data_color_aug(rgb_images):\n\trgb_images = np.asarray(rgb_images).astype(float)\n\tnoise = np.random.rand(4) * 0.4 + 0.8\n\tprint noise\n\trgb_images = rgb_images * noise[0]\n\tfor x in xrange(3):\n\t\trgb_images[:, :, :, x] = rgb_images[:, :, :, x] * noise[x + 1]\n\trgb_images[rgb_images>255.] = 255.\n\trgb_images = np.uint8(rgb_images)\n\treturn rgb_images\n\ndef crop_img(img):\n\tsize = np.shape(img)\n\talpha = 0.1\n\theight_start = int(size[0]*alpha/1.5)\n\theight_end = size[0] - int(size[0]*alpha/2.1)\n\talpha = 0.15\n\twidth_start = int(size[1]*alpha/2.5)\n\twidth_end = size[1] - int(size[1]*alpha/1.8)\n\t\t\t\n\tif len(size) == 2:\n\t\tcropped_img = img[height_start : height_end, width_start : width_end]\n\telif len(size) == 3:\n\t\tcropped_img = img[height_start : height_end, width_start : width_end, :]\n\n\treturn cropped_img\n\nfile_num = 1000\nD_training = deque()\nD_testing = deque()\ntesting = 0.3\ncnt = 0\npath = './data/'\ninpaint_flag = False\n\n\ndepth_dim = (160, 128)\nrgb_dim = (304, 228)\n\nfor i in xrange(1, file_num + 1):\n\tprint 'img', i\n\tdepth_file = Path(path+'depth'+str(i)+'.png')\n\trgb_file = Path(path+'rgb'+str(i)+'.jpg')\n\tif depth_file.is_file() and rgb_file.is_file():\n\t\tcv_depth_img = cv2.imread(path+'depth'+str(i)+'.png', 0)\n\t\tcv_depth_img = crop_img(cv_depth_img)\n\t\tcv_depth_img = np.array(cv_depth_img, dtype=np.float32)\n\t\tcv_depth_img = cv2.resize(cv_depth_img, depth_dim, interpolation = cv2.INTER_NEAREST)\n\t\tcv_depth_img *= (10./255.)\n\t\t\n\t\tmask = copy.deepcopy(cv_depth_img)\n\t\tmask[mask == 0.] = 1.\n\t\tmask[mask != 1.] = 0.\n\t\tmask = 1 - mask\n\n\t\t# print np.shape(cv_depth_img)\n\t\t# print np.mean(cv_depth_img)\n\t\t# cv2.normalize(cv_depth_img, cv_depth_img, 0.0, 1.0, cv2.NORM_MINMAX, dtype=cv2.CV_32F)\n\t\t# cv_depth_img=np.uint8(cv_depth_img*255.0)\n\t\t# cv2.imshow('depth image',cv_depth_img)\n\t\t# cv2.waitKey(0)\n\t\t# mask=np.uint8(mask*255.0)\n\t\t# cv2.imshow('mask',mask)\n\t\t# cv2.waitKey(0)\n\t\t\n\t\tcv_rgb_img = cv2.imread(path+'rgb'+str(i)+'.jpg')\n\t\tcv_rgb_img = crop_img(cv_rgb_img)\n\t\tcv_resized_rgb_img = cv2.resize(cv_rgb_img, rgb_dim, interpolation = cv2.INTER_AREA)\n\t\tsame_flag = False\n\t\tcnt += 1\n\t\tif cnt == 1:\n\t\t\tlast_rgb_img = cv_resized_rgb_img\n\t\t\tcurrent_rgb_img = cv_resized_rgb_img\n\n\t\t\tlast_depth_img = cv_depth_img\n\t\t\tcurrent_depth_img = cv_depth_img\n\n\t\telse:\n\t\t\tlast_rgb_img = current_rgb_img\n\t\t\tcurrent_rgb_img = cv_resized_rgb_img\n\n\t\t\tif np.array_equal(current_rgb_img, last_rgb_img):\n\t\t\t\tprint \"The same rgb image\"\n\t\t\t\tsame_flag = True\n\n\t\t\tlast_depth_img = current_depth_img\n\t\t\tcurrent_depth_img = cv_depth_img\n\t\t\t\n\t\t\tif np.array_equal(current_depth_img, last_depth_img):\n\t\t\t\tprint \"The same depth image\"\n\t\t\t\tsame_flag = True\n\n\t\tsave_rgb_img = current_rgb_img\n\n\t\t# cv2.imshow('rgb image',cv_resized_rgb_img)\n\t\t# cv2.waitKey(0)\n\n\t\tcv_depth_img = np.reshape(cv_depth_img, (depth_dim[1], depth_dim[0], 1))\n\t\tif not same_flag:\n\t\t\tif random.uniform(0, 1.0) > .1:\n\t\t\t\tD_training.append((save_rgb_img, cv_depth_img, mask))\n\t\t\t\t# D_training.append((np.flip(save_rgb_img, axis=1), np.flip(cv_depth_img, axis=1), np.flip(mask, axis=1)))\n\t\t\telse:\n\t\t\t\tD_testing.append((save_rgb_img, cv_depth_img, mask))\n\t\t\t\t# D_testing.append((np.flip(save_rgb_img, axis=1), np.flip(cv_depth_img, axis=1), np.flip(mask, axis=1)))\n\nprint 'rgb size:', cv_resized_rgb_img.shape\nprint 'depth size:', cv_depth_img.shape\npickle.dump(D_training, open(\"rgb_depth_images_training_real.p\", \"wb\"))\npickle.dump(D_testing, open(\"rgb_depth_images_testing_real.p\", \"wb\"))\nprint 'D_training:',len(D_training), '| D_testing:', len(D_testing)\n\n" }, { "path": "Depth/FCRN/models/__init__.py", "content": "from .fcrn import ResNet50UpProj\n" }, { "path": "Depth/FCRN/models/fcrn.py", "content": "from .network import Network\n\nclass ResNet50UpProj(Network):\n def setup(self):\n # 1\n trainable_flag = True\n (self.feed('data')\n .conv(7, 7, 64, 2, 2, relu=False, name='conv1', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn_conv1', trainable=trainable_flag)\n .max_pool(3, 3, 2, 2, name='pool1')\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res2a_branch1', trainable=trainable_flag)\n .batch_normalization(name='bn2a_branch1', trainable=trainable_flag))\n # 2\n trainable_flag = True\n (self.feed('pool1')\n .conv(1, 1, 64, 1, 1, biased=False, relu=False, name='res2a_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn2a_branch2a', trainable=trainable_flag)\n .conv(3, 3, 64, 1, 1, biased=False, relu=False, name='res2a_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn2a_branch2b', trainable=trainable_flag)\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res2a_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn2a_branch2c', trainable=trainable_flag))\n # 3\n trainable_flag = True\n (self.feed('bn2a_branch1',\n 'bn2a_branch2c')\n .add(name='res2a')\n .relu(name='res2a_relu')\n .conv(1, 1, 64, 1, 1, biased=False, relu=False, name='res2b_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn2b_branch2a', trainable=trainable_flag)\n .conv(3, 3, 64, 1, 1, biased=False, relu=False, name='res2b_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn2b_branch2b', trainable=trainable_flag)\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res2b_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn2b_branch2c', trainable=trainable_flag))\n # 4\n trainable_flag = True\n (self.feed('res2a_relu',\n 'bn2b_branch2c')\n .add(name='res2b')\n .relu(name='res2b_relu')\n .conv(1, 1, 64, 1, 1, biased=False, relu=False, name='res2c_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn2c_branch2a', trainable=trainable_flag)\n .conv(3, 3, 64, 1, 1, biased=False, relu=False, name='res2c_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn2c_branch2b', trainable=trainable_flag)\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res2c_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn2c_branch2c', trainable=trainable_flag))\n # 5\n trainable_flag = True\n (self.feed('res2b_relu',\n 'bn2c_branch2c')\n .add(name='res2c')\n .relu(name='res2c_relu')\n .conv(1, 1, 512, 2, 2, biased=False, relu=False, name='res3a_branch1', trainable=trainable_flag)\n .batch_normalization(name='bn3a_branch1', trainable=trainable_flag))\n # 6\n trainable_flag = True\n (self.feed('res2c_relu')\n .conv(1, 1, 128, 2, 2, biased=False, relu=False, name='res3a_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3a_branch2a', trainable=trainable_flag)\n .conv(3, 3, 128, 1, 1, biased=False, relu=False, name='res3a_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3a_branch2b', trainable=trainable_flag)\n .conv(1, 1, 512, 1, 1, biased=False, relu=False, name='res3a_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn3a_branch2c', trainable=trainable_flag))\n # 7\n trainable_flag = True\n (self.feed('bn3a_branch1',\n 'bn3a_branch2c')\n .add(name='res3a')\n .relu(name='res3a_relu')\n .conv(1, 1, 128, 1, 1, biased=False, relu=False, name='res3b_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3b_branch2a', trainable=trainable_flag)\n .conv(3, 3, 128, 1, 1, biased=False, relu=False, name='res3b_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3b_branch2b', trainable=trainable_flag)\n .conv(1, 1, 512, 1, 1, biased=False, relu=False, name='res3b_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn3b_branch2c', trainable=trainable_flag))\n # 8\n trainable_flag = True\n (self.feed('res3a_relu',\n 'bn3b_branch2c')\n .add(name='res3b')\n .relu(name='res3b_relu')\n .conv(1, 1, 128, 1, 1, biased=False, relu=False, name='res3c_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3c_branch2a', trainable=trainable_flag)\n .conv(3, 3, 128, 1, 1, biased=False, relu=False, name='res3c_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3c_branch2b', trainable=trainable_flag)\n .conv(1, 1, 512, 1, 1, biased=False, relu=False, name='res3c_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn3c_branch2c', trainable=trainable_flag))\n # 9\n trainable_flag = True\n (self.feed('res3b_relu',\n 'bn3c_branch2c')\n .add(name='res3c')\n .relu(name='res3c_relu')\n .conv(1, 1, 128, 1, 1, biased=False, relu=False, name='res3d_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3d_branch2a', trainable=trainable_flag)\n .conv(3, 3, 128, 1, 1, biased=False, relu=False, name='res3d_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn3d_branch2b', trainable=trainable_flag)\n .conv(1, 1, 512, 1, 1, biased=False, relu=False, name='res3d_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn3d_branch2c', trainable=trainable_flag))\n # 10\n trainable_flag = True\n (self.feed('res3c_relu',\n 'bn3d_branch2c')\n .add(name='res3d')\n .relu(name='res3d_relu')\n .conv(1, 1, 1024, 2, 2, biased=False, relu=False, name='res4a_branch1', trainable=trainable_flag)\n .batch_normalization(name='bn4a_branch1', trainable=trainable_flag))\n # 11\n trainable_flag = True\n (self.feed('res3d_relu')\n .conv(1, 1, 256, 2, 2, biased=False, relu=False, name='res4a_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4a_branch2a', trainable=trainable_flag)\n .conv(3, 3, 256, 1, 1, biased=False, relu=False, name='res4a_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4a_branch2b', trainable=trainable_flag)\n .conv(1, 1, 1024, 1, 1, biased=False, relu=False, name='res4a_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn4a_branch2c', trainable=trainable_flag))\n # 12 \n (self.feed('bn4a_branch1',\n 'bn4a_branch2c')\n .add(name='res4a')\n .relu(name='res4a_relu')\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res4b_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4b_branch2a', trainable=trainable_flag)\n .conv(3, 3, 256, 1, 1, biased=False, relu=False, name='res4b_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4b_branch2b', trainable=trainable_flag)\n .conv(1, 1, 1024, 1, 1, biased=False, relu=False, name='res4b_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn4b_branch2c', trainable=trainable_flag))\n # 13\n trainable_flag = True\n (self.feed('res4a_relu',\n 'bn4b_branch2c')\n .add(name='res4b')\n .relu(name='res4b_relu')\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res4c_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4c_branch2a', trainable=trainable_flag)\n .conv(3, 3, 256, 1, 1, biased=False, relu=False, name='res4c_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4c_branch2b', trainable=trainable_flag)\n .conv(1, 1, 1024, 1, 1, biased=False, relu=False, name='res4c_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn4c_branch2c', trainable=trainable_flag))\n # 14\n (self.feed('res4b_relu',\n 'bn4c_branch2c')\n .add(name='res4c')\n .relu(name='res4c_relu')\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res4d_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4d_branch2a', trainable=trainable_flag)\n .conv(3, 3, 256, 1, 1, biased=False, relu=False, name='res4d_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4d_branch2b', trainable=trainable_flag)\n .conv(1, 1, 1024, 1, 1, biased=False, relu=False, name='res4d_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn4d_branch2c', trainable=trainable_flag))\n # 15\n trainable_flag = True\n (self.feed('res4c_relu',\n 'bn4d_branch2c')\n .add(name='res4d')\n .relu(name='res4d_relu')\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res4e_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4e_branch2a', trainable=trainable_flag)\n .conv(3, 3, 256, 1, 1, biased=False, relu=False, name='res4e_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4e_branch2b', trainable=trainable_flag)\n .conv(1, 1, 1024, 1, 1, biased=False, relu=False, name='res4e_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn4e_branch2c', trainable=trainable_flag))\n # 16\n (self.feed('res4d_relu',\n 'bn4e_branch2c')\n .add(name='res4e')\n .relu(name='res4e_relu')\n .conv(1, 1, 256, 1, 1, biased=False, relu=False, name='res4f_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4f_branch2a', trainable=trainable_flag)\n .conv(3, 3, 256, 1, 1, biased=False, relu=False, name='res4f_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn4f_branch2b', trainable=trainable_flag)\n .conv(1, 1, 1024, 1, 1, biased=False, relu=False, name='res4f_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn4f_branch2c', trainable=trainable_flag))\n # 17\n trainable_flag = True\n (self.feed('res4e_relu',\n 'bn4f_branch2c')\n .add(name='res4f')\n .relu(name='res4f_relu')\n .conv(1, 1, 2048, 2, 2, biased=False, relu=False, name='res5a_branch1', trainable=trainable_flag)\n .batch_normalization(name='bn5a_branch1', trainable=trainable_flag))\n # 18\n (self.feed('res4f_relu')\n .conv(1, 1, 512, 2, 2, biased=False, relu=False, name='res5a_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn5a_branch2a', trainable=trainable_flag)\n .conv(3, 3, 512, 1, 1, biased=False, relu=False, name='res5a_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn5a_branch2b', trainable=trainable_flag)\n .conv(1, 1, 2048, 1, 1, biased=False, relu=False, name='res5a_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn5a_branch2c', trainable=trainable_flag))\n # 19\n trainable_flag = True\n (self.feed('bn5a_branch1',\n 'bn5a_branch2c')\n .add(name='res5a')\n .relu(name='res5a_relu')\n .conv(1, 1, 512, 1, 1, biased=False, relu=False, name='res5b_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn5b_branch2a', trainable=trainable_flag)\n .conv(3, 3, 512, 1, 1, biased=False, relu=False, name='res5b_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn5b_branch2b', trainable=trainable_flag)\n .conv(1, 1, 2048, 1, 1, biased=False, relu=False, name='res5b_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn5b_branch2c', trainable=trainable_flag))\n # 20\n (self.feed('res5a_relu',\n 'bn5b_branch2c')\n .add(name='res5b')\n .relu(name='res5b_relu')\n .conv(1, 1, 512, 1, 1, biased=False, relu=False, name='res5c_branch2a', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn5c_branch2a', trainable=trainable_flag)\n .conv(3, 3, 512, 1, 1, biased=False, relu=False, name='res5c_branch2b', trainable=trainable_flag)\n .batch_normalization(relu=True, name='bn5c_branch2b', trainable=trainable_flag)\n .conv(1, 1, 2048, 1, 1, biased=False, relu=False, name='res5c_branch2c', trainable=trainable_flag)\n .batch_normalization(name='bn5c_branch2c', trainable=trainable_flag))\n # 21\n trainable_flag = True\n (self.feed('res5b_relu',\n 'bn5c_branch2c')\n .add(name='res5c')\n .relu(name='res5c_relu')\n .conv(1, 1, 1024, 1, 1, biased=True, relu=False, name='layer1', trainable=trainable_flag)\n .batch_normalization(relu=False, name='layer1_BN', trainable=trainable_flag)\n .up_project([3, 3, 1024, 512], id = '2x', stride = 1, BN=True, trainable=trainable_flag)\n .up_project([3, 3, 512, 256], id = '4x', stride = 1, BN=True, trainable=trainable_flag)\n .up_project([3, 3, 256, 128], id = '8x', stride = 1, BN=True, trainable=trainable_flag)\n .up_project([3, 3, 128, 64], id = '16x', stride = 1, BN=True, trainable=trainable_flag)\n .dropout(name = 'drop', keep_prob = 1.)\n .conv(3, 3, 1, 1, 1, name = 'ConvPred', trainable=trainable_flag))\n" }, { "path": "Depth/FCRN/models/network.py", "content": "import numpy as np\nimport tensorflow as tf\n\n# ----------------------------------------------------------------------------------\n# Commonly used layers and operations based on ethereon's implementation \n# https://github.com/ethereon/caffe-tensorflow\n# Slight modifications may apply. FCRN-specific operations have also been appended. \n# ----------------------------------------------------------------------------------\n# Thanks to *Helisa Dhamo* for the model conversion and integration into TensorFlow.\n# ----------------------------------------------------------------------------------\n\nDEFAULT_PADDING = 'SAME'\n\n\ndef get_incoming_shape(incoming):\n \"\"\" Returns the incoming data shape \"\"\"\n if isinstance(incoming, tf.Tensor):\n return incoming.get_shape().as_list()\n elif type(incoming) in [np.array, list, tuple]:\n return np.shape(incoming)\n else:\n raise Exception(\"Invalid incoming layer.\")\n\n\ndef interleave(tensors, axis):\n old_shape = get_incoming_shape(tensors[0])[1:]\n new_shape = [-1] + old_shape\n new_shape[axis] *= len(tensors)\n return tf.reshape(tf.stack(tensors, axis + 1), new_shape)\n\ndef layer(op):\n '''Decorator for composable network layers.'''\n\n def layer_decorated(self, *args, **kwargs):\n # Automatically set a name if not provided.\n name = kwargs.setdefault('name', self.get_unique_name(op.__name__))\n\n # Figure out the layer inputs.\n if len(self.terminals) == 0:\n raise RuntimeError('No input variables found for layer %s.' % name)\n elif len(self.terminals) == 1:\n layer_input = self.terminals[0]\n else:\n layer_input = list(self.terminals)\n # Perform the operation and get the output.\n layer_output = op(self, layer_input, *args, **kwargs)\n # Add to layer LUT.\n self.layers[name] = layer_output\n # This output is now the input for the next layer.\n self.feed(layer_output)\n # Return self for chained calls.\n return self\n\n return layer_decorated\n\n\nclass Network(object):\n\n def __init__(self, inputs, batch, keep_prob, is_training, trainable=False):\n # The input nodes for this network\n self.inputs = inputs\n # The current list of terminal nodes\n self.terminals = []\n # Mapping from layer names to layers\n self.layers = dict(inputs)\n # If true, the resulting variables are set as trainable\n self.trainable = trainable\n self.batch_size = batch\n self.keep_prob = keep_prob\n self.is_training = is_training\n self.setup()\n\n\n def setup(self):\n '''Construct the network. '''\n raise NotImplementedError('Must be implemented by the subclass.')\n\n def load(self, data_path, session, ignore_missing=False):\n '''Load network weights.\n data_path: The path to the numpy-serialized network weights\n session: The current TensorFlow session\n ignore_missing: If true, serialized weights for missing layers are ignored.\n '''\n data_dict = np.load(data_path, encoding='latin1').item()\n for op_name in data_dict: \n with tf.variable_scope(op_name, reuse=True):\n for param_name, data in iter(data_dict[op_name].items()): \n try:\n var = tf.get_variable(param_name)\n session.run(var.assign(data))\n\n except ValueError:\n if not ignore_missing:\n raise\n\n def feed(self, *args):\n '''Set the input(s) for the next operation by replacing the terminal nodes.\n The arguments can be either layer names or the actual layers.\n '''\n assert len(args) != 0\n self.terminals = []\n for fed_layer in args:\n if isinstance(fed_layer, str):\n try:\n fed_layer = self.layers[fed_layer]\n except KeyError:\n raise KeyError('Unknown layer name fed: %s' % fed_layer)\n self.terminals.append(fed_layer)\n return self\n\n def get_output(self):\n '''Returns the current network output.'''\n return self.terminals[-1]\n\n def get_layer_output(self, name):\n return self.layers[name]\n\n def get_unique_name(self, prefix):\n '''Returns an index-suffixed unique name for the given prefix.\n This is used for auto-generating layer names based on the type-prefix.\n '''\n ident = sum(t.startswith(prefix) for t, _ in self.layers.items()) + 1\n return '%s_%d' % (prefix, ident)\n\n def make_var(self, name, shape, trainable=False):\n '''Creates a new TensorFlow variable.'''\n return tf.get_variable(name, shape, dtype = 'float32', trainable=trainable)\n\n def validate_padding(self, padding):\n '''Verifies that the padding is one of the supported ones.'''\n assert padding in ('SAME', 'VALID')\n\n @layer\n def conv(self,\n input_data,\n k_h,\n k_w,\n c_o,\n s_h,\n s_w,\n name,\n relu=True,\n padding=DEFAULT_PADDING,\n group=1,\n biased=True,\n trainable=False):\n\n # Verify that the padding is acceptable\n self.validate_padding(padding)\n # Get the number of channels in the input\n c_i = input_data.get_shape()[-1]\n\n if (padding == 'SAME'):\n input_data = tf.pad(input_data, [[0, 0], [(k_h - 1)//2, (k_h - 1)//2], [(k_w - 1)//2, (k_w - 1)//2], [0, 0]], \"CONSTANT\")\n \n # Verify that the grouping parameter is valid\n assert c_i % group == 0\n assert c_o % group == 0\n # Convolution for a given input and kernel\n convolve = lambda i, k: tf.nn.conv2d(i, k, [1, s_h, s_w, 1], padding='VALID')\n \n with tf.variable_scope(name) as scope:\n kernel = self.make_var('weights', shape=[k_h, k_w, c_i // group, c_o], trainable=trainable)\n\n if group == 1:\n # This is the common-case. Convolve the input without any further complications.\n output = convolve(input_data, kernel)\n else:\n # Split the input into groups and then convolve each of them independently\n\n input_groups = tf.split(3, group, input_data)\n kernel_groups = tf.split(3, group, kernel)\n output_groups = [convolve(i, k) for i, k in zip(input_groups, kernel_groups)]\n # Concatenate the groups\n output = tf.concat(3, output_groups)\n\n # Add the biases\n if biased:\n biases = self.make_var('biases', [c_o], trainable=trainable)\n output = tf.nn.bias_add(output, biases)\n if relu:\n # ReLU non-linearity\n output = tf.nn.relu(output, name=scope.name)\n\n return output\n\n @layer\n def relu(self, input_data, name):\n return tf.nn.relu(input_data, name=name)\n\n @layer\n def max_pool(self, input_data, k_h, k_w, s_h, s_w, name, padding=DEFAULT_PADDING):\n self.validate_padding(padding)\n return tf.nn.max_pool(input_data,\n ksize=[1, k_h, k_w, 1],\n strides=[1, s_h, s_w, 1],\n padding=padding,\n name=name)\n\n @layer\n def avg_pool(self, input_data, k_h, k_w, s_h, s_w, name, padding=DEFAULT_PADDING):\n self.validate_padding(padding)\n return tf.nn.avg_pool(input_data,\n ksize=[1, k_h, k_w, 1],\n strides=[1, s_h, s_w, 1],\n padding=padding,\n name=name)\n\n @layer\n def lrn(self, input_data, radius, alpha, beta, name, bias=1.0):\n return tf.nn.local_response_normalization(input_data,\n depth_radius=radius,\n alpha=alpha,\n beta=beta,\n bias=bias,\n name=name)\n\n @layer\n def concat(self, inputs, axis, name):\n return tf.concat(concat_dim=axis, values=inputs, name=name)\n\n @layer\n def add(self, inputs, name):\n return tf.add_n(inputs, name=name)\n\n @layer\n def fc(self, input_data, num_out, name, relu=True):\n with tf.variable_scope(name) as scope:\n input_shape = input_data.get_shape()\n if input_shape.ndims == 4:\n # The input is spatial. Vectorize it first.\n dim = 1\n for d in input_shape[1:].as_list():\n dim *= d\n feed_in = tf.reshape(input_data, [-1, dim])\n else:\n feed_in, dim = (input_data, input_shape[-1].value)\n weights = self.make_var('weights', shape=[dim, num_out])\n biases = self.make_var('biases', [num_out])\n op = tf.nn.relu_layer if relu else tf.nn.xw_plus_b\n fc = op(feed_in, weights, biases, name=scope.name)\n return fc\n\n @layer\n def softmax(self, input_data, name):\n input_shape = map(lambda v: v.value, input_data.get_shape())\n if len(input_shape) > 2:\n # For certain models (like NiN), the singleton spatial dimensions\n # need to be explicitly squeezed, since they're not broadcast-able\n # in TensorFlow's NHWC ordering (unlike Caffe's NCHW).\n if input_shape[1] == 1 and input_shape[2] == 1:\n input_data = tf.squeeze(input_data, squeeze_dims=[1, 2])\n else:\n raise ValueError('Rank 2 tensor input expected for softmax!')\n return tf.nn.softmax(input_data, name)\n\n @layer\n def batch_normalization(self, input_data, name, scale_offset=True, relu=False, trainable=False):\n\n with tf.variable_scope(name) as scope:\n shape = [input_data.get_shape()[-1]]\n pop_mean = tf.get_variable(\"mean\", shape, initializer = tf.constant_initializer(0.0), trainable=trainable)\n pop_var = tf.get_variable(\"variance\", shape, initializer = tf.constant_initializer(1.0), trainable=trainable)\n epsilon = 1e-4\n decay = 0.999\n if scale_offset:\n scale = tf.get_variable(\"scale\", shape, initializer = tf.constant_initializer(1.0), trainable=trainable)\n offset = tf.get_variable(\"offset\", shape, initializer = tf.constant_initializer(0.0), trainable=trainable)\n else:\n scale, offset = (None, None)\n if self.is_training:\n batch_mean, batch_var = tf.nn.moments(input_data, [0, 1, 2])\n\n train_mean = tf.assign(pop_mean,\n pop_mean * decay + batch_mean * (1 - decay))\n train_var = tf.assign(pop_var,\n pop_var * decay + batch_var * (1 - decay))\n with tf.control_dependencies([train_mean, train_var]):\n output = tf.nn.batch_normalization(input_data,\n batch_mean, batch_var, offset, scale, epsilon, name = name)\n else:\n output = tf.nn.batch_normalization(input_data,\n pop_mean, pop_var, offset, scale, epsilon, name = name)\n\n if relu:\n output = tf.nn.relu(output)\n\n return output\n\n @layer\n def dropout(self, input_data, keep_prob, name):\n return tf.nn.dropout(input_data, keep_prob, name=name)\n \n\n def unpool_as_conv(self, size, input_data, id, stride = 1, ReLU = False, BN = True, trainable=False):\n\n\t\t# Model upconvolutions (unpooling + convolution) as interleaving feature\n\t\t# maps of four convolutions (A,B,C,D). Building block for up-projections. \n\n\n # Convolution A (3x3)\n # --------------------------------------------------\n layerName = \"layer%s_ConvA\" % (id)\n self.feed(input_data)\n self.conv( 3, 3, size[3], stride, stride, name = layerName, padding = 'SAME', relu = False, trainable=trainable)\n outputA = self.get_output()\n\n # Convolution B (2x3)\n # --------------------------------------------------\n layerName = \"layer%s_ConvB\" % (id)\n padded_input_B = tf.pad(input_data, [[0, 0], [1, 0], [1, 1], [0, 0]], \"CONSTANT\")\n self.feed(padded_input_B)\n self.conv(2, 3, size[3], stride, stride, name = layerName, padding = 'VALID', relu = False, trainable=trainable)\n outputB = self.get_output()\n\n # Convolution C (3x2)\n # --------------------------------------------------\n layerName = \"layer%s_ConvC\" % (id)\n padded_input_C = tf.pad(input_data, [[0, 0], [1, 1], [1, 0], [0, 0]], \"CONSTANT\")\n self.feed(padded_input_C)\n self.conv(3, 2, size[3], stride, stride, name = layerName, padding = 'VALID', relu = False, trainable=trainable)\n outputC = self.get_output()\n\n # Convolution D (2x2)\n # --------------------------------------------------\n layerName = \"layer%s_ConvD\" % (id)\n padded_input_D = tf.pad(input_data, [[0, 0], [1, 0], [1, 0], [0, 0]], \"CONSTANT\")\n self.feed(padded_input_D)\n self.conv(2, 2, size[3], stride, stride, name = layerName, padding = 'VALID', relu = False, trainable=trainable)\n outputD = self.get_output()\n\n # Interleaving elements of the four feature maps\n # --------------------------------------------------\n left = interleave([outputA, outputB], axis=1) # columns\n right = interleave([outputC, outputD], axis=1) # columns\n Y = interleave([left, right], axis=2) # rows\n \n if BN:\n layerName = \"layer%s_BN\" % (id)\n self.feed(Y)\n self.batch_normalization(name=layerName, scale_offset=True, relu=False, trainable=trainable)\n Y = self.get_output()\n\n if ReLU:\n Y = tf.nn.relu(Y, name = layerName)\n \n return Y\n\n\n def up_project(self, size, id, stride = 1, BN = True, trainable=False):\n \n # Create residual upsampling layer (UpProjection)\n\n input_data = self.get_output()\n\n # Branch 1\n id_br1 = \"%s_br1\" % (id)\n\n # Interleaving Convs of 1st branch\n out = self.unpool_as_conv(size, input_data, id_br1, stride, ReLU=True, BN=True, trainable=trainable)\n\n # Convolution following the upProjection on the 1st branch\n layerName = \"layer%s_Conv\" % (id)\n self.feed(out)\n self.conv(size[0], size[1], size[3], stride, stride, name = layerName, relu = False, trainable=trainable)\n\n if BN:\n layerName = \"layer%s_BN\" % (id)\n self.batch_normalization(name=layerName, scale_offset=True, relu=False, trainable=trainable)\n\n # Output of 1st branch\n branch1_output = self.get_output()\n\n \n # Branch 2\n id_br2 = \"%s_br2\" % (id)\n # Interleaving convolutions and output of 2nd branch\n branch2_output = self.unpool_as_conv(size, input_data, id_br2, stride, ReLU=False, trainable=trainable)\n\n \n # sum branches\n layerName = \"layer%s_Sum\" % (id)\n output = tf.add_n([branch1_output, branch2_output], name = layerName)\n # ReLU\n layerName = \"layer%s_ReLU\" % (id)\n output = tf.nn.relu(output, name=layerName)\n\n self.feed(output)\n return self\n" }, { "path": "Depth/FCRN/testing.py", "content": "import tensorflow as tf \nimport numpy as np \nimport cv2\nimport pickle\nimport random\nimport models\nimport copy\nimport time\nimport math\n\nfrom cv_bridge import CvBridge, CvBridgeError\nfrom PIL import Image\n\nimport os\n\nos.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'\nos.environ[\"CUDA_DEVICE_ORDER\"] = \"PCI_BUS_ID\"\nos.environ[\"CUDA_VISIBLE_DEVICES\"] = '0'\n\nCHANNEL = 3\nDEPTH_IMAGE_WIDTH = 160\nDEPTH_IMAGE_HEIGHT = 128\nRGB_IMAGE_WIDTH = 304\nRGB_IMAGE_HEIGHT = 228\nMAX_STEP = 200\nMAX_EPOCH = 150\nBATCH = 1\n\ndef output_predict(predict, kinect, rgb, epoch, step):\n\tmax_val = 10.\n\tkinect[kinect>max_val] = max_val\n\tif np.max(kinect) != 0:\n\t\tkinect_save = (kinect/max_val)*255.0\n\t\t# print('kinect_max', np.amax(kinect))\n\telse:\n\t\tkinect_save = kinect*255.0\n\tkinect_save=np.uint8(kinect_save)\n\tname = \"data/%04d\" % epoch + \"_%04d_kinect.png\" % step\n\tcv2.imwrite(name,kinect_save)\n\n\tpredict[predict>max_val] = max_val\n\tif np.max(predict) != 0:\n\t\tpredict_save = (predict/max_val)*255.0\n\t\t# print('predict_max', np.amax(predict))\n\telse:\n\t\tpredict_save = predict*255.0\n\tpredict_save=np.uint8(predict_save)\n\tname = \"data/%04d\" % epoch + \"_%04d_predicted.png\" % step\n\tcv2.imwrite(name,predict_save)\n\n\tname = \"data/%04d\" % epoch + \"_%04d_rgb.png\" % step\n\tcv2.imwrite(name,rgb)\n\ndef SetDiff(first, second):\n\tsecond = set(second)\n\treturn [item for item in first if item not in second]\n\ndef normalize_rgb(rgb_images, value):\n\trgb_images = np.asarray(rgb_images).astype(float)\n\trgb_images /= 255.\n\tfor x in xrange(3):\n\t\trgb_images[:, :, :, x] -= value[x]\n\treturn rgb_images\n\ndef consecutive_sample(data, start, end):\n\t# return a list\n\tpart = []\n\tfor x in xrange(start, end):\n\t\tpart.append(data[x])\n\treturn part\n\nwith tf.Session() as sess:\n\t# Construct network and define loss function\n\tstate = tf.placeholder(\"float\", [None, RGB_IMAGE_HEIGHT, RGB_IMAGE_WIDTH, CHANNEL])\n\tnet = models.ResNet50UpProj({'data': state}, BATCH, 1, True)\n\tdepth_predict = net.get_output()\n\tdepth_kinect = tf.placeholder(\"float\", [None, DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, 1])\n\timg_mask = tf.placeholder(\"float\", [None, DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, 1])\n\n\tprint 'Loading initial network param'\n\tinit_saver = tf.train.Saver() \n\tinit_saver.restore(sess, '../init_network/NYU_FCRN.ckpt')\n\n\t# d_show = tf.subtract(tf.multiply(depth_predict, img_mask), tf.multiply(depth_kinect, img_mask))\n\t# abs_d_show = tf.abs(d_show)\n\t# c = tf.divide(tf.reduce_max(abs_d_show), 5.)\n\t# berHu = tf.where(tf.less_equal(abs_d_show, c), abs_d_show, tf.square(d_show))\n\t# loss = tf.reduce_mean(tf.reduce_mean(berHu, 1))\n\n\t# train_step = tf.train.AdamOptimizer(5e-5).minimize(loss)\n\n\tprint 'Initializing var'\n\tuninitialized_vars = []\n\tstart_time = time.time()\n\tfor var in tf.global_variables():\n\t\ttry:\n\t\t\tsess.run(var)\n\t\texcept tf.errors.FailedPreconditionError: \n\t\t\tuninitialized_vars.append(var)\n\tinit_new_vars_op = tf.variables_initializer(uninitialized_vars)\n\t# print(\" [*] printing unitialized variables\")\n\t# for idx, v in enumerate(uninitialized_vars):\n\t# \tprint(\" var {:3}: {:15} {}\".format(idx, str(v.get_shape()), v.name))\n\tsess.run(init_new_vars_op)\n\tprint 'Var initialized, time:', time.time() - start_time\n\n\ttrainable_var = tf.trainable_variables()\n\t# print \" [*] printing trainable variables\"\n\t# for idx, v in enumerate(trainable_var):\n\t# \tprint(\" var {:3}: {:15} {}\".format(idx, str(v.get_shape()), v.name))\n\n\tdepth_net_saver = tf.train.Saver(trainable_var, max_to_keep=1)\n\tcheckpoint = tf.train.get_checkpoint_state('saved_network')\n\tif checkpoint and checkpoint.model_checkpoint_path:\n\t\tprint 'Loading from checkpoint:', checkpoint\n\t\tdepth_net_saver.restore(sess, checkpoint.model_checkpoint_path)\n\t\tprint \"Depth network model loaded:\", checkpoint.model_checkpoint_path\n\telse:\n\t\tprint 'No new model'\n\n\tprint 'Loading data'\n\ttraining_data = pickle.load(open('../rgb_depth_images_training_real.p', \"rb\"))\n\ttesting_data = pickle.load(open('../rgb_depth_images_testing_real.p', \"rb\"))\n\tprint 'Data loaded'\n\tepoch = 0\n\tfor step in xrange(1,int(len(training_data)/(BATCH))+1):\n\t\tstart_time = time.time()\n\t\ttraining_batch_real = consecutive_sample(training_data, (step-1)*BATCH, step*BATCH)\n\n\t\trgb_img = [d[0] for d in training_batch_real]\n\t\tdepth_img = [d[1] for d in training_batch_real]\n\t\tdepth_img = np.reshape(depth_img, [-1, DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, 1])\n\t\tmask = [d[2] for d in training_batch_real]\n\t\tmask = np.reshape(mask, [-1, DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, 1])\n\t\tdepth_predict_value = sess.run(depth_predict, feed_dict={ state : rgb_img})\n\n\t\toutput_predict(depth_predict_value[0], depth_img[0], np.array(rgb_img)[0], epoch, step)\n\t\t# print(\"epoch: {:2} | step: {:3} | testing loss: {:.4f}, time: {:.3f}\"\\\n\t\t# \t\t.format(epoch, step, testing_loss, time.time()-start_time))\n" }, { "path": "Depth/FCRN/training.py", "content": "import tensorflow as tf \nimport numpy as np \nimport cv2\nimport pickle\nimport random\nimport models\nimport copy\nimport time\nimport math\nimport os\n\nfrom cv_bridge import CvBridge, CvBridgeError\nfrom PIL import Image\n\nos.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'\nos.environ[\"CUDA_DEVICE_ORDER\"] = \"PCI_BUS_ID\"\nos.environ[\"CUDA_VISIBLE_DEVICES\"] = '0'\n\nCHANNEL = 3\nDEPTH_IMAGE_WIDTH = 160\nDEPTH_IMAGE_HEIGHT = 128\nRGB_IMAGE_WIDTH = 304\nRGB_IMAGE_HEIGHT = 228\nMAX_STEP = 200\nMAX_EPOCH = 150\nBATCH = 16\n\ndef output_predict(predict, kinect, rgb, epoch, step):\n\tmax_val = 10.\n\tkinect[kinect>max_val] = max_val\n\tif np.max(kinect) != 0:\n\t\tkinect_save = (kinect/max_val)*255.0\n\t\t# print('kinect_max', np.amax(kinect))\n\telse:\n\t\tkinect_save = kinect*255.0\n\tkinect_save=np.uint8(kinect_save)\n\tname = \"data/%04d\" % epoch + \"_%04d_kinect.png\" % step\n\tcv2.imwrite(name,kinect_save)\n\n\tpredict[predict>max_val] = max_val\n\tif np.max(predict) != 0:\n\t\tpredict_save = (predict/max_val)*255.0\n\t\t# print('predict_max', np.amax(predict))\n\telse:\n\t\tpredict_save = predict*255.0\n\tpredict_save=np.uint8(predict_save)\n\tname = \"data/%04d\" % epoch + \"_%04d_predicted.png\" % step\n\tcv2.imwrite(name,predict_save)\n\n\tname = \"data/%04d\" % epoch + \"_%04d_rgb.png\" % step\n\tcv2.imwrite(name,rgb)\n\ndef SetDiff(first, second):\n\tsecond = set(second)\n\treturn [item for item in first if item not in second]\n\ndef normalize_rgb(rgb_images, value):\n\trgb_images = np.asarray(rgb_images).astype(float)\n\trgb_images /= 255.\n\tfor x in xrange(3):\n\t\trgb_images[:, :, :, x] -= value[x]\n\treturn rgb_images\n\ndef consecutive_sample(data, start, end):\n\t# return a list\n\tpart = []\n\tfor x in xrange(start, end):\n\t\tpart.append(data[x])\n\treturn part\n\nwith tf.Session() as sess:\n\t# Construct network and define loss function\n\tstate = tf.placeholder(\"float\", [None, RGB_IMAGE_HEIGHT, RGB_IMAGE_WIDTH, CHANNEL])\n\tnet = models.ResNet50UpProj({'data': state}, BATCH, 1, True)\n\tdepth_predict = net.get_output()\n\tdepth_kinect = tf.placeholder(\"float\", [None, DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, 1])\n\timg_mask = tf.placeholder(\"float\", [None, DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, 1])\n\n\tprint 'Loading initial network param'\n\tinit_saver = tf.train.Saver() \n\tinit_saver.restore(sess, '../init_network/NYU_FCRN.ckpt')\n\n\td_show = tf.subtract(tf.multiply(depth_predict, img_mask), tf.multiply(depth_kinect, img_mask))\n\tabs_d_show = tf.abs(d_show)\n\tc = tf.divide(tf.reduce_max(abs_d_show), 5.)\n\tberHu = tf.where(tf.less_equal(abs_d_show, c), abs_d_show, tf.square(d_show))\n\tloss = tf.reduce_mean(tf.reduce_mean(berHu, 1))\n\n\ttrain_step = tf.train.AdamOptimizer(5e-5).minimize(loss)\n\n\ttrain_loss_var = tf.Variable(0., trainable=False)\n\ttrain_loss_sum = tf.summary.scalar('training_loss', train_loss_var)\n\ttest_loss_var = tf.Variable(0., trainable=False)\n\ttest_loss_sum = tf.summary.scalar('testing_loss', test_loss_var)\n\tmerged_summary = tf.summary.merge_all()\n\tsummary_writer = tf.summary.FileWriter('./logs', sess.graph)\n\n\tprint 'Initializing var'\n\tuninitialized_vars = []\n\tstart_time = time.time()\n\tfor var in tf.global_variables():\n\t\ttry:\n\t\t\tsess.run(var)\n\t\texcept tf.errors.FailedPreconditionError: \n\t\t\tuninitialized_vars.append(var)\n\tinit_new_vars_op = tf.variables_initializer(uninitialized_vars)\n\tprint(\" [*] printing unitialized variables\")\n\tfor idx, v in enumerate(uninitialized_vars):\n\t\tprint(\" var {:3}: {:15} {}\".format(idx, str(v.get_shape()), v.name))\n\tsess.run(init_new_vars_op)\n\tprint 'Var initialized, time:', time.time() - start_time\n\n\ttrainable_var = tf.trainable_variables()\n\tprint \" [*] printing trainable variables\"\n\tfor idx, v in enumerate(trainable_var):\n\t\tprint(\" var {:3}: {:15} {}\".format(idx, str(v.get_shape()), v.name))\n\n\tdepth_net_saver = tf.train.Saver(trainable_var, max_to_keep=1)\n\tcheckpoint = tf.train.get_checkpoint_state('saved_network')\n\tif checkpoint and checkpoint.model_checkpoint_path:\n\t\tprint 'Loading from checkpoint:', checkpoint\n\t\tdepth_net_saver.restore(sess, checkpoint.model_checkpoint_path)\n\t\tprint \"Depth network model loaded:\", checkpoint.model_checkpoint_path\n\telse:\n\t\tprint 'No new model'\n\n\tprint 'Loading data'\n\ttraining_data = pickle.load(open('../rgb_depth_images_training_real.p', \"rb\"))\n\ttesting_data = pickle.load(open('../rgb_depth_images_testing_real.p', \"rb\"))\n\tprint 'Data loaded'\n\n\tStep = 0\n\tfor epoch in xrange(1,MAX_EPOCH+1):\n\t\tnp.random.shuffle(training_data)\n\t\tnp.random.shuffle(testing_data)\n\t\tfor step in xrange(1,int(len(training_data)/(BATCH))+1):\n\t\t\tstart_time = time.time()\n\t\t\ttraining_batch_real = consecutive_sample(training_data, (step-1)*BATCH, step*BATCH)\n\n\t\t\trgb_img = [d[0] for d in training_batch_real]\n\t\t\tdepth_img = [d[1] for d in training_batch_real]\n\t\t\tdepth_img = np.reshape(depth_img, [-1, DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, 1])\n\t\t\tmask = [d[2] for d in training_batch_real]\n\t\t\tmask = np.reshape(mask, [-1, DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, 1])\n\n\t\t\t# flip\n\t\t\tif np.random.rand() > 0.5:\n\t\t\t\trgb_img = np.flip(rgb_img, axis=2)\n\t\t\t\tdepth_img = np.flip(depth_img, axis=2)\n\t\t\t\tmask = np.flip(mask, axis=2)\n\n\t\t\ttraining_loss = 0.\n\t\t\tdepth_predict_value, _, training_loss = sess.run([depth_predict, train_step, loss], \n\t\t\t\t \t\t\t\t\t\tfeed_dict = { state : rgb_img,\n\t\t\t\t\t\t\t\t\t\t\t\t\t depth_kinect : depth_img,\n\t\t\t\t\t\t\t\t\t\t\t\t\t img_mask : mask})\n\n\t\t\tif step % 10 == 0:\n\t\t\t\ttesting_batch = random.sample(testing_data, BATCH)\n\t\t\t\trgb_img_test = [d[0] for d in testing_batch]\n\t\t\t\tdepth_img_test = [d[1] for d in testing_batch]\n\t\t\t\tdepth_img_test = np.reshape(depth_img_test, [-1, DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, 1])\n\t\t\t\tmask_test = [d[2] for d in testing_batch]\n\t\t\t\tmask_test = np.reshape(mask_test, [-1, DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, 1])\n\t\t\t\tdepth_predict_value, testing_loss, difference= sess.run([depth_predict, loss, abs_d_show], \n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t feed_dict = { state : rgb_img_test,\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tdepth_kinect : depth_img_test,\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\timg_mask : mask_test})\n\t\t\t\t# if epoch % 10 == 0:\n\t\t\t\toutput_predict(depth_predict_value[0], depth_img_test[0], np.asarray(rgb_img_test)[0], epoch, step)\n\t\t\t\t# print(\"epoch: {:2} | step: {:3} | traning loss: {:.4f} | testing loss: {:.4f}, time: {:.2f}\"\\\n\t\t\t\t# \t\t.format(epoch, step, training_loss, testing_loss, (time.time()-start_time)/60.))\n\t\t\t\tsummary_str = sess.run(merged_summary, feed_dict={train_loss_var: training_loss, \n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t test_loss_var: testing_loss})\n\t\t\t\tsummary_writer.add_summary(summary_str, Step)\n\t\t\t\tStep = Step + 1\n\n\t\t# full testing\n\t\ttesting_loss_buff = []\n\t\tfor step in xrange(int(len(testing_data)/(BATCH))):\n\t\t\ttesting_batch = consecutive_sample(testing_data, (step-1)*BATCH, step*BATCH)\n\t\t\trgb_img_test = [d[0] for d in testing_batch]\n\t\t\tdepth_img_test = [d[1] for d in testing_batch]\n\t\t\tmask_test = [d[2] for d in testing_batch]\n\t\t\tmask_test = np.reshape(mask_test, [-1, DEPTH_IMAGE_HEIGHT, DEPTH_IMAGE_WIDTH, 1])\n\n\t\t\tdepth_predict_value, testing_loss, difference = sess.run([depth_predict, loss, abs_d_show], \n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t feed_dict = { state : rgb_img_test,\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\tdepth_kinect : depth_img_test,\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\timg_mask : mask_test})\n\t\t\ttesting_loss_buff.append(testing_loss)\n\n\t\tprint(\"epoch: {:3} | traning loss: {:.4f} | testing loss: {:.4f}, time: {:.2f}\"\\\n\t\t\t\t.format(epoch, training_loss, np.mean(testing_loss_buff), (time.time()-start_time)/60.))\n\t\tif epoch % 30 == 0:\n\t\t\tdepth_net_saver.save(sess, 'saved_network/DepthNet', global_step = epoch)\n" }, { "path": "LICENSE", "content": "Copyright (c) 2016, Iro Laina\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n* Redistributions of source code must retain the above copyright notice, this\n list of conditions and the following disclaimer.\n\n* Redistributions in binary form must reproduce the above copyright notice,\n this list of conditions and the following disclaimer in the documentation\n and/or other materials provided with the distribution.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n" }, { "path": "README.md", "content": "# Towards Monocular Vision based Obstacle Avoidance through Deep Reinforcement Learning\n\nBy [Linhai Xie](https://www.cs.ox.ac.uk/people/linhai.xie/), [Sen Wang](http://senwang.weebly.com/), Niki trigoni, Andrew Markham.\n\nThe tensorflow implmentation for the paper: [Towards Monocular Vision based Obstacle Avoidance through Deep Reinforcement Learning](https://arxiv.org/abs/1706.09829)\n\n## Contents\n0. [Introduction](#introduction)\n0. [Prerequisite](#Prerequisite)\n0. [Instruction](#instruction)\n0. [Citation](#citation)\n\n## Introduction\n\nThis repository contains:\n\n1.Training code for [FCRN](https://arxiv.org/abs/1606.00373). We write our own training code but build the mode directly with the code provided [here](https://github.com/iro-cp/FCRN-DepthPrediction). (We retain Iro's license in the repository)\n\n2.Data preprocessing code for training FCRN.\n\n3.Training code for D3QN(Double Deep Q Network with Dueling architecture) with a turtlebot2 in Gazebo simulator.\n\n4.Testing code for D3QN with a turtlebot2 in real world\n\n5.The interface code between tensorflow and ros\n\nThe network model for D3QN is slightly different from the paper as we find this version has a better performance.\n\nThe video of our real world experiments is available at [Youtube](https://www.youtube.com/watch?v=qNIVgG4RUDM)\n\n## Prerequisites\n\nTensorflow > 1.1\n\nROS Kinetic\n\ncv2\n\n## Instruction\n**Retraining FCRN**\n\nWe have a example dataset collected with a turtlebot in folder ```/Depth/data``` which contains 1k labeled rgb-depth images. \nAnd the full dataset we used with 10k images is [here.](https://drive.google.com/file/d/1OUH0Ioe1uT3MoEz9TwmzzBbtgJWtZVPh/view?usp=sharing)\nWe recommend to collected more than 10k images by yourself to retrain a good model based on the initial one.\n\nAfter collecting enough data, use `DataPreprocess.py` to generate training and testing datasets.\n\nDownload the [initial model of FCRN](http://campar.in.tum.de/files/rupprecht/depthpred/NYU_FCRN-checkpoint.zip) into the folder '''/Depth/init_network'''\n\nThen use `training.py` in ```/Depth/FCRN``` to retrain the model.\n\nFinally copy the generated checkpoint and model file from ```/Depth/saved_network``` to ```/D3QN/saved_networks/depth```\n\n**Training D3QN in Gazebo**\n\nLaunch the Gazebo world with our world file with command:\n\n```roslaunch turtlebot_gazebo turtlebot_world.launch world_file:=/PATH TO/Monocular-Obstacle-Avoidance/D3QN/world/SquareWorld.world```\n\nStart training with:\n\n```python D3QN_training```\n\n**Testing D3QN in real world**\n\n```python D3QN_testing```\n\n## Citation\n\nIf you use this method in your research, please cite:\n\n\t@inproceedings{xie2017towards,\n\t\t title = \"Towards Monocular Vision based Obstacle Avoidance through Deep Reinforcement Learning\",\n\t\t author = \"Xie, Linhai and Wang, Sen and Markham, Andrew and Trigoni, Niki\",\n\t\t year = \"2017\",\n\t\t booktitle = \"RSS 2017 workshop on New Frontiers for Deep Learning in Robotics\",\n\t}\n\n\n\n" } ]