ROSの機械学習用に、OpenPoseから関節角度をTopicで流すパッケージ作った

概要

OpenPoseをROSで使いたかったわけですが、自分が欲しいパッケージがまだ無かったようなので作りました。
my github

ROSのOpenPoseドライバとしてはros-openposeがありますが、これは関節ごとの画面座標を出力するものでした。

自作パッケージはros-openposeのデータをより機械学習で使いやすいように関節角度に変換してTopicで投げるようにしたものです。

例

persons: 
  - 
    layout: 
      dim: []
      data_offset: 0
    data: [22, 0, 0, 56, 76, 0, 25, 29, 1, 0, 0, 0, 0, 34, 0, 0]
  - 
    layout: 
      dim: []
      data_offset: 0
    data: [40, 49, 4, 41, 52, 3, 10, 37, 10, 41, 8, 0, 0, 8, 0, 0]
num: 2

二人の人間が写っている時のTopic

実装した関節角度の計算方法

例えば肘の関節角度が欲しい場合、肘から手首と肩に向かってそれぞれベクトルが得られます。
この時、肘の関節角は２つのベクトルの内積から計算が可能です。
これを主な関節に対して行うことで関節角度を取得しています。

    def convertFormat2Vector(self, msg):
        ''' 
        @param msg : ros-openpose format
        @return output : array (16dim)
        '''
        output = np.zeros((0, 16))
        for person in msg.persons:
            angles = []
            ''' 
            confidence: 0.694853901863, part_id: 17
            x: 368
            y: 341
            '''
            for p in parts_id_pair:
                x0 = person.body_part[p[0]].x
                y0 = person.body_part[p[0]].y
                x1 = person.body_part[p[1]].x
                y1 = person.body_part[p[1]].y
                x2 = person.body_part[p[2]].x
                y2 = person.body_part[p[2]].y
                if all([x0, x1, x2, y0, y1, y2]):
                    u = np.array( [ x1 - x0, y1 - y0])
                    v = np.array( [ x2 - x0, y2 - y0])
                    angle = np.round(self.innerProduct(u, v))
                    angles.append(angle)
                else:
                    angles.append(self.MISSING_VALUE)

            print(angles)
            output = np.vstack((angles, output))
            print(output.shape)

        return output


    def innerProduct(self, u, v):
        ''' 
        calcJointAngle
        '''
        i = np.inner(u, v)
        n = np.linalg.norm(u) * np.linalg.norm(v)

        c = i / n
        deg = np.rad2deg(np.arccos(np.clip(c, -1.0, 1.0)))
        return deg

欠損値の扱い

カメラから体の一部が消えた時は欠損値が出るわけですが、
とりあえずその場合はデフォルトで０が入るようになっています。
一応変えたい場合はlaunchのrosparamをイジればいいように実装されています。

Topicの説明

Listで16個の値を投げています。
単位はDegreeで、16個の値は下の図のIndexと対応します。