# 大角度非迭代的空间坐标旋转C#实现

• 布尔沙模型
• 莫洛琴斯基模型
• 范式模型

1. 仅适用于满足近似处理的小角度转换
2. 设计复杂的三角函数运算
3. 需要迭代计算

1. 罗德里格矩阵坐标转换原理

## 2.1 坐标转换基本矩阵

[\left[\begin{array}{l} X \ Y \ Z \end{array}\right]=\lambda R\left[\begin{array}{l} X^{\prime} \ Y^{\prime} \ Z^{\prime} \end{array}\right]+\left[\begin{array}{l} \Delta X \ \Delta Y \ \Delta Z \end{array}\right] \tag{1} ]

[\begin{aligned} R & =R\left(\varepsilon_Y\right) R\left(\varepsilon_X\right) R\left(\varepsilon_Z\right) \ & =\left[\begin{array}{ccc} \cos \varepsilon_Y \cos \varepsilon_Z-\sin \varepsilon_Y \sin \varepsilon_X \sin \varepsilon_Z & -\cos \varepsilon_Y \sin \varepsilon_Z-\sin \varepsilon_Y \sin \varepsilon_X \cos \varepsilon_Z & -\sin \varepsilon_Y \cos \varepsilon_X \ \cos \varepsilon_X \sin \varepsilon_Z & \cos \varepsilon_X \cos \varepsilon_Z & -\sin \varepsilon_X \ \sin \varepsilon_Y \cos \varepsilon_Z+\cos \varepsilon_Y \sin \varepsilon_X \sin \varepsilon_Z & -\sin \varepsilon_Y \sin \varepsilon_Z+\cos \varepsilon_Y \sin \varepsilon_X \cos \varepsilon_Z & \cos \varepsilon_Y \cos \varepsilon_X \end{array}\right] \end{aligned} ]

## 2.2 计算技巧-重心矩阵

