最近因为项目的需要，实现了将C++工程生成dll在unity中调用，但同时发现如果能让unity中值实时的传给c++则就完美了，而不只是把c++计算的结果给unity。
通过验证是可行的。
C++ 生成dll导出关键部分
cpp
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Manuel Stoiber, German Aerospace Center (DLR)
//#include <srt3d/azure_kinect_camera.h>
#include <srt3d/my_camera.h>
#include <srt3d/camera.h>
#include <srt3d/body.h>
#include <srt3d/common.h>
#include <srt3d/normal_viewer.h>
#include <srt3d/occlusion_renderer.h>
#include <srt3d/region_modality.h>
#include <srt3d/renderer_geometry.h>
#include <srt3d/tracker.h>
#include <cmath>
#include <Eigen/Geometry>
#include <filesystem>
#include <memory>
#include <string>
#include <Windows.h>
//Generate body1 template view 2562 of 2562
//global variables
const char* modelName = "";
const std::filesystem::path model_directory{ "D:\\sourcecode\\STR3D\\rbotdataset\\" };
constexpr bool kSaveViewerImage = false;
const std::filesystem::path viewer_save_directory{ "D:\\sourcecode\\STR3D\\SRT3D\\images\\" };
Set up tracker and renderer geometry
auto tracker_ptr{ std::make_shared<srt3d::Tracker>("tracker") };
auto renderer_geometry_ptr{ std::make_shared<srt3d::RendererGeometry>("renderer geometry") };
Set up camera
auto camera_ptr{ std::make_shared < srt3d::MyCamera>("RGB") };
Set up viewers
auto viewer_ptr{ std::make_shared<srt3d::NormalViewer>("viewer", camera_ptr, renderer_geometry_ptr) };
Set up body1 (change accordingly)
//const std::filesystem::path body1_geometry_path{ "D:\\sourcecode\\STR3D\\rbotdataset\\motorpart01.obj" };
const std::filesystem::path body1_geometry_path{ "D:\\sourcecode\\STR3D\\rbotdataset\\duck.obj" };
srt3d::Transform3fA body1_geometry2body_pose{ Eigen::Translation3f(0.0f, 0.0f, 0.0f) };
srt3d::Transform3fA body1_world2body_pose;
srt3d::Transform3fA realtime_myObj2world_pose;
auto body1_ptr{ std::make_shared<srt3d::Body>("body1", body1_geometry_path,
0.001f, true, false, 0.3f,
body1_geometry2body_pose, 1) };
float location[7] = { 0,0, 0, 0,0, 0, 0 };
exe程序
extern "C" __declspec(dllexport)
void main()
{
body1_world2body_pose.matrix() <<
1.0f, 0.0f, 0.0f, 0.095f,
0.0f, 1.0f, 0.0f, 0.1f,
0.0f, 0.0f, 1.0f, -0.4f,
0.0f, 0.0f, 0.0f, 1.0f;
tracker_ptr->AddViewer(viewer_ptr);
if (kSaveViewerImage) viewer_ptr->StartSavingImages(viewer_save_directory);
//body1_ptr->set_body2world_pose(body1_world2body_pose);
body1_ptr->set_world2body_pose(body1_world2body_pose);//原论文code
renderer_geometry_ptr->AddBody(body1_ptr);
// Set up model body 1
//auto body1_model_ptr{ std::make_shared<srt3d::Model>("body1_model", body1_ptr, model_directory, "motorpart01_model.bin") };
auto body1_model_ptr{ std::make_shared<srt3d::Model>("body1_model", body1_ptr, model_directory, "duck.bin") };
// Set up region modality body 1
auto body1_region_modality_ptr{ std::make_shared<srt3d::RegionModality>(
"body1_region_modality", body1_ptr, body1_model_ptr, camera_ptr) };
tracker_ptr->AddRegionModality(body1_region_modality_ptr);
// Set up occlusion renderer
auto occlusion_renderer_ptr{ std::make_shared<srt3d::OcclusionRenderer>(
"occlusion_renderer", renderer_geometry_ptr, camera_ptr) };
body1_region_modality_ptr->UseOcclusionHandling(occlusion_renderer_ptr);
Start tracking
tracker_ptr->SetUpTracker();
//tracker_ptr->StartTracker(false);
//float pos_value[3] = {1., 2.8, 2.33};
//tracker_ptr->StartTracker(false, pos_value);
//tracker_ptr->StartTracker(false, body1_ptr, &a);
//
if (!tracker_ptr->set_up()) {
std::cerr << "Set up tracker " << tracker_ptr->set_name() << " first" << std::endl;
return;
}
tracker_ptr->set_start_tracking(false);
}
//将旋转矩阵转化为四元数
Eigen::Quaterniond Matrix2Quaternion(Eigen::Matrix<float, 4, 4> matrixofBody)
{
Eigen::Matrix<float, 4, 4> matrix_body2world_pose;
//Eigen::Matrix<float, 3, 3> matrix_rotation;
Eigen::Matrix3d matrix_rotation;
//Eigen::Quaterniond matrix_quarternion;
matrix_body2world_pose = matrixofBody;
//matrix_body2world_pose = body1_ptr->body2world_pose().matrix();
//body1_ptr->geometry2world_pose().matrix();
//for (size_t i = 0; i < 4; i++)
//{
// for (size_t j = 0; j < 4; j++)
// {
// std::cout << matrix_body2world_pose(i, j) << "\t";
// }
//}
//取出旋转矩阵
for (size_t i = 0; i < 3; i++)
{
for (size_t j = 0; j < 3; j++)
{
matrix_rotation(i, j) = matrix_body2world_pose(i, j);
}
}
//std::cout << "---------------matrix_quarternion-----------------" << std::endl;
std::cout << matrix_quarternion << std::endl;
//std::cout << matrix_quarternion.x() << "\t" <<matrix_quarternion.y() << "\t"
// << matrix_quarternion.z() << "\t" << matrix_quarternion.z() << "\t" << std::endl;
return Eigen::Quaterniond(matrix_rotation);
}
Eigen::Quaterniond matrix_quarternion;
Eigen::Matrix<float, 4, 4> matrix_body2world_pose;
extern "C" __declspec(dllexport)
int start_track(int iteration, float* pos_array, bool* update, float x, float y, float z, float w, float pos_x, float pos_y, float pos_z)
//int start_track(int iteration, float* pos_array, bool* update, float pos_z)
{
/// <summary>
/// For unity dll test
/// </summary>
/// <param name="pos_array"></param>
/// <returns></returns>
if (tracker_ptr->get_start_tracking())
{
if (!tracker_ptr->StartRegionModalities())
{
return -1;
}
tracker_ptr->set_tracking_started(true);
tracker_ptr->set_start_tracking(false);
//std::cout << "StartRegionModalities" << std::endl;
}
if (tracker_ptr->get_tracking_started())
{
//std::cout << "--------------------------------" << std::endl;
//std::cout << body1_ptr->body2world_pose().matrix() << std::endl;
//当物体追踪后实时更新将我们的旋转和平移传给unity
matrix_body2world_pose = body1_ptr->body2world_pose().matrix();
matrix_quarternion = Matrix2Quaternion(matrix_body2world_pose);
//右手坐标转化为左手坐标 x,z取反
保存表示旋转的四元数和平移的值
float location[7] = { -matrix_quarternion.x(), matrix_quarternion.y(), -matrix_quarternion.z(), matrix_quarternion.w(),
matrix_body2world_pose(0, 3), -matrix_body2world_pose(1, 3),matrix_body2world_pose(2, 3) };
memcpy(pos_array, location, sizeof(float) * 7);
std::cout << "---------------------translation-----------------------------" << std::endl;
std::cout << matrix_body2world_pose(0, 3) << "\t" << matrix_body2world_pose(1, 3) << "\t" << matrix_body2world_pose(2, 3) << std::endl;
//控制unity3d 进程的退出
int update_is[2] = { tracker_ptr->get_update(), 0 };
memcpy(update, update_is, sizeof(int)*2);
if (!tracker_ptr->ExecuteTrackingCycle(iteration))
{
std::cout << "ExecuteTrackingCycle--break" << std::endl;
int update_is[2] = { tracker_ptr->get_update(), 2 };
memcpy(update, update_is, sizeof(int) * 2);
return -1;
}
}
else
{
//传unity物体最初的位置
//程序开始后将初始化的旋转和平移传给unity
if (!tracker_ptr->get_update())
{
matrix_body2world_pose = body1_ptr->body2world_pose().matrix();
matrix_quarternion = Matrix2Quaternion(matrix_body2world_pose);
float location[7] = { -matrix_quarternion.x(), matrix_quarternion.y(), -matrix_quarternion.z(), matrix_quarternion.w(),
matrix_body2world_pose(0, 3), -matrix_body2world_pose(1, 3),matrix_body2world_pose(2, 3) };
memcpy(pos_array, location, sizeof(float) * 7);
}
else
{
int update_is[2] = { tracker_ptr->get_update(), 1 };
memcpy(update, update_is, sizeof(int) * 2); //控制unity3d 进程的退出
接收unity中的物体位姿
Eigen::Quaterniond quat_u3d(w, x, y, z);
Eigen::Matrix3d rotation_matrix = quat_u3d.matrix();
for (size_t i = 0; i < 3; i++)
{
for (size_t j = 0; j < 3; j++)
{
body1_world2body_pose.matrix()(i, j) = rotation_matrix.matrix()(i, j);
}
}
body1_world2body_pose.matrix()(0, 3) = pos_x;
body1_world2body_pose.matrix()(1, 3) = pos_y;
body1_world2body_pose.matrix()(2, 3) = pos_z;
body1_world2body_pose.matrix()(3, 0) = 0;
body1_world2body_pose.matrix()(3, 1) = 0;
body1_world2body_pose.matrix()(3, 2) = 0;
body1_world2body_pose.matrix()(3, 3) = 1;
body1_ptr->set_body2world_pose(body1_world2body_pose);
}
if (!tracker_ptr->ExecuteViewingCycle(iteration))
{
//控制unity3d 进程的退出
int update_is[2] = { tracker_ptr->get_update(), 2 };
memcpy(update, update_is, sizeof(int)*2);
return -1;
}
}
return 0;
}set_body2world_pose实现物体绕自身的旋转
如果用set_world2body_pose则是物体绕世界坐标原点旋转。
cpp
void Body::set_body2world_pose(const Transform3fA &body2world_pose) {
body2world_pose_ = body2world_pose;
world2body_pose_ = body2world_pose_.inverse();
geometry2world_pose_ = body2world_pose_ * geometry2body_pose_;
world2geometry_pose_ = geometry2world_pose_.inverse();
}
void Body::set_world2body_pose(const Transform3fA &world2body_pose) {
world2body_pose_ = world2body_pose;
body2world_pose_ = world2body_pose_.inverse();
geometry2world_pose_ = body2world_pose_ * geometry2body_pose_;
world2geometry_pose_ = geometry2world_pose_.inverse();
}
void Body::set_geometry2body_pose(const Transform3fA &geometry2body_pose) {
geometry2body_pose_ = geometry2body_pose;
geometry2world_pose_ = body2world_pose_ * geometry2body_pose_;
world2geometry_pose_ = geometry2world_pose_.inverse();
}
const Transform3fA &Body::geometry2body_pose() const {
return geometry2body_pose_;
}
const Transform3fA &Body::body2world_pose() const { return body2world_pose_; }
const Transform3fA &Body::world2body_pose() const { return world2body_pose_; }
const Transform3fA &Body::geometry2world_pose() const {
return geometry2world_pose_;
}
const Transform3fA &Body::world2geometry_pose() const {
return world2geometry_pose_;
}unity中调用 dll并将值传给c++工程
cs
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.Runtime.InteropServices;
using System;
using fts;
public class useDll : MonoBehaviour
{
[DllImport("run_on_camera_sequence")]
static extern void main();
[DllImport("run_on_camera_sequence")]
//static extern int start_track(int iteration, float[] pos_array, int[] is_update,float[] pos_rt);
static extern int start_track(int iteration, float[] pos_array, int[] is_update, float x, float y, float z, float w, float pos_x, float pos_y, float pos_z);
// 为接收dll中计算值开辟空间
private float[] pos_array = new float[7];
private int[] is_update = new int[2];
// 申请变量向dll中传值，改变c++程序的运行逻辑
// 物体的position
private float pos_x = 0.095f;
private float pos_y = 0.1f;
private float pos_z = -0.4f;
// 物体的旋转
private float x = 0.0f;
private float y = 0.0f;
private float z = 0.0f;
private float w = 0.0f;
Quaternion rotation_ = new Quaternion(0, 0, 0, 1);
Vector3 trans_ = new Vector3(0, 0, 0);
public GameObject Target;
int stop_run = 0;
bool change_pos = true;
int iteration = 0;
// Start is called before the first frame update
void Start()
{
main();
//FooPluginAPI_Auto.mainFun();
}
// Update is called once per frame
void Update()
{
if (stop_run != -1)
{
//FooPluginAPI_Auto.start_trackFun(iteration, pos_array, is_update);
stop_run = start_track(iteration, pos_array, is_update, x, y, z, w, pos_x, pos_y, pos_z);
print(pos_array[4] + "**********" + pos_array[5] + "**********" + pos_array[6] + "**********");
iteration += 1;
if (is_update[0] != is_update[1] && (is_update[0] * is_update[1]) == 0)
{
rotation_.x = pos_array[0];
rotation_.y = pos_array[1];
rotation_.z = pos_array[2];
rotation_.w = pos_array[3];
Target.transform.localRotation = rotation_;
trans_.x = pos_array[4];
trans_.y = pos_array[5];
trans_.z = pos_array[6];
Target.transform.localPosition = trans_;
print("****localRotation*****" + Target.transform.localRotation + "****localPosition*****" + Target.transform.localPosition);
print("---change---");
}
print("-------[0]------" + is_update[0] + "-------[1]------" + is_update[1]);
if (is_update[0] == is_update[1])
{
x = -Target.transform.localRotation.x;
y = Target.transform.localRotation.y;
z = -Target.transform.localRotation.z;
w = Target.transform.localRotation.w;
pos_x = Target.transform.localPosition.x;
pos_y = -Target.transform.localPosition.y;
pos_z = Target.transform.localPosition.z;
print("***********pos_z************" + pos_z + "******is_update[0]*****" + is_update[0] + "******is_update[1]*****" + is_update[1]);
//pos_z = Target.transform.localPosition.z;
}
}
//if ( is_update[1] == 2)
//{
// //is_update[0] = -1;
// //is_update[1] = 0;
// print("free room");
//}
}
}float[] pos_array, int[] is_update
这两个变量是在unity中使用c++的计算的结果
int iteration,float x, float y, float z, float w, float pos_x, float pos_y, float pos_z
这八个值是从unity中赋值传给c++ dll让其控制程序的逻辑。
应该有更好的方法通过数组一下传过去，但还没搞明白，如果哪位大神整明白了，望分享。