Update the script for running the dragon stand expt.

expts/dragon_expts.py

@@ -1,56 +1,34 @@
 #!/usr/bin/env python
 #coding=utf-8
 
-import os, numpy, subprocess, time
+"""
+This Python script can be used to test the gmmreg algorithm on the Stanford
+"dragon stand" dataset as described in Section 6.1 of the gmmreg PAMI paper.
+"""
+
+import os, subprocess, time
+import numpy as np
 
 # https://github.com/bistromath/gr-air-modes/blob/master/python/Quaternion.py
 from Quaternion import Quat, normalize
 
-gmmreg_binary = {'nt' : r'gmmreg_demo.exe',
-                 'posix': r'gmmreg_demo'}
+BINARY_DIR = '../C++/build'
 
-data_path = '../data/dragon_stand'
-tmp_path = './tmp'
-config_file = './dragon_stand.ini'
+GMMREG_BINARY = {
+        'nt' : r'gmmreg_demo.exe',
+        'posix': r'gmmreg_demo'
+}
 
-if not os.path.exists(tmp_path):
-    os.makedirs(tmp_path)
 
-def run_rigid(gmmreg_exe, model, scene, f_config, method = 'rigid'):
-    if type(model) == type('abc'):
-        model = numpy.loadtxt(model)
-        scene = numpy.loadtxt(scene)
-    if model.shape[0] > 10000:
-        model = model[0::10]
-        scene = scene[0::10]
-    numpy.savetxt(os.path.join(tmp_path, 'model.txt'), model)
-    numpy.savetxt(os.path.join(tmp_path, 'scene.txt'), scene)
-    cmd = '%s %s %s'%(gmmreg_exe, f_config, 'rigid')
-    t1 = time.time()
-    subprocess.call(cmd, shell=True)
-    t2 = time.time()
-    print "Run time : %s seconds" % (t2-t1)
-    param = numpy.loadtxt(os.path.join(tmp_path, 'final_rigid.txt'))
-    return param, t2-t1
+BINARY_FULLPATH = os.path.join(BINARY_DIR, GMMREG_BINARY[os.name])
+DATA_PATH = '../data/dragon_stand'
+TMP_PATH = './tmp'
+CONFIG_FILE = './dragon_stand.ini'
 
-def ply2txt(plyfile):
-    return '%s.txt' % os.path.splitext(plyfile)[0]
 
-def pairwise_rigid3d(model_ply, scene_ply, f_config):
-    model_txt = ply2txt(model_ply)
-    scene_txt = ply2txt(scene_ply)
-    print model_txt, scene_txt
-    binary = os.path.join(os.getcwd(), gmmreg_binary[os.name])
-    return run_rigid(binary, model_txt, scene_txt, f_config)
-
-def ground_truth(model_ply, scene_ply, gt_pos):
-    pos_i = gt_pos[os.path.basename(model_ply)]
-    pos_j = gt_pos[os.path.basename(scene_ply)]
-    q_i = Quat(normalize(numpy.array(pos_i[3::])))
-    q_j = Quat(normalize(numpy.array(pos_j[3::])))
-    q_ji = q_i / q_j
-    q_ij = q_j / q_i
-    return q_ij, q_ji
+if not os.path.exists(TMP_PATH):
+    os.makedirs(TMP_PATH)
+
 
 def load_dragon_conf(data_path):
     conf = os.path.join(data_path, 'dragonStandRight.conf')
@@ -63,44 +41,146 @@ def load_dragon_conf(data_path):
         pos[fname] = param
     return pos
 
+GT_POS = load_dragon_conf(DATA_PATH)
+
 def get_all_plyfiles(data_path):
     ind = range(0, 360, 24)
     return [os.path.join(data_path,
                          'dragonStandRight_%d.ply'%j) for j in ind]
 
