From 5c07e28b9270eeb7e9c48ea90b81fe73e8f2bf80 Mon Sep 17 00:00:00 2001 From: Bing Jian Date: Tue, 4 Jun 2019 00:38:42 -0700 Subject: [PATCH] Update the script for running the dragon stand expt. --- expts/dragon_expts.py | 218 ++++++++++++++++++++++++++++------------- expts/dragon_stand.ini | 4 +- 2 files changed, 152 insertions(+), 70 deletions(-) diff --git a/expts/dragon_expts.py b/expts/dragon_expts.py index 0106e16..ce8e7df 100644 --- a/expts/dragon_expts.py +++ b/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(": %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(": %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() diff --git a/expts/dragon_stand.ini b/expts/dragon_stand.ini index c296941..e0d16ba 100644 --- a/expts/dragon_stand.ini +++ b/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