QXRService：基于高通QXRService获取头显SLAM Pose和IMU Data

本文主要是介绍QXRService：基于高通QXRService获取头显SLAM Pose和IMU Data，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

前述：

QXRService博文：

QVRService：基于SnapdragonXR-SDK 4.0.6进行QVRService的开发

QXRService:高通SnapdragonXR OpenXR SDK v1.x 概略

QXRService：基于高通QXRService获取头显SLAM Pose和IMU Data

QXRService：基于高通QXRService获取SLAM Camera图像

正文：

在上一篇博文的最后提到过，基于高通QXRService已经开发出了能够获取到几乎所有基础数据的工具应用。

今天就开始详细讲解如何基于高通QXRService进行程序开发，这一篇主要讲如何获取高通SLAM Pose和IMU Data。

在之前的博文中已经介绍过，由于高通新的SDK在创建几个关键结构体句柄时，需要传入Java虚拟机内存首地址(JavaVM*)以及运行上下文(Context)，所以对QXRService的开发是JNI层的Native开发，需要具备一些JNI编程的基础知识。

另外，此文的一些具体细节对之前的这一篇博文进行了补充和修正：《QVRService：基于SnapdragonXR-SDK 4.0.6进行QVRService的开发》

例如在上述博文中，曾经提到了JNI中创建QXRService相关结构体时，需要导入github上二次封装的开源jnipp.cpp和jnipp.h后才能正常使用，这一点已经不再需要。

如果还有其他未能提及到的差异点，以新的博客内容为准。

废话不多说，开始了。

一.使用AndroidStudio新建一个空的JNI应用

使用AndroidStudio新建一个名为 qvr-test 空的JNI应用，具体操作就不详细讲了，AndroidStudio的基本操作，网上资料也很多，请自行查阅。

二.添加依赖头文件和资源包到JNI应用中

2.1 添加依赖头文件

前面的博客中提到过，由于高通往后发布的基于OpenXR的Snapdragon XR OpenXR SDK v1.x系列SDK，将qxrservice封装在了Runtime中，而且以后会弱化掉qxrservice对外部接口暴露，所以我们程序中编译所需的qxrservice的相关头文件在新的Snapdragon XR OpenXR SDK v1.x中已不再提供。

所幸的是，这部分头文件我们从旧的SnapdragonXR-SDK 4.0.6中拷出后，仍然可以使用。

将SnapdragonXR-SDK 4.0.6/3rdparty/ 下的qvr和qxr两个子目录中inc文件夹里的头文件都拷贝到新建的qvr-test应用中：

在qvr-test应用的Jni部分代码中，新建一个inc和一个src文件夹

将上述目录中的头文件除个别文件外，其他都拷贝到 jni 目录的 inc 中

2.2 添加依赖so

在之前的博文中也有介绍，针对QXRService的开发，需要加载QXRService的三个基础so，SnapdragonXR-SDK 4.0.6这种老的SDK版本中，这些so可以直接找到，但是在新版Snapdragon XR OpenXR SDK v1.x系列SDK中，需要我们做点不一样的操作：

将openxr_runtime_app-inputService-release.aar的后缀由aar改为zip后解压得到如下：

我们需要的三个基础so就在lib里面：

将这三个so拷贝到qvr-test的jniLibs下面：

2.3 添加依赖jar包

在最终整个qvr-test应用顺利编译完成，并且install到设备上开始运行时，qxrservice client时会调用一个QXRSocketFetcher类，其作用是高通QXRService内部从native与java层进行AIDL进程间通信。

因此在我们创建的qvr-test应用里，还需要再加载一个包含了QXRSocketFetcher以及相关AIDL文件的jar包，如果没有这个jar包，一运行就会crash，报如下异常：

这个jar包在SnapdragonXR-SDK-source.rel.4.0.6的老版本中直接就有，就在 \SnapdragonXR-SDK-source.rel.4.0.6\3rdparty\qxr\libs 目录下，但是，我们现在做的是基于高通Snapdragon XR OpenXR SDK v1.x 系列SDK的QXRService开发，所以即使从老版本中拷贝过来也无法使用。

因为是这个类的依赖路径在新的SDK中，已经从老版本的 /com/qualcomm/qti/qxrsocketfetcher/ 变成了 /com/qualcomm/qti/qxrservice_client/

但是在Snapdragon XR OpenXR SDK v1.x 系列新版SDK中，高通并没有开放这个jar包给用户。

所以，需要找高通提case获取。

