已知两个坐标系下的坐标,求坐标系之间的转换矩阵(一)

本文介绍了一个使用GTEngine库进行4维空间中坐标系旋转的例子,详细展示了如何通过矩阵操作实现从左手坐标系到右手坐标系的转换,并验证了转换的正确性。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

本例子只有旋转,没有平移

#include <iostream>
#include <GTEngine/Mathematics/GteConvertCoordinates.h>
using namespace gte;

// #define Vector4<double> Vector<4, double>
int main(int argc, char const *argv[])
{
// // Affine change of basis.
	ConvertCoordinates<4, double> convert;

	Vector<4, double> X, Y, P0, P1, diff;
	Matrix<4, 4, double> U, V, A, B;
	bool isRHU, isRHV;
	// U.SetCol(0,  Vector<4, double>{1.0, 0.0, 0.0, 0.0});
	// U.SetCol(1,  Vector<4, double>{0.0, 1.0, 0.0, 0.0});
	// U.SetCol(2,  Vector<4, double>{0.0, 0.0, 1.0, 0.0});
	// U.SetCol(3,  Vector<4, double>{0.0, 0.0, 0.0, 1.0});
	
	// V.SetCol(0,  Vector<4, double>{0.866, 0.5, 0.0, 0.0});
	// V.SetCol(1,  Vector<4, double>{-0.5, 0.866, 0.0, 0.0});
	// V.SetCol(2,  Vector<4, double>{0.0, 0.0, 1.0, 0.0});
	// V.SetCol(3,  Vector<4, double>{0.0, 0.0, 0.0, 1.0});

    V.SetCol(0,  Vector<4, double>{1.0, 0.0, 0.0, 0.0});
	V.SetCol(1,  Vector<4, double>{0.0, 1.0, 0.0, 0.0});
	V.SetCol(2,  Vector<4, double>{0.0, 0.0, 1.0, 0.0});
	V.SetCol(3,  Vector<4, double>{0.0, 0.0, 0.0, 1.0});
	
	U.SetCol(0,  Vector<4, double>{0.866, 0.5, 0.0, 0.0});
	U.SetCol(1,  Vector<4, double>{-0.5, 0.866, 0.0, 0.0});
	U.SetCol(2,  Vector<4, double>{0.0, 0.0, 1.0, 0.0});
	U.SetCol(3,  Vector<4, double>{0.0, 0.0, 0.0, 1.0});
	convert(U, true, V, false);

    // Matrix<4, 4, double> matrix; 
    // matrix = convert.GetC();
    
    // for(int i = 0; i <4 ; i++)
    // {
    //      Vector<4,double > row = matrix.GetRow(i);
        
    //     for(size_t j = 0; j < 4; j++)
    //     {
	//         std::cout<< row[i]<<" ";
    //     }
    //     std::cout<<std::endl;
        
    // }
    
	isRHU = convert.IsRightHandedU();  // false
	isRHV = convert.IsRightHandedV();  // true
    // std::cout<<isRHU<<std::endl;
    // std::cout<<isRHV<<std::endl;
	X = { 0.0, 2.0, 0, 1.0 };

	Y = convert.UToV(X);  //
    
    for(int i = 0; i < 4; i++)
    {
     std::cout<<"Y  "<< i<<": " <<Y[i]<<std::endl;
    }
    
	// std::cout<<"Y   "<<Y<<std::endl;
	P0 = U*X;
	P1 = V*Y;
	diff = P0 - P1;  // { 0, 0, 0, 0 }
	return 0;
}

编译:

 g++ gte_test.cpp -I/usr/local/include/GTEngine -std=c++11 -L/home/itfanr/GeometricTools/GTEngine/lib/Release -lgtengine

输出:

itfanr@itfanr-pc:test$ ./a.out
Y  0: -1
Y  1: 1.732
Y  2: 0
Y  3: 1

对比《机器人学导论》中的例子:
在这里插入图片描述

### 计算两个坐标系之间转换矩阵的方法 对于给定的两对来计算坐标变换矩阵,实际上通常需要至少三对不共线的空间以确保能够唯确定刚体运动所对应的变换矩阵。然而,在仅有两对的情况下,可以假设某些条件简化问题。 当有更多对时,可以通过奇异值分解(SVD)方法得更精确的结果[^2]。具体到仅有的两对情况: #### 基本思路 1. **构建基础框架** 需要先基于这两对建立初步的方向信息。设第坐标系中的两分别为 \(P_1\) 和 \(Q_1\) ,第二个坐标系中相应的两为 \(P_2\) 和 \(Q_2\) 。通过这两个矢量 \(\overrightarrow{P_1 Q_1}\) 和 \(\overrightarrow{P_2 Q_2}\),可以获得两者间的主要旋转方向。 2. **定义辅助结构** 使用上述得到的方向向量作为新坐标系的第轴,并据此推导其余两个正交基底形成完整的右手直角坐标系。这步骤涉及到计算叉乘以及可能的标准化操作,从而获得组相互垂直的标准基向量集合 {i', j', k'}[^1]。 3. **创建旋转和平移部分** 构造个 3×3 的旋转矩阵 R,其各列为新的标准基 i'、j' 和 k';同时还需要考虑平移分量 t,即从源坐标系至目标坐标系的位置偏移量。最终形成的齐次变换矩阵 H 将会是个 4×4 的方阵,其中左上角是R,最右侧列表达t,而最后行为 [0, 0, 0, 1]^T。 ```python import numpy as np def compute_transformation_matrix(P1, P2, Q1, Q2): v1 = (Q1 - P1).reshape(-1,) v2 = (Q2 - P2).reshape(-1,) # Normalize vectors to get unit direction vectors u_v1 = v1 / np.linalg.norm(v1) u_v2 = v2 / np.linalg.norm(v2) # Compute rotation axis and angle using Rodrigues formula or SVD method. cross_product = np.cross(u_v1, u_v2) dot_product = np.dot(u_v1, u_v2) # For simplicity here we assume the simplest case where points are not collinear, # otherwise more complex handling would be required. # Constructing an orthogonal basis from given directions if np.allclose(cross_product, 0): raise ValueError('Vectors cannot be parallel.') z_axis = cross_product / np.linalg.norm(cross_product) y_axis = u_v1 x_axis = np.cross(y_axis, z_axis) rot_mat = np.column_stack((x_axis, y_axis, z_axis)) trans_vec = P2.reshape(-1,) - np.matmul(rot_mat, P1.reshape(-1,)) homog_rot = np.vstack([np.hstack([rot_mat, trans_vec[:, None]]), [0, 0, 0, 1]]) return homog_rot ```
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值