空飛ぶロボットのつくりかた

ロボットをつくるために必要な技術をまとめます。ロボットの未来についても考えたりします。

Oepn3Dのお勉強（三回目）

Open3D Python

やりたいこと

Depthセンサで取得したデータをOpen3Dで自由自在に操りたい

f:id:robonchu:20180224200259p:plain

教科書

Open3D: A Modern Library for 3D Data Processing — Open3D 0.1 dev documentation

Colored point cloud registration

Point to Point ICPでは幾何的な平面は揃うが、平面の三角形の模様などはズレてしまう.

しかし、Colored point cloud registrationでは色情報から、模様などからフィッティングできる.

Colored Point Cloud Registration Revisited, ICCV 2017

の実装👇

  1. Downsample with a voxel size.

  2. Estimate normal.

  3. Applying colored point cloud registration.

# src/Python/Tutorial/Advanced/colored_pointcloud_registration.py

import sys
import numpy as np
import copy
sys.path.append("../..")
from py3d import *


def draw_registration_result_original_color(source, target, transformation):
    source_temp = copy.deepcopy(source)
    source_temp.transform(transformation)
    draw_geometries([source_temp, target])


if __name__ == "__main__":

    print("1. Load two point clouds and show initial pose")
    source = read_point_cloud("../../TestData/ColoredICP/frag_115.ply")
    target = read_point_cloud("../../TestData/ColoredICP/frag_116.ply")

    # draw initial alignment
    current_transformation = np.identity(4)
    draw_registration_result_original_color(
            source, target, current_transformation)

    # point to plane ICP
    current_transformation = np.identity(4);
    print("2. Point-to-plane ICP registration is applied on original point")
    print("   clouds to refine the alignment. Distance threshold 0.02.")
    result_icp = registration_icp(source, target, 0.02,
            current_transformation, TransformationEstimationPointToPlane())
    print(result_icp)
    draw_registration_result_original_color(
            source, target, result_icp.transformation)

    # colored pointcloud registration
     # This is implementation of following paper
     # J. Park, Q.-Y. Zhou, V. Koltun,
     # Colored Point Cloud Registration Revisited, ICCV 2017
    voxel_radius = [ 0.04, 0.02, 0.01 ];
    max_iter = [ 50, 30, 14 ];
    current_transformation = np.identity(4)
    print("3. Colored point cloud registration")
    for scale in range(3):
        iter = max_iter[scale]
        radius = voxel_radius[scale]
        print([iter,radius,scale])

        print("3-1. Downsample with a voxel size %.2f" % radius)
        source_down = voxel_down_sample(source, radius)
        target_down = voxel_down_sample(target, radius)

        print("3-2. Estimate normal.")
        estimate_normals(source_down, KDTreeSearchParamHybrid(
                radius = radius * 2, max_nn = 30))
        estimate_normals(target_down, KDTreeSearchParamHybrid(
                radius = radius * 2, max_nn = 30))

        print("3-3. Applying colored point cloud registration")
        result_icp = registration_colored_icp(source_down, target_down,
                radius, current_transformation,
                ICPConvergenceCriteria(relative_fitness = 1e-6,
                relative_rmse = 1e-6, max_iteration = iter))
        current_transformation = result_icp.transformation
        print(result_icp)
    draw_registration_result_original_color(
            source, target, result_icp.transformation)

Before f:id:robonchu:20180225182439p:plain

After f:id:robonchu:20180225182458p:plain

Global registration

  • これまでのICPは初期値に依存するlocalなregistrationメソッド

  • ここでは、もうひとつのクラスであるglobal registrationの紹介

  • この計算結果はこれまでのlocal ICPの初期値として用いられる

# src/Python/Tutorial/Advanced/global_registration.py

import sys
sys.path.append("../..")
from py3d import *
import numpy as np
import copy

def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    draw_geometries([source_temp, target_temp])

