Если матрица 4x4 - это то, что я думаю (однородная матрица преобразования твердого тела, такая же, как элемент SE (3)), тогда будет довольно легко получить то, что вы хотите. Любое преобразование твердого тела может быть представлено матрицей 4x4 вида
g_ab = [ R, p;
0, 1]
в обозначении блочной матрицы. Индекс ab означает, что преобразование будет принимать координаты точки, представленной в кадре b, и сообщит вам, какие координаты представлены в кадре a. R - это матрица вращения 3x3, а p - это вектор, который, когда матрица вращения равна единице (без вращения), сообщает вам координаты начала b в кадре a. Обычно, однако, вращение присутствует, поэтому вы должны сделать, как показано ниже.
Положение системы координат, описываемой матрицей, будет дано путем применения преобразования к точке (0,0,0). Это поможет вам понять, в каком мире находятся координаты точки. Хитрость заключается в том, что, имея дело с SE (3), вы должны добавить 1 в конце точек и 0 в конце векторов, что делает их векторами длины 4 вместо длины 3, и, следовательно, может использоваться матрица! Таким образом, чтобы преобразовать точку (0,0,0) в вашей локальной системе координат в мировую систему, вы бы умножили матрицу (назовем ее g_SA) на вектор (0,0,0,1). Чтобы получить мировые координаты вектора (x, y, z), вы умножаете матрицу на (x, y, z, 0). Вы можете думать об этом, потому что векторы - это различия точек, поэтому 1 в последнем элементе исчезает. Так, например, чтобы найти представление вашей локальной оси x в мировых координатах, вы умножаете g_SA * (1,0,0,0). Чтобы найти ось Y, вы делаете g_SA * (0,1,0,0) и т. Д.
Лучшее место, где я видел это обсуждаемое (и откуда я это узнал), это Математическое введение в роботизированную манипуляцию Мюррея, Ли и Шастри , и интересующая вас глава - 2.3.1 .