-def run_rigid_batch(ply_files, step, config_file, res):
+
+PLY_FILES = get_all_plyfiles(DATA_PATH)
+
+
+
+# Run pairwise rigid registration using specified binary and configuration.
+def run_rigid_pairwise(gmmreg_exe, model, scene, f_config):
+    if type(model) == type('abc'):
+        model = np.loadtxt(model)
+        scene = np.loadtxt(scene)
+    if model.shape[0] > 10000:
+        model = model[0::10]
+        scene = scene[0::10]
+    print(model.shape)
+    print(scene.shape)
+    np.savetxt(os.path.join(TMP_PATH, 'model.txt'), model)
+    np.savetxt(os.path.join(TMP_PATH, 'scene.txt'), scene)
+    cmd = '%s %s %s'%(gmmreg_exe, f_config, 'rigid')
+    t1 = time.time()
+    subprocess.call(cmd, shell=True)
+    t2 = time.time()
+    print("Run time : %s seconds" % (t2 - t1))
+    param = np.loadtxt(os.path.join(TMP_PATH, 'final_rigid.txt'))
+    matrix = np.loadtxt(os.path.join(TMP_PATH, 'final_rigid_matrix.txt'))
+    return param, matrix, t2 - t1
+
+
+def ply2txt(plyfile):
+    return '%s.txt' % os.path.splitext(plyfile)[0]
+
+
+def run_rigid_batch(step, res):
     for i in range(15):
         j = (i + step) % 15
-        model_ply = ply_files[i]
-        scene_ply = ply_files[j]
-        print model_ply, scene_ply
-        res[(i, j)] = pairwise_rigid3d(model_ply, scene_ply, config_file)
+        model_ply = PLY_FILES[i]
+        scene_ply = PLY_FILES[j]
+        model_txt = ply2txt(model_ply)
+        scene_txt = ply2txt(scene_ply)
+        print(model_txt, scene_txt)
+        res[(i, j)] = run_rigid_pairwise(BINARY_FULLPATH, model_txt, scene_txt, CONFIG_FILE)
     return res
 
-def compare_rotation(ply_files, reg_result, gt_pos):
-    res = {}
-    for i, j in reg_result:
-        model_ply = ply_files[i]
-        scene_ply = ply_files[j]
-        q_ij, q_ji = ground_truth(model_ply, scene_ply, gt_pos)
-        param = reg_result[(i,j)][0]
-        q = Quat(normalize(param[0:4]))
-        res[(i, j)] = abs(numpy.dot(q.q, q_ji.q))
-    return res
 
-def run_single_step(step, reg_result):
-    ply_files = get_all_plyfiles(data_path)
-    reg_result = run_rigid_batch(ply_files, step, config_file, reg_result)
-    return reg_result
+def lookup_ground_truth(i, j):
+    model_ply = PLY_FILES[i]
+    scene_ply = PLY_FILES[j]
+    pos_i = GT_POS[os.path.basename(model_ply)]
+    pos_j = GT_POS[os.path.basename(scene_ply)]
+    q_i = Quat(normalize(np.array(pos_i[3::])))
+    q_j = Quat(normalize(np.array(pos_j[3::])))
+    q_ji = q_i / q_j
+    q_ij = q_j / q_i
+    return q_ij, q_ji
 
-def run_all_steps():
-    reg_result = {}
-    for step in [-1, 1, -2, 2, -3, 3]:
-        reg_result = run_single_step(step, reg_result)
-    return reg_result
 
+# Distance between rotations
+# http://www.boris-belousov.net/2016/12/01/quat-dist/
 def compute_accuracy(reg_result):