if __name__ == "__main__":

    print("1. Load two point clouds and disturb initial pose.")
    source = read_point_cloud("../../TestData/ICP/cloud_bin_0.pcd")
    target = read_point_cloud("../../TestData/ICP/cloud_bin_1.pcd")
    trans_init = np.asarray([[0.0, 1.0, 0.0, 0.0],
                            [1.0, 0.0, 0.0, 0.0],
                            [0.0, 0.0, 1.0, 0.0],
                            [0.0, 0.0, 0.0, 1.0]])
    source.transform(trans_init)
    draw_registration_result(source, target, np.identity(4))

    print("2. Downsample with a voxel size 0.05.")
    source_down = voxel_down_sample(source, 0.05)
    target_down = voxel_down_sample(target, 0.05)

    print("3. Estimate normal with search radius 0.1.")
    estimate_normals(source_down, KDTreeSearchParamHybrid(
            radius = 0.1, max_nn = 30))
    estimate_normals(target_down, KDTreeSearchParamHybrid(
            radius = 0.1, max_nn = 30))

    print("4. Compute FPFH feature with search radius 0.25")
    source_fpfh = compute_fpfh_feature(source_down,
            KDTreeSearchParamHybrid(radius = 0.25, max_nn = 100))
    target_fpfh = compute_fpfh_feature(target_down,
            KDTreeSearchParamHybrid(radius = 0.25, max_nn = 100))

    print("5. RANSAC registration on downsampled point clouds.")
    print("   Since the downsampling voxel size is 0.05, we use a liberal")
    print("   distance threshold 0.075.")
    result_ransac = registration_ransac_based_on_feature_matching(
            source_down, target_down, source_fpfh, target_fpfh, 0.075,
            TransformationEstimationPointToPoint(False), 4,
            [CorrespondenceCheckerBasedOnEdgeLength(0.9),
            CorrespondenceCheckerBasedOnDistance(0.075)],
            RANSACConvergenceCriteria(4000000, 500))
    print(result_ransac)
    draw_registration_result(source_down, target_down,
            result_ransac.transformation)

    print("6. Point-to-plane ICP registration is applied on original point")
    print("   clouds to refine the alignment. This time we use a strict")
    print("   distance threshold 0.02.")
    result_icp = registration_icp(source, target, 0.02,
            result_ransac.transformation,
            TransformationEstimationPointToPlane())
    print(result_icp)
    draw_registration_result(source, target, result_icp.transformation)

f:id:robonchu:20180225190834p:plain

Extract geometric feature

Fast Point Feature Histograms
  • PFH の改良版. 特徴記述の組み合わせ数を削減して高速化.

参考：

The FPFH feature is a 33-dimensional vector that describes the local geometric property of a point.

A nearest neighbor query in the 33-dimensinal space can return points with similar local geometric structures. See [Rasu2009] for details.

RANSAC

  • Random Sampling Consensusの略

  • ロバスト推定法の1つ. 外れ値の影響を除外して、フィッティングなどができる.

参考：

The core function is registration_ransac_based_on_feature_matching.

The most important hyperparameter of this function is RANSACConvergenceCriteria.

It defines the maximum number of RANSAC iterations and the maximum number of validation steps.

The larger these two numbers are, the more accurate the result is, but also the more time the algorithm takes.

f:id:robonchu:20180225190904p:plain

これを初期値として、ICP registrationを行うと

f:id:robonchu:20180225191019p:plain

こんな感じ. いい感じ.

Multiway registration

手順

  1. Build a pose graph

    • A pose graph has two key elements: nodes and edges.

    • The script creates a pose graph with three nodes and three edges. Among the edges, two of them are odometry edges (uncertain = False) and one is a loop closure edge (uncertain = True).

  2. Optimize a pose graph

  3. Visualize optimization

Multiway registration is the process to align multiple pieces of geometry in a global space.

Open3D implements multiway registration via pose graph optimization. The backend implements the technique presented in [Choi2015].

# src/Python/Tutorial/Advanced/multiway_registration.py

import sys
sys.path.append("../..")
from py3d import *

