text: # Open3D: www.open3d.org
  # The MIT License (MIT)
  # See license file or visit www.open3d.org for details
  
  # following code is tested with OpenCV 3.2.0 and Python2.7
  # how to install opencv
  # conda create --prefix py27opencv python=2.7
  # source activate py27opencv
  # conda install -c conda-forge opencv
  # conda install -c conda-forge openblas (if openblas conflicts)
  import numpy as np
  import cv2
  from open3d import *
  from matplotlib import pyplot as plt # for visualizing feature matching
  import copy
  
  
  def pose_estimation(source_rgbd_image, target_rgbd_image,
          pinhole_camera_intrinsic, debug_draw_correspondences):
      success = False
      trans = np.identity(4)
  
      # transform double array to unit8 array
      color_cv_s = np.uint8(np.asarray(source_rgbd_image.color)*255.0)
      color_cv_t = np.uint8(np.asarray(target_rgbd_image.color)*255.0)
  
      orb = cv2.ORB_create(scaleFactor=1.2, nlevels = 8, edgeThreshold = 31,
              firstLevel = 0, WTA_K = 2, scoreType=cv2.ORB_HARRIS_SCORE,
              nfeatures = 100, patchSize = 31) # to save time
      [kp_s, des_s] = orb.detectAndCompute(color_cv_s, None)
      [kp_t, des_t] = orb.detectAndCompute(color_cv_t, None)
      if len(kp_s) is 0 or len(kp_t) is 0:
          return success, trans
  
      bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
      matches = bf.match(des_s,des_t)
  
      pts_s = []
      pts_t = []
      for match in matches:
          pts_t.append(kp_t[match.trainIdx].pt)
          pts_s.append(kp_s[match.queryIdx].pt)
      pts_s = np.asarray(pts_s)
      pts_t = np.asarray(pts_t)
      # inlier points after initial BF matching
      if debug_draw_correspondences:
          draw_correspondences(np.asarray(source_rgbd_image.color),
                  np.asarray(target_rgbd_image.color), pts_s, pts_t,
                  np.ones(pts_s.shape[0]), "Initial BF matching")
  
      focal_input = (pinhole_camera_intrinsic.intrinsic_matrix[0,0] +
              pinhole_camera_intrinsic.intrinsic_matrix[1,1]) / 2.0
      pp_x = pinhole_camera_intrinsic.intrinsic_matrix[0,2]
      pp_y = pinhole_camera_intrinsic.intrinsic_matrix[1,2]
  
      # Essential matrix is made for masking inliers
      pts_s_int = np.int32(pts_s + 0.5)
      pts_t_int = np.int32(pts_t + 0.5)
      [E, mask] = cv2.findEssentialMat(pts_s_int, pts_t_int, focal=focal_input,
              pp=(pp_x, pp_y), method=cv2.RANSAC, prob=0.995, threshold=1.0)
      if mask is None:
          return success, trans
  
      # inlier points after 5pt algorithm
      if debug_draw_correspondences:
          draw_correspondences(np.asarray(source_rgbd_image.color),
                  np.asarray(target_rgbd_image.color),
                  pts_s, pts_t, mask, "5-pt RANSAC")
  
      # make 3D correspondences
      depth_s = np.asarray(source_rgbd_image.depth)
      depth_t = np.asarray(target_rgbd_image.depth)
      pts_xyz_s = np.zeros([3, pts_s.shape[0]])
      pts_xyz_t = np.zeros([3, pts_s.shape[0]])
      cnt = 0
      for i in range(pts_s.shape[0]):
          if mask[i]:
              xyz_s = get_xyz_from_pts(pts_s[i,:],
                      depth_s, pp_x, pp_y, focal_input)
              pts_xyz_s[:,cnt] = xyz_s
              xyz_t = get_xyz_from_pts(pts_t[i,:],
                      depth_t, pp_x, pp_y, focal_input)
              pts_xyz_t[:,cnt] = xyz_t
              cnt = cnt + 1
      pts_xyz_s = pts_xyz_s[:,:cnt]
      pts_xyz_t = pts_xyz_t[:,:cnt]
  
      success, trans, inlier_id_vec = estimate_3D_transform_RANSAC(
              pts_xyz_s, pts_xyz_t)
  
      if debug_draw_correspondences:
          pts_s_new = np.zeros(shape=(len(inlier_id_vec),2))
          pts_t_new = np.zeros(shape=(len(inlier_id_vec),2))
          mask = np.ones(len(inlier_id_vec))
          cnt = 0
          for i in inlier_id_vec:
              u_s,v_s = get_uv_from_xyz(pts_xyz_s[0,i], pts_xyz_s[1,i],
                      pts_xyz_s[2,i], pp_x, pp_y, focal_input)
              u_t,v_t = get_uv_from_xyz(pts_xyz_t[0,i], pts_xyz_t[1,i],
                      pts_xyz_t[2,i], pp_x, pp_y, focal_input)
              pts_s_new[cnt,:] = [u_s,v_s]
              pts_t_new[cnt,:] = [u_t,v_t]
              cnt = cnt + 1
          draw_correspondences(np.asarray(source_rgbd_image.color),
                  np.asarray(target_rgbd_image.color),
                  pts_s_new, pts_t_new, mask, "5-pt RANSAC + 3D Rigid RANSAC")
      return success, trans
  
  
  def draw_correspondences(img_s, img_t, pts_s, pts_t, mask, title):
      ha,wa = img_s.shape[:2]
      hb,wb = img_t.shape[:2]
      total_width = wa+wb
      new_img = np.zeros(shape=(ha, total_width))
      new_img[:ha,:wa]=img_s
      new_img[:hb,wa:wa+wb]=img_t
  
      fig = plt.figure()
      fig.canvas.set_window_title(title)
      for i in range(pts_s.shape[0]):
          if mask[i]:
              sx = pts_s[i,0]
              sy = pts_s[i,1]
              tx = pts_t[i,0] + wa
              ty = pts_t[i,1]
              plt.plot([sx,tx], [sy,ty], color=np.random.random(3)/2+0.5, lw=1.0)
      plt.imshow(new_img)
      plt.pause(0.5)
      plt.close()
  
  
  def estimate_3D_transform_RANSAC(pts_xyz_s, pts_xyz_t):
      max_iter = 1000
      max_distance = 0.03
      n_sample = 5
      n_points = pts_xyz_s.shape[1]
      Transform_good = np.identity(4)
      max_inlier = n_sample
      inlier_vec_good = []
      success = False
  
      if n_points < n_sample:
          return False, np.identity(4), []
  
      for i in range(max_iter):
  
          # sampling
          rand_idx = np.random.randint(n_points, size=n_sample)
          sample_xyz_s = pts_xyz_s[:,rand_idx]
          sample_xyz_t = pts_xyz_t[:,rand_idx]
          R_approx, t_approx = estimate_3D_transform(sample_xyz_s, sample_xyz_t)
  
          # evaluation
          diff_mat = pts_xyz_t - (np.matmul(R_approx, pts_xyz_s) +
                  np.tile(t_approx, [1, n_points]))
          diff = [np.linalg.norm(diff_mat[:,i]) for i in range(n_points)]
          n_inlier = len([1 for diff_iter in diff if diff_iter < max_distance])
  
          # note: diag(R_approx) > 0 prevents ankward transformation between
          # RGBD pair of relatively small amount of baseline.
          if (n_inlier > max_inlier) and (np.linalg.det(R_approx) != 0.0) and \
                  (R_approx[0,0] > 0 and R_approx[1,1] > 0 and R_approx[2,2] > 0):
              Transform_good[:3,:3] = R_approx
              Transform_good[:3,3] = [t_approx[0],t_approx[1],t_approx[2]]
              max_inlier = n_inlier
              inlier_vec = [id_iter for diff_iter, id_iter \
                      in zip(diff, range(n_points)) \
                      if diff_iter < max_distance]
              inlier_vec_good = inlier_vec
              success = True
  
      return success, Transform_good, inlier_vec_good
  
  
  # singular value decomposition approach
  # based on the description in the sec 3.1.2 in
  # http://graphics.stanford.edu/~smr/ICP/comparison/eggert_comparison_mva97.pdf
  def estimate_3D_transform(input_xyz_s, input_xyz_t):
      # compute H
      xyz_s = copy.copy(input_xyz_s)
      xyz_t = copy.copy(input_xyz_t)
      n_points = xyz_s.shape[1]
      mean_s = np.mean(xyz_s, axis=1)
      mean_t = np.mean(xyz_t, axis=1)
      mean_s.shape = (3,1)
      mean_t.shape = (3,1)
      xyz_diff_s = xyz_s - np.tile(mean_s, [1, n_points])
      xyz_diff_t = xyz_t - np.tile(mean_t, [1, n_points])
      H = np.matmul(xyz_diff_s,xyz_diff_t.transpose())
      # solve system
      U, s, V = np.linalg.svd(H)
      R_approx = np.matmul(V.transpose(), U.transpose())
      if np.linalg.det(R_approx) < 0.0:
          det = np.linalg.det(np.matmul(U,V))
          D = np.identity(3)
          D[2,2] = det
          R_approx = np.matmul(U,np.matmul(D,V))
      t_approx = mean_t - np.matmul(R_approx, mean_s)
      return R_approx, t_approx
  
  
  def get_xyz_from_pts(pts_row, depth, px, py, focal):
      u = pts_row[0]
      v = pts_row[1]
      u0 = int(u)
      v0 = int(v)
      height = depth.shape[0]
      width = depth.shape[1]
      # bilinear depth interpolation
      if u0 > 0 and u0 < width-1 and v0 > 0 and v0 < height-1:
          up = pts_row[0] - u0
          vp = pts_row[1] - v0
          d0 = depth[v0, u0]
          d1 = depth[v0, u0+1]
          d2 = depth[v0+1, u0]
          d3 = depth[v0+1, u0]
          d = (1-vp) * (d1 * up + d0 * (1-up)) + vp * (d3 * up + d2 * (1-up))
          return get_xyz_from_uv(u, v, d, px, py, focal)
      else:
          return [0,0,0]
  
  
  def get_xyz_from_uv(u, v, d, px, py, focal):
      if focal != 0:
          x = (u - px) / focal * d
          y = (v - py) / focal * d
      else:
          x = 0
          y = 0
      return np.array([x, y, d]).transpose()
  
  
  def get_uv_from_xyz(x, y, z, px, py, focal):
      if z != 0:
          u = focal * x / z + px
          v = focal * y / z + py
      else:
          u = 0
          v = 0
      return u, v
  