-    gt_pos = load_dragon_conf(data_path)
-    ply_files = get_all_plyfiles(data_path)
-    accuracy = compare_rotation(ply_files, reg_result, gt_pos)
-    return accuracy
+    error = {} # error in degrees.
+    run_time = {}
+    for i, j in reg_result:
+        q_ij, q_ji = lookup_ground_truth(i, j)
+        param = reg_result[(i,j)][0]
+        q = Quat(normalize(param[0:4]))
+        similarity = np.abs(np.dot(q.q, q_ji.q))
+        error[(i, j)] = 2 * np.arccos(similarity)*180 / np.pi
+        run_time[(i, j)] = reg_result[(i,j)][-1]
+    return error, run_time
+
+
+# Run pair-wise registrations, record errors and run time.
+def main():
+    errors = {}
+    run_times = {}
+    for step in [1, 2, 3]:
+        reg_result = {}
+        reg_result = run_rigid_batch(step, reg_result)
+        reg_result = run_rigid_batch(-step, reg_result)
+        error, run_time = compute_accuracy(reg_result)
+        errors[step] = np.array([error[ij] for ij in error])
+        run_times[step] = np.array([run_time[ij] for ij in run_time])
+    for step in errors:
+        print("-----------")
+        print("Input pairs with pose difference ~= %d degrees" % (step * 24))
+        error = errors[step]
+        print("<avg_err, min_err, max_err, median_err>: %f, %f, %f, %f (in degrees)" % (
+            error.mean(), error.min(), error.max(), np.median(error)))
+        print("<# of small errors (<3 degree)>: %d out of %d)" % (
+            len(np.where(error < 3)[0]), len(error)))
+        run_time = run_times[step]
+        print("<avg_time, min_time, max_time, median_time>: %f, %f, %f, %f (in seconds)" % (
+            run_time.mean(), run_time.min(), run_time.max(), np.median(run_time)))
+        print("Please note that the run time reported above includes time spend on reading/writing files.")
+
+
+# Please comment the code using open3d (www.open3d.org) if it is not installed.
+def visualize_registration(i, j):
+    try:
+        from open3d.geometry import PointCloud
+        from open3d.utility import Vector3dVector
+        from open3d.visualization import draw_geometries
+    except:
+        from open3d import PointCloud, Vector3dVector, draw_geometries
+    model_ply = PLY_FILES[i]
+    scene_ply = PLY_FILES[j]
+    model_txt = ply2txt(model_ply)
+    scene_txt = ply2txt(scene_ply)
+    model = np.loadtxt(model_txt)
+    scene = np.loadtxt(scene_txt)
+    print(model_txt, scene_txt)
+    pcloud_model = PointCloud()
+    pcloud_model.points = Vector3dVector(model)
+    pcloud_model.paint_uniform_color([1, 0, 0]) # red
+    pcloud_scene = PointCloud()
+    pcloud_scene.points = Vector3dVector(scene)
+    pcloud_scene.paint_uniform_color([0, 1, 0]) # green
+    draw_geometries([pcloud_model, pcloud_scene])
+    res = run_rigid_pairwise(BINARY_FULLPATH, model, scene, CONFIG_FILE)
+    print(res)
+    transformed = np.loadtxt(os.path.join(TMP_PATH, 'transformed_model.txt'))
+    pcloud_transformed = PointCloud()
+    pcloud_transformed.points = Vector3dVector(transformed)
+    pcloud_transformed.paint_uniform_color([0, 0, 1]) # blue
+    draw_geometries([pcloud_transformed, pcloud_scene])
+
+
+import sys
+if __name__ == "__main__":
+    # Register just one pair, visualize point clouds before/after alignment.
+    try:
+        import open3d
+        visualize_registration(0, 1)
+    except:
+        pass
+    # Run registration on many more specified pairs and collect metrics.
+    main()

expts/dragon_stand.ini

@@ -2,9 +2,11 @@
 model = ./tmp/model.txt
 scene = ./tmp/scene.txt
 final_rigid = ./tmp/final_rigid.txt
+final_rigid_matrix = ./tmp/final_rigid_matrix.txt
 transformed_model = ./tmp/transformed_model.txt
+
 [GMMREG_OPT]
 normalize = 0
-max_function_evals = 100 100 400 400
+max_function_evals = 50 100 100 100
 sigma = .01  .005  .1  .05
 level = 1