用自己的6的imu捕获数据然后测试

[xinzi2018]

我用自己的6个传感器获取了数据，但是预测结果的可视化人物姿态完全不对。
1.初始的姿态我初始化了24/3/3的单位矩阵（T-pose）
2.由于传感器得到的加速度单位是g ,做了乘以9.8的操作；并将传感器获取到的zyx的欧拉角转换成了旋转矩阵
3.在读取六个传感器的时候，有把六个同步、坐标归0、陀螺仪校正的操作
4.我传感器数据的顺序依次是左肘部, 右肘部, 左膝盖, 右膝盖, 头, 腹部（和preprocess取出来的6个部位对应）
不知道具体到底是哪个步骤操作错误？

传感器代码的读取如下：

import openzen
import sys
import time
import numpy as np
import torch
from scipy.spatial.transform import Rotation as R

class GetIMUData(object):
    def __init__(self,device_list=[]):
        openzen.set_log_level(openzen.ZenLogLevel.Warning)
        error, self.client = openzen.make_client()
        if not error == openzen.ZenError.NoError:
            print ("Error while initializing OpenZen library")
            sys.exit(1)
       
        self.device_list = device_list

    def setting(self, device_list):
        sensors = []
        for mac in device_list:
            error, tmp_sensor = self.client.obtain_sensor_by_name("Bluetooth", mac)
            if not error == openzen.ZenSensorInitError.NoError:
                print ("Error connecting to sensor: {}".format(mac))
                sys.exit(1)
            sensors.append(tmp_sensor)
            time.sleep(5)

        print ("All sensors have been connected!")

        imus = []
        for i,sensor in enumerate(sensors):
            tmp_imu = sensor.get_any_component_of_type(openzen.component_type_imu)
            if tmp_imu is None:
                print ("No IMU found in sensor: {}".format(device_list[i]))
                sys.exit(1)
            imus.append(tmp_imu)


        print('开始设置')
        for i in range(len(sensors)):
            imus[i].set_bool_property(openzen.ZenImuProperty.StreamData, False)
            # imus[i].set_int32_property(openzen.ZenImuProperty.Id, int(i)+100)
            # error, imu_id = imus[i].get_int32_property(openzen.ZenImuProperty.Id)
            # print('设置成功imu_id=',imu_id)

        for i in range(len(sensors)):
            imus[i].execute_property(openzen.ZenImuProperty.StartSensorSync)

        time.sleep(9)
        for i in range(len(sensors)):
            imus[i].execute_property(openzen.ZenImuProperty.StopSensorSync)

        for i in range(len(sensors)):
            imus[i].set_bool_property(openzen.ZenImuProperty.StreamData, True)
        print('开始校验')
        for i in range(len(sensors)):
            imus[i].execute_property(openzen.ZenImuProperty.ResetOrientationOffset)
            
          # disable automatic calibration
            imus[i].set_bool_property(openzen.ZenImuProperty.GyrUseAutoCalibration, False)

        for i in range(len(sensors)):
            # start manual calibration
            imus[i].execute_property(openzen.ZenImuProperty.CalibrateGyro)


        print('T-pose姿态保持')
        time.sleep(6)


        return imus,sensors

    def g2ms2(self,data):
        return [x*9.8 for x in data]
         
   def r2matrix(self,data):
       r4 = R.from_euler('zyx', data, degrees=True)
       return r4.as_matrix() # numpy
    

    

    def Cat(self,batchData, imu_data):
        accs = torch.Tensor(self.g2ms2(imu_data.a)).unsqueeze(0)
        rot = torch.Tensor(self.r2matrix(imu_data.r)).unsqueeze(0)
        # rot = self.quaternion_to_rotation_matrix(torch.Tensor(imu_data.q)) 

        batchData['accs'] = torch.cat((batchData['accs'],accs),0)
        batchData['rot'] = torch.cat((batchData['rot'],rot),0)

        return batchData
    def Cat2(self,Datas,batchData):
        accs = batchData['accs'].unsqueeze(0)
        rot = batchData['rot'].unsqueeze(0)


        Datas['accs'] = torch.cat((Datas['accs'],accs),0)
        Datas['rot'] = torch.cat((Datas['rot'],rot),0)
        return Datas
 
    def getData(self,):
        imus,sensors = self.setting(self.device_list)
        batchData={}
        Datas={}
        flag = 0
        times=0
        while times<6000:

            zenEvent = self.client.wait_for_next_event()
            if zenEvent.event_type == openzen.ZenEventType.ImuData:
                for i, imu in enumerate(imus):
                    if zenEvent.component.handle == imu.component.handle:
                        imu_data = zenEvent.data.imu_data
                        print("Sensor{}'s data".format(i))
                        # self.printData(imu_data)

                        if i ==0:# 第一个传感器
                            batchData['accs'] =  torch.Tensor(self.g2ms2(imu_data.a)).unsqueeze(0)
                            batchData['rot'] =  torch.Tensor(self.r2matrix(imu_data.r)).unsqueeze(0)
                      else:
                            batchData = self.Cat(batchData,imu_data)


                        if i ==5:# 表示最后一个传感器
                            if flag==0:# 表示第一帧数据未被记录
                                Datas['accs'] = batchData['accs'].unsqueeze(0)
                                Datas['rot'] = batchData['rot'].unsqueeze(0)
                                flag=1
                            else:  
                                Datas = self.Cat2(Datas,batchData) 

                            print(batchData)                        
                            print('======')
                            print(Datas['accs'].shape)
                            print(Datas['rot'].shape)
                            batchData={}
            times = times+1
 
        Datas['accs'] = [Datas['accs']]
        Datas['rot'] = [Datas['rot']]

        torch.save(Datas, 'captured_data/data_real_Orientation1.pt')
        accs, rots  = torch.load('captured_data/data_real_Orientation1.pt').values()
        print('--------------')
        print(len(accs),accs[0].shape)# 1 torch.Size([2629, 6, 3])
        print(len(rots),rots[0].shape)
 
def main():
    device_list = [
            '00:04:3E:6C:52:CF','00:04:3E:6C:51:E3',
            '00:04:3E:6C:52:D0',
            '00:04:3E:6C:52:FA','00:04:3E:6C:51:F5','00:04:3E:6C:52:16'
        ]
    getImuData = GetIMUData(device_list)
    getImuData.getData()

if __name__ == '__main__':
    main()

[lucas-chung]

请问解决了吗