C#
Unity
法線
点群

点群から法線を推定する

a.png

b.png

概要

  1. 各点ごとに最も近いk個の頂点を取得する。(kは手動で決める)
  2. そのk個の点群でPCA(主成分分析)を行う。
  3. PCAの第三主軸を法線とする。

参考

http://hhoppe.com/proj/recon/
の論文の3.2 Tangent Plane Estimationを参考にしています。
入力データは、上のURLからダウンロードできるデータファイルの中にあるmechpart.4102.ptsを使っています。

PCAの主軸を求めるために、固有値問題を解く必要があります。
Computer Graphics Gems JP 2012という本のShape Matchingの項目に載っている3x3行列用のヤコビ法を使いました。

ソースコードの断片と使い方

使い方

Load_Pts_DataとKnbhn_PCA_Axis3を空のゲームオブジェクトAddすると使えます。

共分散行列、固有値、固有ベクトル、PCAの主軸を求める。

using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class RomaCov : MonoBehaviour {

    struct RomaEigen
    {
        public float val; // 固有値
        public Vector3 vec; // 固有ベクトル

        // 固有値の大きい順に並べるために使う関数
        static public int Compare(RomaEigen e1, RomaEigen e2)
        {
            float a = Mathf.Abs(e1.val);
            float b = Mathf.Abs(e2.val);

            if (a > b)
                return -1;
            else if (a < b) 
                return 1;
            else
                return 0;
        }

        // デバッグ表示用
        public void Show()
        {
            print("val : " + val + ", vec : " + vec);
        }
    }

    // Use this for initialization

    RomaEigen[] e;
    Transform[] tt;

    void Start () {
        tt = new Transform[transform.childCount];
        for (int i = 0; i < tt.Length; i++)
        {
            tt[i] = transform.GetChild(i);
        }
    }

    void CalcCV()
    {
        Vector3[] vv = new Vector3[tt.Length];
        for (int i = 0; i < tt.Length; i++)
        {
            vv[i] = tt[i].position;
        }

        Matrix4x4 cv = CoVarianceMatrix(vv); // 共分散行列

        // print("共分散行列");
        // print(cv);

        Matrix4x4 eVec; // 固有ベクトルが縦に並んでいる。
        Matrix4x4 eVal = Jacobi33(cv, out eVec); // 対角行列、固有値が対角に並んでいる。

        // print(eVal);
        // print(eVec);

        e = new RomaEigen[3];
        for (int i = 0; i < e.Length; i++)
        {
            e[i].val = eVal[i, i];
            float x = eVec[0, i];
            float y = eVec[1, i];
            float z = eVec[2, i];
            e[i].vec = new Vector3(x, y, z);
        }

        Array.Sort(e, RomaEigen.Compare);
    }

    // 主成分分析の第三主軸を取得する。
    static public Vector3 GetPCA_Axis3(Vector3[] vv)
    {
        Matrix4x4 cv = CoVarianceMatrix(vv); // 共分散行列

        Matrix4x4 eVec; // 固有ベクトルが縦に並んでいる。
        Matrix4x4 eVal = Jacobi33(cv, out eVec); // 対角行列、固有値が対角に並んでいる。

        RomaEigen[] ee = new RomaEigen[3];
        for (int i = 0; i < ee.Length; i++)
        {
            ee[i].val = eVal[i, i];
            float x = eVec[0, i];
            float y = eVec[1, i];
            float z = eVec[2, i];
            ee[i].vec = new Vector3(x, y, z);
        }

        Array.Sort(ee, RomaEigen.Compare);

        return ee[2].vec;
    }

    // Update is called once per frame
    void Update () {
        if(Input.GetKeyDown(KeyCode.E))
        {
            CalcCV();
        }
    }

    // 分散を求める
    // E[(Xi - E[Xi])(Xi - E[Xi])]
    static Vector3 Variance(Vector3[] vv)
    {
        Vector3 E = Ave(vv); // 期待値

        Vector3[] vv_a = new Vector3[vv.Length];
        for (int i = 0; i < vv_a.Length; i++)
        {
            Vector3 v = vv[i] - E;
            vv_a[i] = new Vector3(v.x * v.x, v.y * v.y, v.z * v.z);
        }

        return Ave(vv_a);
    }

    // 共分散を求める
    // E[(Xi - E[Xi])(Xj - E[Xj])]
    // xy,xz,yzの順番で返す。
    static Vector3 CoVariance(Vector3[] vv)
    {
        Vector3 E = Ave(vv); // 期待値

        Vector3[] vv_a = new Vector3[vv.Length];
        for (int i = 0; i < vv_a.Length; i++)
        {
            Vector3 v = vv[i] - E;
            vv_a[i] = new Vector3(v.x * v.y, v.x * v.z, v.y * v.z);
        }

        return Ave(vv_a);
    }

    // 共分散行列を求める
    static Matrix4x4 CoVarianceMatrix(Vector3[] vv)
    {
        Matrix4x4 mat = Matrix4x4.identity;
        Vector3 v = Variance(vv);
        Vector3 cv = CoVariance(vv);
        mat[0, 0] = v.x;
        mat[1, 1] = v.y;
        mat[2, 2] = v.z;

        mat[1, 0] = mat[0, 1] = cv.x;
        mat[2, 0] = mat[0, 2] = cv.y;
        mat[1, 2] = mat[2, 1] = cv.z;

        return mat;
    }

    // 対称行列Mから固有値が対角に並んだ行列(戻り値)と、固有ベクトルが縦に並んだ直交行列Pを得る。
    // 入力のMは、必ず対称行列でなければならない。
    static Matrix4x4 Jacobi33(Matrix4x4 M, out Matrix4x4 _P)
    {
        int cnt = 0;
        Matrix4x4 P = Matrix4x4.identity;

        while (cnt < 30) // 最大繰り返し回数
        {
            float[] aa = new float[3];
            aa[0] = M[1, 0];
            aa[1] = M[2, 0];
            aa[2] = M[1, 2];
            int id = 0;
            float aa_max = 0;
            for (int i = 0; i < aa.Length; i++)
            {
                float hoge = Mathf.Abs(aa[i]);
                if (hoge > aa_max)
                {
                    id = i;
                    aa_max = hoge;
                }
            }

            if (aa_max < 0.0001) // 打切り誤差
                break;

            float m12 = aa[0];
            float m13 = aa[1];
            float m23 = aa[2];
            float m11 = M[0, 0];
            float m22 = M[1, 1];
            float m33 = M[2, 2];

            Matrix4x4 R = Matrix4x4.identity;
            switch (id)
            {
                case 0:
                    {
                        float th = 0.5f * Mathf.Atan(-2 * m12 / (m11 - m22));
                        R[0, 0] = Mathf.Cos(th); R[0, 1] = Mathf.Sin(th);
                        R[1, 0] = -Mathf.Sin(th); R[1, 1] = Mathf.Cos(th);
                    }
                    break;
                case 1:
                    {
                        float th = 0.5f * Mathf.Atan(-2 * m13 / (m11 - m33));
                        R[0, 0] = Mathf.Cos(th); R[0, 2] = Mathf.Sin(th);
                        R[2, 0] = -Mathf.Sin(th); R[2, 2] = Mathf.Cos(th);
                    }
                    break;
                case 2:
                    {
                        float th = 0.5f * Mathf.Atan(-2 * m23 / (m22 - m33));
                        R[1, 1] = Mathf.Cos(th); R[1, 2] = Mathf.Sin(th);
                        R[2, 1] = -Mathf.Sin(th); R[2, 2] = Mathf.Cos(th);
                    }
                    break;
            }

            M = R.transpose * M * R;
            P = P * R;

            cnt++;
        }

        _P = P;
        return M;
    }

    // 平均値を求める。本当は期待値。座標値の場合は平均値でいいらしい?
    // 重心を求めるのにも使える
    static public Vector3 Ave(Vector3[] vv)
    {
        Vector3 sum = Vector3.zero;
        for (int i = 0; i < vv.Length; i++)
        {
            sum += vv[i];
        }

        return sum / vv.Length;
    }

    private void OnDrawGizmos()
    {
        Gizmos.color = Color.red;
        Gizmos.DrawLine(Vector3.zero, Vector3.right * 10);
        Gizmos.color = Color.green;
        Gizmos.DrawLine(Vector3.zero, Vector3.up * 10);

        if (e == null)
            return;

        // 主成分分析で出てきた軸を描画する。
        Gizmos.color = Color.magenta;
        Gizmos.DrawLine(Vector3.zero, e[0].vec * 10);

        Gizmos.color = Color.yellow;
        Gizmos.DrawLine(Vector3.zero, e[1].vec * 10);

        Gizmos.color = Color.cyan;
        Gizmos.DrawLine(Vector3.zero, e[2].vec * 10);
    }
}

