Lidarで取得した角度と距離からⅩ、Yを求める

Question

前提・実現したいこと

初めて質問させていただきます。
現在、Slamtec社のRPLidar A3(https://www.slamtec.com/en/Lidar/A3)を用いた、マッピングシステムを、RaspberryPi3 B+ にて作成しております。
SDKが公開されているので、その中のultra_simpleというアプリケーションでテストを行い、C++には不慣れながらも、出力データに日付と時刻を付与するところまではできました。
出力結果は以下の通りです。

2020-02-14 16:41:06.3,0.12,02084.00
2020-02-14 16:41:06.3,0.34,02088.00
2020-02-14 16:41:06.3,0.56,02084.00
(中略)
2020-02-14 16:41:06.3,1.48,02084.00
2020-02-14 16:41:06.3,1.72,02084.00

データは「日時,theta,dist」という要素で、Lidarが取得した点群までの角度（theat）と距離（dist）を表しています。
そのままでは、点群を閲覧するソフト等で閲覧することができない(*1)ため、これらのデータから、Ｘ座標とＹ座標に変換する必要があります。

*1：これはもしかしたら私が探しきれていないだけかもしれません。もしご存知の方がおられたらご教示いただけますと幸いです。

また、Lidarはクローラーに搭載して使用しているのですが、従来通りの水平ではなく、垂直に設置しています。

閉鎖空間を進みながら、その内壁を点群で取得していくことが目的です。
そのため、実際には進んだ距離も必要となりますが、こちらはクローラーにエンコーダーを搭載して、取得しています。

発生している問題

現在、最後の、ＸＹ座標への変換の工程で、行き詰っております。
当初、こちらのサイトを参考に変換を行ったのですが、うまくいかず、
三角関数部分を少し変更して、以下のように変更しました。

[参考ソース]

#!/usr/bin/python3

import re
import math

file = "test.dat" # Lidarログ出力ファイル

f = open(file, 'r')
line = f.readline()

while line:
    line = line.strip()
    m = re.search("theta: ([0-9.]+) Dist: ([0-9.]+) Q: ([0-9]+)", line)
    s = float(m.group(1))
    c = float(m.group(2))
    r = s * math.pi /180
    a = c * math.sin(r)
    b = (-1.0) * c * math.cos(r)
    print(str(a) + " " + str(b) + " " + "1.0")
    line = f.readline()

f.close()

[修正後ソース]

#!/usr/bin/python3

import math

file = "lidarlog2.txt" # Lidarログ出力ファイル

f = open(file, 'r')
line = f.readline()
ignore_line = 1

for line in f:
    if ignore_line > 9: # headerを無視
        line = line.strip().rstrip('\n')
        line_data = line.split(",")
        s = float(line_data[1])
        c = float(line_data[2])
        print(s)
        a = c * math.cos(s) # X座標
        b = c * math.sin(s) # Y座標
        print(line_data[0] + " " + str(a) + " " + str(b))
        line = f.readline()
    else:    
        ignore_line = ignore_line + 1

f.close()

いずれの場合でも、リダイレクト出力したファイルを点群閲覧ソフトで読み込んだところ、以下のようになりました。

上記画像は、クローラーを停止させた状態で、数秒間、Lidarを回して取得した点群を、変換したものです。
正しくは、赤で囲った部分が、左の天井から壁、床に当たり、全体では長方形になるはずのところ、
左に行けば行くほど曲がってしまいます。

計算がまずいのか、そもそもの考え方が間違っているのか、自分では思い至らないため、先達の皆様の知見をお借りいただければと、
望む次第です。

試したこと

はっきり言って、まったくなす術がないのですが、補足にも書いております通り、幸いにしてPythonで点群取得を実装されている方が
おられたので、そのコードから該当しそうな部分を参考に、ソースを変更したりもしましたが、今度は点群が全て同心円上に並んでしまい、
まともな形状にすらなりませんでした。

補足情報（FW/ツールのバージョンなど）

最初、こちらのソースを利用していたのですが、
C++のほうが取得点群数が倍以上も多いため、このような回りくどいことを行っています。

最後に

何をどう書いていいかもよくわからないまま、思いつくことを書いてまいりましたが、ほかに必要な情報がありましたら、ご指摘いただければ幸いです。
よろしくお願いいたします。

修正（2020/2/27 16:30）

修正後のソースで、角度をラジアンに変換して、三角関数の引数のする箇所を間違っていました。
ただ、この点に関して、ラジアンに変換せずにthetaを直接使用するように改変したかといいうと、

1. Lidarの仕様書に、thetaが角度として図示されており、
2. 参考ソースで変換した際に点群が前述の画像の通り、歪んでいたため

です。
最初に使用した修正後ソースは以下の通りです。

[修正後ソース]

#!/usr/bin/python3

import math

file = "lidarlog2.txt" # Lidarログ出力ファイル

f = open(file, 'r')
line = f.readline()
ignore_line = 1

for line in f:
    if ignore_line > 9: # headerを無視
        line = line.strip().rstrip('\n')
        line_data = line.split(",")
        s = float(line_data[1])
        c = float(line_data[2])
        print(s)
        r = s * math.pi /180
        a = c * math.cos(r) # X座標
        b = c * math.sin(r) # Y座標
        print(line_data[0] + " " + str(a) + " " + str(b))
        line = f.readline()
    else:    
        ignore_line = ignore_line + 1

f.close()

修正と追加（2020/2/28 11:05）

Githubに専用のリポジトリを作って、Pythonのソースと部屋の中で取得した点群データをアップしました。
ソースも一部修正をしています。
具体的には、if ブロック内の以下の点です。

• 無視するheader部分の行数が間違っていたので、ignore_line > 9 を、 ignore_line > 8 に変更
• line = f.readline() を削除

修正：解決編（2020/2/28 14:55）

yuki23様のご指摘にあった、出力されている角度が360を超えている問題を解決すべく、元々のC++のソースと、自身が改変したC++を見比べ、可能な限り元の状態を維持しつつ、必要なデータを加えていくように修正を行いました。
その結果、360度以上のデータが出力されることはなくなりました。
そのままの形では、依然θと距離しか出力されてませんので、上述の修正後ソースをさらに修正し、Ⅹ座標とY座標に変換したところ、以下のように、部屋の形がきちんと描画されました。

元のソース、修正しておかしくなっていたソース、最終的に正常に動いたソースは以下の通りです。

[オリジナルC++ソース(抜粋）]

    while (1) {
        rplidar_response_measurement_node_hq_t nodes[8192];
        size_t   count = _countof(nodes);

        op_result = drv->grabScanDataHq(nodes, count);
        if (IS_OK(op_result)) {
            drv->ascendScanData(nodes, count);
            for (int pos = 0; pos < (int)count ; ++pos) {
                printf("%s theta: %03.2f Dist: %08.2f Q: %d \n", 
                    (nodes[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) ?"S ":"  ", 
                    (nodes[pos].angle_z_q14 * 90.f / (1 << 14)), 
                    nodes[pos].dist_mm_q2/4.0f,
                    nodes[pos].quality);
            }
        }

        if (ctrl_c_pressed){ 
            break;
        }
    }

[修正失敗版C++ソース(抜粋）]

    while (1) {
        rplidar_response_measurement_node_t nodes[8192];
        size_t   count = _countof(nodes);

        op_result = drv->grabScanData(nodes, count);

        if (IS_OK(op_result)) {
            drv->ascendScanData(nodes, count);
            for (int pos = 0; pos < (int)count ; ++pos) {
                clock_gettime(CLOCK_REALTIME, &ts);
                std::strftime(buf, sizeof buf, "%F %T", std::localtime(&ts.tv_sec));

                millisec = ts.tv_nsec / 100000000;
                theta = (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
                dist = nodes[pos].distance_q2/4.0f;

                outfile << buf 
                        << ":" 
                        << millisec 
                        << ","
                        << std::to_string(theta)
                        << ","
                        << std::to_string(dist)
                        << "\n";

                printf("%s.%ld,%03.2f,%08.2f\n",
                        buf,
                        millisec,
                        (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f,
                        nodes[pos].distance_q2/4.0f);
            }
        }

        if (ctrl_c_pressed){
            break;
        }
    }

[最終版C++ソース(抜粋）]

    while (1) {
        rplidar_response_measurement_node_hq_t nodes[8192];
        size_t   count = _countof(nodes);

        op_result = drv->grabScanDataHq(nodes, count);
        if (IS_OK(op_result)) {
            drv->ascendScanData(nodes, count);
            for (int pos = 0; pos < (int)count ; ++pos) {
				clock_gettime(CLOCK_REALTIME, &ts);
				std::strftime(buf, sizeof buf, "%F %T", std::localtime(&ts.tv_sec));
				millisec = ts.tv_nsec / 100000000;
                printf("%s.%ld,%03.2f,%08.2f\n",
                    buf,
					millsec,
                    (nodes[pos].angle_z_q14 * 90.f / (1 << 14)),
                    nodes[pos].dist_mm_q2/4.0f);
            }
        }

        if (ctrl_c_pressed){ 
            break;
        }
    }

whileブロックに入ってすぐの、rplidar_response_measurement_node_hq_t nodes[8192];というところで、hqがついたりつかなかったりしていますが、
修正失敗版を最初にmakeした時、「hqありは使えない、ないのを使え」みたいなエラーをはかれたため、修正し、その後発生するエラーに対応していったら、結果的に上述のようになった次第です。
今回、改めてSDKを落とすと、バージョンが上がっており、それが原因か、単に以前が自分のミスだったのかは不明ですが、するっとmakeできてしまい、動くに至りました。
もっと細かく、何がどうしてエラーだったか、ご報告するのが本来の在り方だとは承知しておりますが、いかんせん、この問題とはこの1年、何度か中断しつつ付き合ってきたため、最初の頃の記憶があいまいで、説明のしようがありません。
ご理解、ご容赦の程よろしくお願いいたします。

謝辞

最後に、皆様のご助言のおかげで、無事にきれいな点群（と言っても16000点前後/秒程度ですが、これはLidarの限界です）の取得に成功しましたこと、心より感謝いたします。
ずっと一人で作業をしていたら、動いているからと、元のソースを見直すことを頑なに拒んでいたかもしれません。
こちらで、私などよりもはるかに知見のある皆様の意見に、素直に耳を傾けることができ、解決することができました。
本当にありがとうございました。

Comments

[yuki23]

とりあえず、その計測データをどこかにアップロードしてもらえますか？

[fana]

今さらですが念のため…　ファイルに出力した角度の単位って大丈夫ですか？
（参考ソース　と　修正後ソース　とで，そこが違いそうに見えますが）

[yoddy]

>yuki23様
あれこれとテストしてデータを取っておりましたので、整理して準備出来次第アップいたします。

>fana様
何度もご助言ありがとうございます。
修正後のソースが間違っておりましたので、質問本文末尾に再掲いたしました。
ただ、本文にも書いた通り、こちらで変換をかけると、画像が歪んでしまいます。

[ozwk]

念の為c++の方のコードください

[yuki23]

githubにアップロードしてもらったデータですが、ultra_simpleで出力させたままのデータですか？何か加工してませんか？

[yuki23]

あ、出力させるプログラムが自作なんですね。そこが原因の可能性が高いですね

[yoddy]

＞ozwk様、
以下に、元になったSDK同梱のultra_simpleのソースをアップしました。
https://github.com/taobody/RPLidar_A3/tree/master/ultra_simple
また、リポジトリのルートに、私が修正したmain.cppをアップしております。
修正と言っても、出力されるデータを変えただけなので、影響はないと思っておりましたが、問題があればご指摘ください。
修正箇所はソース末尾付近の標準出力の内容です。

[yoddy]

＞yuki23様
ultra_simpleで出力させたままのデータを、test.dat という名前で、リポジトリのルートにアップしました。
もともと質問に記載しております【参考ソース】は、このデータを整形するものでした。
改めて今、何もない部屋でデータを取り、整形したところ、きちんと部屋の形状が出ました。
つまり、私の改変したC++のソースに問題があったということがわかりました。

Answer 1

計測データを見ると、ultra_simpleのオリジナルの方はthetaの範囲が0-360ですが、修正された方は0-512になっています。
出力プログラムの単位計算に問題があると思われます。

Comments

[yoddy]

ありがとうございます。
見落としておりました。
C++のほうを見直して、修正してみます。

[yoddy]

無事に解決いたしました。
質問に解決編として追記しましたが、仰る通り、角度の取り出し方に問題があったようです。
ありがとうございました。

[yuki23]

ご丁寧にどうも。お役に立てて幸いです。

Answer 2

従来通りの水平ではなく、垂直に設置しています。

これのせい，ということは考えられませんか？

コードの問題なのか（まぁ間違うような処理内容でもないとは思いますが）
それとも環境的(実施形態的?)要因なのかを判断するための実験を行ってみてはどうでしょうか？

・まずは普通に水平に設置した状態で
・窓ガラスとか，異様に反射が強い物体，etcとかが測定範囲に無い「きれいな」条件で

やってみても，同様に「曲がった」結果になるのか否か，等．

---

（「質問への追記・修正」にも書いた話ですが）
もしも，角度θの単位がdegreeなのかradianなのかはっきりしていない状態なのであれば，この点は明確に解決しておくべきです．
（まともなSDKのマニュアルなら明記されているんじゃないかな？とか思うのですが）
測定値から判断するならば，例えば，
【装置から見て「概ね45度」である方向（且つ，背景とは明らかに異なる距離となる位置）に何か物を置いて，物を置いた距離に相当する測定値のθの値を見る】
みたいなことを行ってみればよいでしょう．
それでθの値が45付近であれば単位はdegreeで，π/4付近であれば単位はradianであろう，という感じで．
（数パターンの角度でやれば，より確信が持てるか．
複数の結果の差分を見て判断するなら，「θ=0」がどこなのかがわからなくてもいける．）

Comments

[fana]

（屋内用途が考えられている製品なら，最小測定距離みたいなのは厳しくなかろうと想像するので）
Lider装置を床に置いて，大きめのダンボール箱をかぶせてみるとかでやれるなら，外乱光の影響も少なくて良いかも？

[yoddy]

ご教示ありがとうございます。
ご指摘の設置形態の問題についてですが、SDKに同梱されているWindows用の点群可視化プログラムでリアルタイムに確認しても、特に問題は見られませんでした。
（もちろん、厳密にまったく水平設置と同等か、と言われると、おそらくは誤差はあるのでしょうが、今回求めているデータ精度には影響がない程度、という意味合いの「問題は見られない」ということです）

[fana]

垂直設置した場合でも，その「SDKに同梱されているWindows用の点群可視化プログラム」で見た場合にはデータは曲がらないのでしょうか．
もしそうであれば，
(a)データを取得して記録している側のプログラムにバグがある
(b)今回質問に提示されている座標計算のプログラムにバグがある
のどちらかでしょう．
そうなれば，(b)の側にてきとーな角度と距離を食わせて正しいX,Yが出るかどうかをチェックする等して，原因が(a)か(b)かの切り分けを行うと良いかと思います．

[fana]

あと，この回答で言っているのは，
【「水平な状態で」且つ「ご自身の書いたコード(前記(a)と(b))を使った場合」にどうなるのか？】です．
（「SDKに同梱されているWindows用の点群可視化プログラム」　ではなく）

[fana]

姿勢による影響なのかどうかを見るために
・垂直＆自身のコード　→　曲がる
との対比として
・水平＆自身のコード　→　どうなる？
を見る，という話ですね．

上記14:05コメントなら
・水平＆SDK同梱ソフト　→　ＯＫ
にとの対比として
・垂直＆SDK同梱ソフト　→　どうなる？
という話．

[yoddy]

重ねてご回答、ありがとうございます。

＞垂直設置した場合でも，その「SDKに同梱されているWindows用の点群可視化プログラム」で見た場合にはデータは曲がらないのでしょうか．
はい、そちらに関しては曲がっていません。

ひとまず、ご教示いただいたように、答えのわかっている適当な角度と距離を使って検証してみます。（というか、この視点が完全に抜け落ちていました。ありがとうございます。）

＞【「水平な状態で」且つ「ご自身の書いたコード(前記(a)と(b))を使った場合にどうなるのか？】です．
こちらについても検証してみます。

[yoddy]

数々のご助言、ありがとうございました。
今回は、「出力したものを整形するプログラム」ではなく、「そもそもの出力するプログラム（自己改変版）」に問題がありました。
慌てず、いろいろな組み合わせで検証していく、という本来当たり前のことが、焦りの中で出来ていなかったことに思い至らせていただきました。
ありがとうございます。

Answer 3

ソースを見る感じ、極座標を直交座標に変換しているだけで、
特におかしいところも見られないので、計測データの方を疑う必要があると思います

計測データの曲がっている部分を特定して、
計測データがどうなっているのかを確認する必要があると思います

Comments

[yoddy]

お返事遅くなり、申し訳ございません。
ご教示ありがとうございます。
ソースは（と言ってもただの三角関数なので）おかしいところがないとのことで、問題の原因の一つはつぶせて、大変助かります。
計測データがおかしいのでは、とのご指摘と具体的な方策のご提案にも感謝いたします。
ただ」このセンサー用のROSのパッケージ（？）があるのですが、そちらで可視化した限りでは、問題なく部屋の形状が出ますので、謎は深まるばかりなのですが…。