找高通要到这个class.jar之后，将其拷贝到libs下面，我们就能看到刚刚引用不到报错的QXRSocketFetcher类以及其他用于进程通信的相关AIDL文件：

三.编写CMakeLists.txt

在创建空的JNI应用后，会在jni目录下生成一个CMakeLists.txt文件，因为我们需要在外部加载so，所以方便起见，把这个CMakeLists.txt挪到app根目录下：

因为我们对QXRService的代码实现在jni代码目录的src下：

Jni的实现代码qxrtest.cpp最终会被编译成so，被java层Load()，Api会被调用，所以CMakeLists.txt也需要对qxrtest.cpp进行编写。

综合我们在前面已经加载了依赖的头文件和so，现在就将它们都写进CMakeLists.txt文件中，代码如下：

# CMakeLists.txt
# For more information about using CMake with Android Studio, read the
# documentation: https://d.android.com/studio/projects/add-native-code.html# Sets the minimum version of CMake required to build the native library.
#CMakeLists.txt
cmake_minimum_required(VERSION 3.4.1)set(jni_base_dir "${CMAKE_SOURCE_DIR}/src/main/jni")
set(jniLibs_base_dir "${CMAKE_SOURCE_DIR}/src/main/jniLibs")include_directories(${jni_base_dir}/inc)# Creates and names a library, sets it as either STATIC
# or SHARED, and provides the relative paths to its source code.
add_library(#设置so文件名称.qxrtest#设置这个so文件为共享.SHARED#Provides a relative path to your source file(s).${jni_base_dir}/src/qxrtest.cpp)# Searches for a specified prebuilt library and stores the path as a
# variable. Because CMake includes system libraries in the search path by
# default, you only need to specify the name of the public NDK library
# you want to add. CMake verifies that the library exists before
# completing its build.
find_library( # Sets the name of the path variable.log-lib#Specifies the name of the NDK library that#you want CMake to locate.log)#动态方式加载 STATIC：表示静态的.a的库 SHARED：表示.so的库。
add_library(qxrcamclient SHARED IMPORTED)
add_library(qxrcoreclient SHARED IMPORTED)
add_library(qxrsplitclient SHARED IMPORTED)#设置要连接的so的相对路径 ${CMAKE_SOURCE_DIR}：表示CMake.txt的当前文件夹路径 
#${ANDROID_ABI}：编译时会自动根据CPU架构去选择相应的库
set_target_properties(qxrcamclient PROPERTIESIMPORTED_LOCATION "${jniLibs_base_dir}/${ANDROID_ABI}/libqxrcamclient.so")
set_target_properties(qxrcoreclient PROPERTIESIMPORTED_LOCATION "${jniLibs_base_dir}/${ANDROID_ABI}/libqxrcoreclient.so")
set_target_properties(qxrsplitclient PROPERTIESIMPORTED_LOCATION "${jniLibs_base_dir}/${ANDROID_ABI}/libqxrsplitclient.so")#添加第三方头文件
target_include_directories(qxrtest PRIVATE ${jni_base_dir}/inc ${jni_base_dir}/src)# Specifies libraries CMake should link to your target library. You
# can link multiple libraries, such as libraries you define in this
# build script, prebuilt third-party libraries, or system libraries.
target_link_libraries( # Specifies the target library.#制定目标库.qxrtest#Links the target library to the log library#included in the NDK.${log-lib}qxrcamclientqxrcoreclientqxrsplitclient)

四.编写Java层代码

在Java层，我们会做一个很简单的界面，其中包含两个Button和Boolean变量，用于对QXRService输出的SLAM Pose和IMU Data获取的Start和Stop控制。

获取到的数据我们会在Jni中就地保存到 /data/data/com.qvr.test/ 目录下，保存成标准的TUM格式文件。

Java代码只有两个文件，一个MainActivity.java，一个JNI.java：

4.1 MainActivity.java代码：

package com.qvr.test;import androidx.appcompat.app.AppCompatActivity;import android.os.Bundle;
import android.os.Handler;
import android.view.View;
import android.widget.Button;public class MainActivity extends AppCompatActivity implements View.OnClickListener {private String TAG = "APP_LOG";private Button btnGetPose;private Button btnGetIMU;private boolean mStartGetPose = false;private boolean mStartGetIMU = false;private JNI mJni = new JNI();private Handler mHandler = new Handler();@Overrideprotected void onCreate(Bundle savedInstanceState) {super.onCreate(savedInstanceState);setContentView(R.layout.activity_main);initView();//在java app加载完成native库后把 context传到native库中。mJni.nativeStoreContext(getApplicationContext());//context传入到native之后，对QXRService进行初始化mJni.nativeInitQxrService();}@Overridepublic void onResume() {super.onResume();}@Overridepublic void onPause() {super.onPause();}@Overridepublic void onDestroy() {super.onDestroy();}public void initView() {btnGetPose = (Button) this.findViewById(R.id.btn_get_pose);btnGetPose.setOnClickListener(this);btnGetIMU = (Button) this.findViewById(R.id.btn_get_imu);btnGetIMU.setOnClickListener(this);}@Overridepublic void onClick(View v) {switch (v.getId()) {case R.id.btn_get_pose: {if (!mStartGetPose) {btnGetPose.setText("Stop-GetPose");mStartGetPose = true;//调用jni api,开始获取QXRService输出的SLAM PosemJni.nativeStartSavePose();} else {btnGetPose.setText("Start-GetPose");mStartGetPose = false;//调用jni api,结束获取QXRService输出的SLAM PosemJni.nativeStopSavePose();}}break;case R.id.btn_get_imu: {if (!mStartGetIMU) {btnGetIMU.setText("Stop-GetIMU");mStartGetIMU = true;//调用jni api,开始获取QXRService输出的IMU datamJni.nativeStartGetIMU();} else {btnGetIMU.setText("Start-GetIMU");mStartGetIMU = false;//调用jni api,结束获取QXRService输出的IMU datamJni.nativeStopGetIMU();}}break;default:break;}}
}

4.2 activity_main.xml代码：

<?xml version="1.0" encoding="utf-8"?>
<androidx.constraintlayout.widget.ConstraintLayout xmlns:android="http://schemas.android.com/apk/res/android"xmlns:app="http://schemas.android.com/apk/res-auto"xmlns:tools="http://schemas.android.com/tools"android:layout_width="match_parent"android:layout_height="match_parent"tools:context=".MainActivity"><Buttonandroid:id="@+id/btn_get_pose"android:layout_width="155dp"android:layout_height="60dp"android:layout_marginLeft="16dp"android:layout_marginTop="52dp"android:text="Start-GetPose"android:textAllCaps="false"app:layout_constraintLeft_toLeftOf="parent"app:layout_constraintTop_toTopOf="parent" /><Buttonandroid:id="@+id/btn_get_imu"android:layout_width="155dp"android:layout_height="60dp"android:layout_marginLeft="16dp"android:layout_marginTop="128dp"android:text="Start-GetIMU"android:textAllCaps="false"app:layout_constraintLeft_toLeftOf="parent"app:layout_constraintTop_toTopOf="parent" /></androidx.constraintlayout.widget.ConstraintLayout>

4.3 JNI.java 代码：

package com.qvr.test;import android.content.Context;import androidx.annotation.NonNull;public class JNI {{System.loadLibrary("qxrtest");}public native void nativeStoreContext(@NonNull Context context);public native void nativeInitQxrService();public native void nativeStartSavePose();public native void nativeStopSavePose();public native void nativeStartGetIMU();public native void nativeStopGetIMU();
}

MainActivity.java 和 JNI.java 两个类中的代码较为简单，其中也已添加注释，稍微有点JNI开发基础知识的童鞋一看就明白，不再做过多介绍。

五.JNI代码编写

Jni部分的文件也只有两个，一个是qxrtest.h，一个是qxrtest.cpp

5.1 qxrtest.h代码

我们将一些要初始化的变量写在.h文件中，另外创建一个结构体保存从Java层传下来的(JavaVM*)指针和Context，代码如下：

/*
* Created by shawn.xiao on 2022/6/20.
*/
#include <jni.h>
#include <string>
#include <unistd.h>
#include <android/log.h>#include <stdlib.h>
#include <stdio.h>
#include <time.h>#include "QXRCamClient.h"
#include "QXRCoreClient.h"
#include "QVRServiceClient.h"#include <pthread.h>
#include <fstream>
#include <iostream>
#include <typeinfo>
#include <string>
#include <cmath>#define LOG_TAG "QVR-Test"
#define LOGW(...) __android_log_print( ANDROID_LOG_WARN, LOG_TAG, __VA_ARGS__ )#ifdef __cplusplus
extern "C" {//用于保存Java层传下来的Java虚拟机内存首地址和运行上下文
static struct {struct _JavaVM *vm;jobject context;
} jni_android_info;//QXRServiceClient handler
static qvrservice_client_helper_t *qvrservice_client = NULL;//用于保存QXRService SLAM输出Pose
static qvrservice_head_tracking_data_t *head_tracking_data = NULL;//用于保存QXRService IMU输出数据
static qvrservice_sensor_data_raw_t *sensor_raw_data = NULL;//用于控制抓取QXRService SLAM Pose抓取线程
bool mIsStopSavePose = false;
//用于控制抓取QXRService IMU Data抓取线程
bool mIsStopGetIMU = false;}
#endif

5.2 qxrtest.cpp代码：

在cpp代码中主要做如下几件事：
1.使用JavaVM*和Context创建QXRService基础结构体实例，设置VR模式:
  1.1 创建qvrservice_client
  1.2 设置VR Mode为6DOF
  1.3 StartVRMode
2.创建两个线程savePoseThread()和saveImuThread()用于从QXRService获取数据
3.保存数据到文件

在贴代码之前我们先对高通QXRService输出的Pose和IMU数据做个基本的了解。

高通CreatePoint网站上有一篇"80-PV306-1-SXR2130 XR Platform API Reference.pdf"文档(文档可能已不是最新，前缀有可能不同，搜索"SXR2130 XR Platform API Reference"关键字即可)，这篇文档中有QXRService几乎所有API、参数、变量、结构体的详细注解。

其中用于获取Pose和IMU数据的结构体分别是：
struct qvrservice_head_tracking_data_t
struct qvrservice_sensor_data_raw_t

文档中这两个结构体及其每个成员都有详细的注解，其中包含各种位姿数据，状态，标志位等，Pose数据结构体中甚至还包含了3DOF模式下的相关数据，IMU数据结构体中也包含了磁力计等相关数据。

由于文档有高通Logo水印，我就不在这截图显示了，有需要的同学自行下载查看即可。
或者直接在QVRTypes.h等相关头文件中的看代码定义也一样。

在qvrtest.cpp中对Pose和IMU数据进行文件存储时，只选取了部分关键成员数据
Pose：{时间戳，Position，四元数}
IMU：{时间戳，Gyroscope(陀螺仪)，Accelerometer(加速度计)}

代码如下：

/*
/* Created by shawn1.xiao on 2022/6/20.
*/
#include "qxrtest.h"using namespace std;#ifdef __cplusplus
extern "C" {string txt = ".txt";
string rootPath = "/data/data/com.qvr.test/";JNIEXPORT void JNICALL
Java_com_qvr_test_JNI_nativeStoreContext(JNIEnv *env, jobject thiz, jobject context) {JavaVM *jvm = nullptr;jint result = env->GetJavaVM(&jvm);assert(result == JNI_OK);assert(jvm);jobject jContext = env->NewGlobalRef(context);//保存JavaVM*、Contextjni_android_info.vm = jvm;jni_android_info.context = jContext;
}//Init QVRService Start
JNIEXPORT void JNICALL
Java_com_qvr_test_JNI_nativeInitQxrService(JNIEnv *env, jobject thiz) {int ret = QVR_ERROR;//*********************** QVRServiceClient Init**************************qvrservice_client = QXRCoreClient_Create(jni_android_info.vm, jni_android_info.context);if (qvrservice_client == NULL) {LOGW("Fail to create qvrservice_client!");} else {LOGW("Success to create qvrservice_client!");}//设置TrackingMode为6DOFret = QVRServiceClient_SetTrackingMode(qvrservice_client, TRACKING_MODE_POSITIONAL);if (ret != QVR_SUCCESS) {LOGW("set tracking mode 6dof failed!  ret:%d", ret);}sleep(1);//获取当前VRMode，必须要是VRMODE_STOPPED状态才能StartQVRSERVICE_VRMODE_STATE vrmode = QVRServiceClient_GetVRMode(qvrservice_client);LOGW("get vr mode:%d", vrmode);if (VRMODE_STOPPED == vrmode) {//当前VRMode为VRMODE_STOPPED状态下，Start VRModeret = QVRServiceClient_StartVRMode(qvrservice_client);LOGW("start vr mode ret:%d, vrmode:%d", ret, QVRServiceClient_GetVRMode(qvrservice_client));if (ret != QVR_SUCCESS) {LOGW("start vr mode failed");}}
}
//Init QVRService End//获取当前系统时间，按format进行转换
string getDateTime() {  //24H data formatstruct tm tm;time_t ts = time(0);localtime_r(&ts, &tm);char buff[128];strftime(buff, sizeof(buff), "%Y-%m%d-%H%M-%S", &tm);string time = buff;return time;
}//GetPose Start
//抓取QXRService SLAM Pose数据线程
void *savePoseThread(void *arg) {int ret = QVR_ERROR;mIsStopSavePose = false;//用于保存每条Posestring trajContent;//文件全路径为：根目录路径+"traj-"+当前时间+".txt"string trajPath = rootPath + "traj-" + getDateTime() + txt;ofstream os_traj;                   //创建文件输出流对象os_traj.open(trajPath, ios::app);   //将对象与文件关联//while循环获取，当mIsStopSavePose为true时，跳出循环while (!mIsStopSavePose) {//通过创建的qvrservice_client获取Pose数据ret = QVRServiceClient_GetHeadTrackingData(qvrservice_client, &head_tracking_data);if (ret == QVR_SUCCESS && head_tracking_data != NULL) {//每一条Pose包含：{时间戳，Position，四元素}//四元素可以自行转换欧拉角，相关函数已实现，此处不作说明trajContent = to_string(head_tracking_data->ts)+ " " + to_string(head_tracking_data->translation[0])+ " " + to_string(head_tracking_data->translation[1])+ " " + to_string(head_tracking_data->translation[2])+ " " + to_string(head_tracking_data->rotation[0])+ " " + to_string(head_tracking_data->rotation[1])+ " " + to_string(head_tracking_data->rotation[2])+ " " + to_string(head_tracking_data->rotation[3])+ "\n";os_traj << trajContent;}usleep(10000);}sleep(1);os_traj.close();pthread_exit(NULL);return NULL;
}JNIEXPORT void JNICALL
Java_com_qvr_test_JNI_nativeStartSavePose(JNIEnv *env, jobject obj) {//创建抓取Pose数据线程pthread_t myThread;int res = pthread_create(&myThread, NULL, savePoseThread, NULL);if (res != 0) {LOGW("savePoseThread create failed!");return;}
}JNIEXPORT void JNICALL
Java_com_qvr_test_JNI_nativeStopSavePose(JNIEnv *env, jobject obj) {LOGW("nativeStopSavePose()    mIsStopSavePose:%d", mIsStopSavePose);//由Java调用，Stop的时候，线程停止运行mIsStopSavePose = true;
}
//GetPose End//GetImu Start
//流程与PoseThread类似，仅差异部分作注解，其他部分请看看代码就明白了
void *saveImuThread(void *arg) {int ret = QVR_ERROR;mIsStopGetIMU = false;//IMU 陀螺仪和加速度计数据string gyroContent, acceContent;string gyroPath = rootPath + "Gyro-" + getDateTime() + txt;string accePath = rootPath + "Acce-" + getDateTime() + txt;ofstream os_gyro, os_acce;os_gyro.open(gyroPath, ios::app);os_acce.open(accePath, ios::app);while (!mIsStopGetIMU) {ret = QVRServiceClient_GetSensorRawData(qvrservice_client, &sensor_raw_data);if (ret == QVR_SUCCESS && sensor_raw_data != NULL) {LOGW("GetIMU Gyroscope     :{%lu, %f, %f, %f}", sensor_raw_data->gts,sensor_raw_data->gx,sensor_raw_data->gy,sensor_raw_data->gz);LOGW("GetIMU Accelerometer :{%lu, %f, %f, %f}", sensor_raw_data->ats,sensor_raw_data->ax,sensor_raw_data->ay,sensor_raw_data->az);gyroContent = to_string(sensor_raw_data->gts)+ " " + to_string(sensor_raw_data->gx)+ " " + to_string(sensor_raw_data->gy)+ " " + to_string(sensor_raw_data->gz)+ "\n";os_gyro << gyroContent;acceContent = to_string(sensor_raw_data->ats)+ " " + to_string(sensor_raw_data->ax)+ " " + to_string(sensor_raw_data->ay)+ " " + to_string(sensor_raw_data->az)+ "\n";os_acce << acceContent;}usleep(10000);}sleep(1);os_gyro.close();os_acce.close();pthread_exit(NULL);return NULL;
}JNIEXPORT void JNICALL
Java_com_qvr_test_JNI_nativeStartGetIMU(JNIEnv *env, jobject obj) {pthread_t myThread;int res = pthread_create(&myThread, NULL, saveImuThread, NULL);if (res != 0) {LOGW("saveImuThread create failed!");return;}
}JNIEXPORT void JNICALL
Java_com_qvr_test_JNI_nativeStopGetIMU(JNIEnv *env, jobject obj) {mIsStopGetIMU = true;
}
//GetImu End}
#endif

到此，基于高通QXRService获取头显SLAM Pose和IMU Data的Demo代码开发工作就已完成，在我们对代码进行编译、安装后，再做个简单的测试，看是否能得到我们想要的结果。