k個の近傍頂点を選び、そこから第三主軸を求め法線としている。法線の始点はk個の頂点の重心としている。

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Knbhn_PCA_Axis3 : MonoBehaviour
{ 
    int KK = 10; // 近傍頂点5個を選ぶ。
    Vector3[] vv;

    class CandN
    {
        public Vector3 n;
        public Vector3 c;
    }

    CandN[] cn;

    // Use this for initialization
    void Start()
    {

    }

    // Update is called once per frame
    void Update () {
        if(Input.GetKeyDown(KeyCode.A))
        {
            if (vv == null)
            {
                vv = new Vector3[transform.childCount];
                for (int i = 0; i < vv.Length; i++)
                {
                    var t = transform.GetChild(i);
                    t.name = i.ToString();
                    vv[i] = t.position;
                }
                cn = new CandN[vv.Length];

                for (int j = 0; j < vv.Length; j++)
                {
                    int select = j;
                    int[] ids = GetNearID(select);

                    Vector3[] vvv = new Vector3[ids.Length];
                    for (int i = 0; i < vvv.Length; i++)
                    {
                        vvv[i] = vv[ids[i]];
                    }

                    Vector3 cen = RomaCov.Ave(vvv);
                    Vector3 n = RomaCov.GetPCA_Axis3(vvv);
                    cn[j] = new CandN();
                    cn[j].c = cen;
                    cn[j].n = n;

                    //transform.GetChild(select).GetComponent<MeshRenderer>().material.color = Color.red;
                    //for (int i = 0; i < ids.Length; i++)
                    //{
                    //    int id = ids[i];
                    //    transform.GetChild(id).GetComponent<MeshRenderer>().material.color = Color.green;
                    //}
                }
            }
        }

        if (cn != null)
        {
            for (int i = 0; i < cn.Length; i++)
            {
                Debug.DrawLine(cn[i].c, cn[i].c + cn[i].n);
            }
        }
    }

    int[] GetNearID(int j)
    {
        Stack_k sk = new Stack_k(KK);
        for (int i = 0; i < vv.Length; i++)
        {
            if (i == j)
                continue;

            float d = Vector3.Distance(vv[j], vv[i]);
            sk.Push(d, i);
        }

        return sk.id;
    }
}

