如何从日志文件中获取姿势和 4x4 矩阵并将它们保存为 JSON 文件？

Question

我有一个包含姿势和 4x4 矩阵的大型日志文件，我想将每个矩阵拆分为 3x3 旋转矩阵和 3x1 平移矩阵，每个矩阵由pose_number指定（每个矩阵顶部的第一个数字是pose_number）和将它们保存到类似这种格式的 JSON 文件中 ( {"pose_0": {"rotation": [[1.00000000 , 0.00000000 , 0.00000000 ], [-0.00000000 , 1.00000000 , -0.00000000 ], [ 0.00000000 , -0.0000 0 , -0.00000] 0翻译": [ 0.00000000 ,0.00000000 , 0.00000000 ]}, "pose_1": {"旋转": ......) ) ,.我也在图片中展示了所有这些1。 因为我是这个领域的新手，我不知道该怎么做，如果有人能帮助我或给我一些类似的例子，我将不胜感激。谢谢These is my log sample 这是日志文件的示例：

0 0 1
1.00000000 0.00000000 0.00000000 0.00000000
-0.00000000 1.00000000 -0.00000000 0.00000000
0.00000000 -0.00000000 1.00000000 0.00000000
0.00000000 0.00000000 0.00000000 1.00000000
1 1 2
0.99987256 -0.00294761 -0.01569025 -0.00016514
0.00261951 0.99977833 -0.02089080 0.00523247
0.01574835 0.02084704 0.99965864 -0.00371881
0.00000000 0.00000000 0.00000000 1.00000000
2 2 3
0.99973679 -0.00740811 -0.02171352 0.00091496
0.00681182 0.99960112 -0.02740798 0.01052933
0.02190790 0.02725285 0.99938848 -0.00518150
0.00000000 0.00000000 0.00000000 1.00000000

第一个矩阵的输出应该是这样的：

{"pose_0": {"rotation": [[1.00000000 , 0.00000000 , 0.00000000 ], [-0.00000000 , 1.00000000  , -0.00000000 ], [ 0.00000000 , -0.00000000 , 1.00000000 ]], "translation": [ 0.00000000 ,0.00000000  , 0.00000000 ]},

Answer 1

由于很容易确​​定每组数据的开始（3 个整数），您可以使用itertools.groupby 为您分解它。每次文件从 3 事物更改为非 3 事物时，这都会为您提供一个子迭代器。当它的 3 件事时，拆分姿势，否则让 numpy 从其余部分构建一个数组。

import itertools
import io
import numpy as np

test = io.StringIO("""0 0 1
1.00000000 0.00000000 0.00000000 0.00000000
-0.00000000 1.00000000 -0.00000000 0.00000000
0.00000000 -0.00000000 1.00000000 0.00000000
0.00000000 0.00000000 0.00000000 1.00000000
1 1 2
0.99987256 -0.00294761 -0.01569025 -0.00016514
0.00261951 0.99977833 -0.02089080 0.00523247
0.01574835 0.02084704 0.99965864 -0.00371881
0.00000000 0.00000000 0.00000000 1.00000000
2 2 3
0.99973679 -0.00740811 -0.02171352 0.00091496
0.00681182 0.99960112 -0.02740798 0.01052933
0.02190790 0.02725285 0.99938848 -0.00518150
0.00000000 0.00000000 0.00000000 1.00000000""")


def get_pose_and_matrix_from_log(fileobj):
    """yield pose, np.array for each group in log file"""
    pose = None
    # each group starts with a 3 integer sequence
    for is_pose_number, data in itertools.groupby(test, 
            lambda line: len(line.strip().split()) == 3):
        if is_pose_number:
            if pose is None:
                raise ValueError("Invalid file format")
            pose = next(data).split(" ", 1)[0]
        else:
            matrix = np.genfromtxt(data)
            yield (pose, matrix)

for pose, matrix in get_pose_and_matrix_from_log(test):
    pass # transform and make json

要从文件中读取，请将循环更改为

with open("\\path\\trajectory.log") as log_file:
    for pose, matrix in get_post_and_matrix_from_log(log_file):
        pass # ... xform