[\left{\begin{array}{l} X_k=\frac{\sum_{i=1}^n X_i}{n}, Y_k=\frac{\sum_{i=1}^n Y_i}{n}, Z_k=\frac{\sum_{i=1}^n Z_i}{n} \ X_k^{\prime}=\frac{\sum_{i=1}^n X_i^{\prime}}{n}, Y_k^{\prime}=\frac{\sum_{i=1}^n Y_i^{\prime}}{n}, Z_k^{\prime}=\frac{\sum_{i=1}^n Z_i^{\prime}}{n} \ \bar{X}_i=X_i-X_k, \bar{Y}_i=Y_i-Y_k, \bar{Z}_i=Z_i-Z_k \ \bar{X}_i^{\prime}=X_i^{\prime}-X_k^{\prime}, \bar{Y}_i^{\prime}=Y_i^{\prime}-Y_k^{\prime}, \bar{Z}_i^{\prime}=Z_i^{\prime}-Z_k^{\prime} \end{array}\right. ]

[\left[\begin{array}{l} \bar{X} \ \bar{Y} \ \bar{Z} \end{array}\right]=\lambda R\left[\begin{array}{l} \bar{X}^{\prime} \ \bar{Y}^{\prime} \ \bar{Z}^{\prime} \end{array}\right] \tag{2} ]

[\left[\begin{array}{l} \Delta X \ \Delta Y \ \Delta Z \end{array}\right]=\left[\begin{array}{l} X_g \ Y_g \ Z_g \end{array}\right]-\lambda R\left[\begin{array}{l} X_g^{\prime} \ Y_g^{\prime} \ Z_g^{\prime} \end{array}\right] \tag{3} ]

## 2.3 基于罗德里格斯矩阵的转换方法

[\Vert [\bar{X}, \bar{Y}, \bar{Z}]^T \Vert = \lambda \Vert [\bar{X’}, \bar{Y’}, \bar{Z’}]^T \Vert \tag{4} ]

[\lambda=\frac{\sum_{i=1}^n\left(\left\|\left[\bar{X}i \bar{Y}_i \bar{Z}_i\right]^{\mathrm{T}}\right\| \cdot\left\|\left[\bar{X}_i^{\prime} \bar{Y}_i^{\prime} \bar{Z}_i^{\prime}\right]^{\mathrm{T}}\right\|\right)}{\sum_i^n\left(\left\|\left[\bar{X}{\prime}^{\prime} \bar{Y}_i^{\prime} \bar{Z}_i^{\prime}\right]^{\mathrm{T}}\right\|\right)^2} ]

[R=(I-S)^{-1} (I+S) \tag{5} ]

[\left[\begin{array}{c} \bar{X}-\lambda \bar{X}^{\prime} \ \bar{Y}-\lambda \bar{Y}^{\prime} \ \bar{Z}-\lambda \bar{Z}^{\prime} \end{array}\right]=\left[\begin{array}{ccc} 0 & -\left(\bar{Z}+\lambda \bar{Z}^{\prime}\right) & -\left(\bar{Y}+\lambda \bar{Y}^{\prime}\right) \ -\left(\bar{Z}+\lambda \bar{Z}^{\prime}\right) & 0 & \bar{X}+\lambda \bar{X}^{\prime} \ \bar{Y}+\lambda \bar{Y}^{\prime} & \bar{X}+\lambda \bar{X}^{\prime} & 0 \end{array}\right]\left[\begin{array}{l} a \ b \ c \end{array}\right] \tag{6} ]

1. C#代码实现

## 3.1 计算矩阵重心坐标

Vector BarycentricCoord(Matrix coordinate)
{
Vector barycentric = vb.Dense(3, 1);

int lenCoord = coordinate.ColumnCount;

if (lenCoord > 2)
barycentric = coordinate.RowSums();

barycentric /= lenCoord;

return barycentric;
}



## 3.2 计算比例因子

double ScaleFactor(Matrix sourceCoord, Matrix targetCoord)
{
double k = 0;

double s1 = 0;
double s2 = 0;

Vector sourceColL2Norm = sourceCoord.PointwisePower(2.0).ColumnSums();

Vector targetColL2Norm = targetCoord.PointwisePower(2.0).ColumnSums();

int lenSourceCoord = sourceCoord.ColumnCount;

int lenTargetCoord = targetCoord.ColumnCount;

//只有在目标矩阵和源矩阵大小一致时，才能计算
if (lenSourceCoord == lenTargetCoord)
{
s1 = sourceColL2Norm.PointwiseSqrt().PointwiseMultiply(targetColL2Norm.PointwiseSqrt()).Sum();

s2 = sourceColL2Norm.Sum();
}

k = s1 / s2;
return k;
}



## 3.3 计算罗德里格参数

Vector RoderickParas(double scalceFactor, Matrix sourceCoord, Matrix targetCoord)
{
Vector roderick = vb.Dense(new double[] { 0, 0, 0 });

int lenData = sourceCoord.ColumnCount;

//常系数矩阵
var lConstant = vb.Dense(new double[3 * lenData]);

//系数矩阵
var coefficient = mb.DenseOfArray(new double[3 * lenData, 3]);

//构造相应矩阵
for (int i = 0; i < lenData; i++)
{
lConstant[3 * i] = targetCoord[0, i] - scalceFactor * sourceCoord[0, i];
lConstant[3 * i + 1] = targetCoord[1, i] - scalceFactor * sourceCoord[1, i];
lConstant[3 * i + 2] = targetCoord[2, i] - scalceFactor * sourceCoord[2, i];

coefficient[3 * i, 0] = 0;
coefficient[3 * i, 1] = -(targetCoord[2, i] + scalceFactor * sourceCoord[2, i]);
coefficient[3 * i, 2] = -(targetCoord[1, i] + scalceFactor * sourceCoord[1, i]);
coefficient[3 * i + 1, 0] = -(targetCoord[2, i] + scalceFactor * sourceCoord[2, i]);
coefficient[3 * i + 1, 1] = 0;
coefficient[3 * i + 1, 2] = targetCoord[0, i] + scalceFactor * sourceCoord[0, i];
coefficient[3 * i + 2, 0] = targetCoord[1, i] + scalceFactor * sourceCoord[1, i];
coefficient[3 * i + 2, 1] = targetCoord[0, i] + scalceFactor * sourceCoord[0, i];
coefficient[3 * i + 2, 2] = 0;

}

roderick = coefficient.TransposeThisAndMultiply(coefficient).Inverse() * coefficient.Transpose() * lConstant;

return roderick;
}



## 3.4 解析罗德里格矩阵

///
/// 解析罗德里格矩阵为旋转矩阵和平移矩阵
///
/// 比例因子
/// 罗德里格矩阵
/// 原坐标系坐标
/// 目标坐标系坐标
///
(Matrix, Vector) RotationMatrix(double scaleFactor, Vector roderick, Vector coreSourceCoord, Vector coreTargetCoord)
{
Matrix rotation = mb.DenseOfArray(new double[,]
{
{0,0,0 },
{0,0,0 },
{0,0,0 }
});

//反对称矩阵
Matrix antisymmetric = mb.DenseOfArray(new double[,]
{
{          0, -roderick[2], -roderick[1] },
{roderick[2],            0, -roderick[0] },
{roderick[1],  roderick[0],            0 }
});

// 创建单位矩阵
// 然后与式(5)的 S 执行 + 和 - 操作
rotation = (DenseMatrix.CreateIdentity(3) - antisymmetric).Inverse() * (DenseMatrix.CreateIdentity(3) + antisymmetric);

translation = coreTargetCoord - scaleFactor * rotation * coreSourceCoord;

return (rotation, translation);
}



## 3.5 调用逻辑

// 1. 字段值准备
MatrixBuilder mb = Matrix.Build;
VectorBuilder vb = Vector.Build;

// 2. 写入源坐标系的坐标。注意这里的x,y,z输入顺序
Matrix source = mb.DenseOfArray(new double[,]
{
{-17.968, -12.829, 11.058 },
{-0.019 , 7.117,   11.001 },
{0.019  , -7.117,  10.981 }
}).Transpose();

// 3. 写入目标坐标系的坐标
Matrix target = mb.DenseOfArray(new double[,]
{
{ 3392088.646,504140.985,17.958 },
{ 3392089.517,504167.820,17.775 },
{ 3392098.729,504156.945,17.751 }
}).Transpose();

// 4. 重心化
var coreSource = BarycentricCoord(source);
var coreTarget = BarycentricCoord(target);

var sourceCoords = source - mb.DenseOfColumnVectors(coreSource, coreSource, coreSource);
var targetCoords = target - mb.DenseOfColumnVectors(coreTarget, coreTarget, coreTarget);

// 5. 求比例因子
double k = ScaleFactor(sourceCoords, targetCoords);

// 6. 解算咯德里格参数
var roderick = RoderickParas(k, sourceCoords, targetCoords);

// 7. 旋转
(Matrix ro, Vector tran) = RotationMatrix(k, roderick, coreSource, coreTarget);

Console.WriteLine("比例因子为：");
Console.WriteLine(k);

Console.WriteLine("旋转矩阵为：");
Console.WriteLine(ro.ToString());

Console.WriteLine("平移参数为：");
Console.WriteLine(tran.ToString());

Console.WriteLine("计算结果为：");
Console.WriteLine(source2.ToString());


1. 总结

Original: https://www.cnblogs.com/AidanLee/p/16988300.html
Author: Aidan_Lee
Title: 大角度非迭代的空间坐标旋转C#实现

(0)

### 大家都在看

• #### conda环境创建及复制

一、 conda环境创建与激活 conda create -n name python=3.8conda activate name 其中的name为创建环境的名字，python=…

Python 2023年9月8日
041
• #### Python 函数递归教程

注入产生的原理: 数据库设置为GBK编码: 宽字节注入源于程序员设置MySQL连接时错误配置为:set character_set_client=gbk,这样配置会引发编码转换从而…

Python 2023年2月2日
068
• #### （编程语言界的丐帮 C#）.NET MD5 HASH 哈希 加密 与JAVA 互通

注入产生的原理: 数据库设置为GBK编码: 宽字节注入源于程序员设置MySQL连接时错误配置为:set character_set_client=gbk,这样配置会引发编码转换从而…

Python 2023年1月31日
0103
• #### Pandas简明教程（一）：Series数据类型、DataFrame数据类型

1.1 简介 Pandas是Python生态中非常重要的数据分析包，它是一个开源的库，采用BSD开源协议。 Pandas是基于 NumPy构建的数据分析包，但它含有比 ndarra…

Python 2023年8月16日
043
• #### conda创建虚拟环境，jupyter更换内核

注入产生的原理: 数据库设置为GBK编码: 宽字节注入源于程序员设置MySQL连接时错误配置为:set character_set_client=gbk,这样配置会引发编码转换从而…

Python 2023年1月16日
088
• #### 【django学习】——Django介绍和实战（开发简易版博客网页）

注入产生的原理: 数据库设置为GBK编码: 宽字节注入源于程序员设置MySQL连接时错误配置为:set character_set_client=gbk,这样配置会引发编码转换从而…

Python 2022年12月27日
0121
• #### Python数据可视化工具matpoltlib使用

注入产生的原理: 数据库设置为GBK编码: 宽字节注入源于程序员设置MySQL连接时错误配置为:set character_set_client=gbk,这样配置会引发编码转换从而…

Python 2023年1月14日
076
• #### MySQL的索引与事务

注入产生的原理: 数据库设置为GBK编码: 宽字节注入源于程序员设置MySQL连接时错误配置为:set character_set_client=gbk,这样配置会引发编码转换从而…

Python 2023年1月19日
077
• #### Pytest+selenium+allure+Jenkins自动化测试框架搭建及使用

注入产生的原理: 数据库设置为GBK编码: 宽字节注入源于程序员设置MySQL连接时错误配置为:set character_set_client=gbk,这样配置会引发编码转换从而…

Python 2023年1月17日
093
• #### 陈都灵现身海南国际电影节，新片《关索岭》票房有望超《阿凡达》

注入产生的原理: 数据库设置为GBK编码: 宽字节注入源于程序员设置MySQL连接时错误配置为:set character_set_client=gbk,这样配置会引发编码转换从而…

Python 2023年1月23日
084
• #### 【手写 Vue2.x 源码】第三十一篇 – diff算法-比对优化（下）

一，前言 上篇，diff 算法-比对优化（上），主要涉及以下几个点： 介绍了如何对儿子节点进行比对； 新老儿子节点可能存在的 3 种情况及代码实现； 新老节点都有儿子时，diff …

Python 2023年11月7日
012
• #### 【pyspark】DataFrame基础操作（二）

介绍一下 pyspark 的 DataFrame 基础操作。 一、选择和访问数据 PySpark DataFrame 是惰性计算的，简单地选择一列不会触发计算，但它会返回一个 Co…

Python 2023年8月8日
044
• #### matplotlib学习笔记（1）—安装

matplotlib学习笔记（1） matplotlip安装前的准备 matplotlib是python里面的一个画图的包，想要使用matplotlib的话，需要先安装python…

Python 2023年9月1日
050
• #### python 可视化分析北京2008-2016年的PM2.5数据

注入产生的原理: 数据库设置为GBK编码: 宽字节注入源于程序员设置MySQL连接时错误配置为:set character_set_client=gbk,这样配置会引发编码转换从而…

Python 2023年1月15日
0103
• #### python代码~创意圣诞树

注入产生的原理: 数据库设置为GBK编码: 宽字节注入源于程序员设置MySQL连接时错误配置为:set character_set_client=gbk,这样配置会引发编码转换从而…

Python 2023年1月24日
074
• #### Python条件语句的用法

注入产生的原理: 数据库设置为GBK编码: 宽字节注入源于程序员设置MySQL连接时错误配置为:set character_set_client=gbk,这样配置会引发编码转换从而…

Python 2023年2月1日
083