class Stack_k
{
    public int[] id;
    float[] dist;

    public Stack_k(int stackNum)
    {
        id = new int[stackNum];
        dist = new float[stackNum];
        for (int i = 0; i < dist.Length; i++)
        {
            id[i] = -999;
            dist[i] = float.MaxValue;
        }
    }

    public void Push(float f, int _id)
    {
        if (Check(f))
        {
            for (int i = 0; i < dist.Length; i++)
            {
                if (f < dist[i])
                {
                    for (int j = dist.Length - 1; j > i; j--)
                    {
                        dist[j] = dist[j - 1];
                        id[j] = id[j - 1];
                    }

                    dist[i] = f;
                    id[i] = _id;
                    break;
                }
            }
        }
    }

    public void Show()
    {
        for (int i = 0; i < dist.Length; i++)
        {
            Debug.Log("[" + i + "] dist:" + dist[i] + " id:" + id[i]);
        }
    }


    bool Check(float f)
    {
        if (f < dist[dist.Length - 1])
            return true;
        else
            return false;
    }
}

ptsフォーマットから点群を読み込み、点のある位置に球体を生成する。

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Load_Pts_Data : MonoBehaviour {

    // Use this for initialization
    void Start () {
        string[] pts_data = Roma.Util.Load("Assets/mechpart.4102.pts");
        List<Vector3> po = new List<Vector3>();
        Vector3 cen = Vector3.zero;
        Vector3 min = Vector3.one * 999, max = -Vector3.one * 999;
        foreach (var ss in pts_data)
        {
            var s = ss.Split(' ');
            if (s[0] == "p")
            {
                float x = float.Parse(s[1]);
                float y = float.Parse(s[2]);
                float z = float.Parse(s[3]);
                Vector3 v = new Vector3(x, y, z);
                po.Add(v);
                cen += v;

                {
                    if (v.x < min.x)
                        min.x = v.x;
                    if (v.y < min.y)
                        min.y = v.y;
                    if (v.z < min.z)
                        min.z = v.z;

                    if (v.x > max.x)
                        max.x = v.x;
                    if (v.y > max.y)
                        max.y = v.y;
                    if (v.z > max.z)
                        max.z = v.z;
                }
            }
            else if (s[0] == "#")
            {
                print(ss);
            }
        }

        cen /= po.Count;
        Vector3 size = max - min;
        print("size : " + size);
        print("cen : " + cen);
        float dispScale = 5;

        foreach (var p in po)
        {
            var t = GameObject.CreatePrimitive(PrimitiveType.Sphere).transform;
            t.position = (p - cen) * (dispScale / size.x);
            t.parent = transform;
            t.localScale = Vector3.one * (size.x * dispScale * 0.1f);
        }
    }

    // Update is called once per frame
    void Update () {

    }
}