if __name__ == "__main__":

    set_verbosity_level(VerbosityLevel.Debug)
    pcds = []
    for i in range(3):
        pcd = read_point_cloud(
                "../../TestData/ICP/cloud_bin_%d.pcd" % i)
        downpcd = voxel_down_sample(pcd, voxel_size = 0.02)
        pcds.append(downpcd)
    draw_geometries(pcds)

    pose_graph = PoseGraph()
    odometry = np.identity(4)
    pose_graph.nodes.append(PoseGraphNode(odometry))

    n_pcds = len(pcds)
    for source_id in range(n_pcds):
        for target_id in range(source_id + 1, n_pcds):
            source = pcds[source_id]
            target = pcds[target_id]

            print("Apply point-to-plane ICP")
            icp_coarse = registration_icp(source, target, 0.3,
                    np.identity(4),
                    TransformationEstimationPointToPlane())
            icp_fine = registration_icp(source, target, 0.03,
                    icp_coarse.transformation,
                    TransformationEstimationPointToPlane())
            transformation_icp = icp_fine.transformation
            information_icp = get_information_matrix_from_point_clouds(
                    source, target, 0.03, icp_fine.transformation)
            print(transformation_icp)

            # draw_registration_result(source, target, np.identity(4))
            print("Build PoseGraph")
            if target_id == source_id + 1: # odometry case
                odometry = np.dot(transformation_icp, odometry)
                pose_graph.nodes.append(
                        PoseGraphNode(np.linalg.inv(odometry)))
                pose_graph.edges.append(
                        PoseGraphEdge(source_id, target_id,
                        transformation_icp, information_icp, uncertain = False))
            else: # loop closure case
                pose_graph.edges.append(
                        PoseGraphEdge(source_id, target_id,
                        transformation_icp, information_icp, uncertain = True))

    print("Optimizing PoseGraph ...")
    option = GlobalOptimizationOption(
            max_correspondence_distance = 0.03,
            edge_prune_threshold = 0.25,
            reference_node = 0)
    global_optimization(pose_graph,
            GlobalOptimizationLevenbergMarquardt(),
            GlobalOptimizationConvergenceCriteria(), option)

    print("Transform points and display")
    for point_id in range(n_pcds):
        print(pose_graph.nodes[point_id].pose)
        pcds[point_id].transform(pose_graph.nodes[point_id].pose)
    draw_geometries(pcds)

Before f:id:robonchu:20180225194151p:plain

After f:id:robonchu:20180225194206p:plain

RGBD integration

Open3D implements a scalable RGBD image integration algorithm.

In order to support large scenes, we use a hierarchical hashing structure introduced in Integrater in ElasticReconstruction.

# src/Python/Tutorial/Advanced/rgbd_integration.py

import sys
sys.path.append("../..")
from py3d import *
from trajectory_io import *
import numpy as np

if __name__ == "__main__":
    camera_poses = read_trajectory("../../TestData/RGBD/odometry.log")  # <-抜けている
    volume = ScalableTSDFVolume(voxel_length = 4.0 / 512.0,
            sdf_trunc = 0.04, with_color = True)

    for i in range(len(camera_poses)):
        print("Integrate {:d}-th image into the volume.".format(i))
        color = read_image("../../TestData/RGBD/color/{:05d}.jpg".format(i))
        depth = read_image("../../TestData/RGBD/depth/{:05d}.png".format(i))
        rgbd = create_rgbd_image_from_color_and_depth(color, depth,
                depth_trunc = 4.0, convert_rgb_to_intensity = False)
        volume.integrate(rgbd, PinholeCameraIntrinsic.prime_sense_default,
                np.linalg.inv(camera_poses[i].pose))

    print("Extract a triangle mesh from the volume and visualize it.")
    mesh = volume.extract_triangle_mesh()
    mesh.compute_vertex_normals()
    draw_geometries([mesh])

Camera Trajectory

TSDF (Truncated Signed Distance Function) volume integration

面の位置を0として、各Voxelに面までの距離を符号付で格納

Extract a mesh

Mesh extraction uses the marching cubes algorithm [LorensenAndCline1987].

参考：　マーチングキューブ法 - Wikipedia

f:id:robonchu:20180225195949p:plain

所感

たくさんregistrationする方法はあるけど、タスク（環境や精度or速度の取捨選択）に応じて適切なものを選ぶにはカンコツがいりそう.

たくさん使って経験値をためたい.