Actualizado sobre la base del anterior 1
La matriz de pose 4*4 del objeto se ha obtenido mediante posicionamiento, y se cambia a un cuaternión y se convierte del sistema de coordenadas de mano derecha de OpenGL al sistema de coordenadas de mano izquierda de Unity.
body2world_pose().matrix() es la matriz de pose 4*4 calculada
ejemplo
cuerpo1_mundo2cuerpo_pose.matriz() <<
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;
------------------------------DLL de C++----------------------------
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 = 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);
}
}
//matrix_rotation(0, 2) *= -1;
//matrix_rotation(1, 2) *= -1;
//matrix_rotation(2, 0) *= -1;
//matrix_rotation(2, 1) *= -1;
//将旋转矩阵转化为四元数
matrix_quarternion = Eigen::Quaterniond(matrix_rotation);
//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;
//右手坐标转化为左手坐标：旋转 x,z取反，平移y取反
保存表示旋转的四元数和平移的值
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);
Otra forma de encontrar cuaterniones
//计算四元数
int sign(float x)
{
return x >= 0 ? 1 : -1;
}
float myMax(float x, float y)
{
return x > y ? x : y;
}
void QuaternionFromMatrix(const cv::Mat& R, float quat[])
{
// Adapted from: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
/*quat[0] = (float)sqrt(myMax(0.0, 1 + R.ptr<float>(0)[0] + R.ptr<float>(1)[1] + R.ptr<float>(2)[2]))/2;
quat[1] = (float)sqrt(myMax(0.0, 1 + R.ptr<float>(0)[0] - R.ptr<float>(1)[1] - R.ptr<float>(2)[2]))/2;
quat[2] = (float)sqrt(myMax(0.0, 1 - R.ptr<float>(0)[0] + R.ptr<float>(1)[1] - R.ptr<float>(2)[2]))/2;
quat[3] = (float)sqrt(myMax(0.0, 1 - R.ptr<float>(0)[0] - R.ptr<float>(1)[1] + R.ptr<float>(2)[2]))/2;
quat[1] *= sign(R.ptr<float>(2)[1] - R.ptr<float>(1)[2]);
quat[2] *= sign(R.ptr<float>(0)[2] - R.ptr<float>(2)[0]);
quat[3] *= sign(R.ptr<float>(1)[0] - R.ptr<float>(0)[1]);*/
//R.convertTo(R, CV_32F);
quat[0] = (float)sqrt(myMax(0.0, 1 + R.at<float>(0, 0) + R.at<float>(1, 1) + R.at<float>(2, 2))) / 2;
quat[1] = (float)sqrt(myMax(0.0, 1 + R.at<float>(0, 0) - R.at<float>(1, 1) - R.at<float>(2, 2))) / 2;
quat[2] = (float)sqrt(myMax(0.0, 1 - R.at<float>(0, 0) + R.at<float>(1, 1) - R.at<float>(2, 2))) / 2;
quat[3] = (float)sqrt(myMax(0.0, 1 - R.at<float>(0, 0) - R.at<float>(1, 1) + R.at<float>(2, 2))) / 2;
quat[1] *= sign(R.at<float>(2, 1) - R.at<float>(1, 2));
quat[2] *= sign(R.at<float>(0, 2) - R.at<float>(2, 0));
quat[3] *= sign(R.at<float>(1, 0) - R.at<float>(0, 1));
//quat[0]为w, quat[1]为x, quat[2]为y, quat[3]为z
}
另一种方法计算四元数
cv::Mat body_r = cv::Mat(3, 3, CV_32F);
cv::Mat matrix_r = cv::Mat(3, 3, CV_32F);
//取出旋转矩阵
for (size_t i = 0; i < 3; i++)
{
for (size_t j = 0; j < 3; j++)
{
matrix_r.at<float>(i,j) = matrix_body2world_pose(i, j);
}
}
float siyuanshu[4] = { 0, 0, 0, 0 };
QuaternionFromMatrix(matrix_r, siyuanshu);
cv::Mat matrix_trans = cv::Mat(3, 1, CV_32F);
matrix_trans.at<float>(0, 0) = matrix_body2world_pose(0, 3);
matrix_trans.at<float>(1, 0) = matrix_body2world_pose(1, 3);
matrix_trans.at<float>(2, 0) = matrix_body2world_pose(2, 3);
//右手坐标转化为左手坐标：旋转 x,z取反，平移y取反
保存表示旋转的四元数和平移的值
float location[7] = { siyuanshu[0], -siyuanshu[1], siyuanshu[2], -siyuanshu[3],
matrix_body2world_pose(0, 3), -matrix_body2world_pose(1, 3), matrix_body2world_pose(2, 3) };
memcpy(pos_array, location, sizeof(float) * 7);------------------------------------------------------unidad---------------------------------
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.Runtime.InteropServices;
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);
private float[] pos_array = new float[7];
private int[] is_update = new int[2];
bool stop_run = false;
public GameObject Target;
int iteration = 0;
// Start is called before the first frame update
void Start()
{
main();
//print(is_update[0]);
}
// Update is called once per frame
void Update()
{
start_track(iteration, pos_array, is_update);
iteration += 1;
// unity Quaternion的赋值顺序是x,y,z,w
//Target.transform.localRotation = new Quaternion( pos_array[1], pos_array[2], pos_array[3], pos_array[0]);
Target.transform.localRotation = new Quaternion(pos_array[0], pos_array[1], pos_array[2], pos_array[3]);
Target.transform.localPosition = new Vector3(pos_array[4], pos_array[5], pos_array[6]);
print("-------[0]------" + is_update[0] + "-------[1]------" + is_update[1]);
}
}
La asignación en Unity se debe asignar primero para rotar y luego asignar traslación para que sea correcta, la secuencia es diferente, porque asignar un valor primero es equivalente a rotar en el origen de las coordenadas, que es lo mismo que el efecto de rotación no en el origen de las coordenadas, pero si primero trasladas y luego rotas, no es lo mismo.
En unidad, primero ajuste la rotación del objeto para que la inmersión opengl sea la misma que la cámara en unidad. Creo que esto es un deber? Por qué, la pose del objeto se da en tiempo real ------------------ pero resulta que esto es correcto.
Luego asigna el objeto en la motopart
