SLAM : 旋转向量和旋转矩阵的互转换

1.c++

处理矩阵三维转换时,通常采用旋转矩阵,但是旋转变换其实只有三个自由度,用旋转向量表达时更为简洁。因此,需要实现从旋转向量和旋转矩阵之间的互转换。
旋转向量和旋转矩阵之间可以通过罗德里格斯公式进行转换:

这里写图片描述
代码实现:
可以直接采用opencv中的Rodrigues函数实现,函数原型:

void Rodrigues( InputArray src, OutputArray dst, OutputArray jacobian = noArray() );

参数:
输入src:旋转向量(3*1或者1*3)或者旋转矩阵(3*3);
输出dst:旋转矩阵(3*3)或者旋转向量(3*1或者1*3);
输出jacobin:可选项,输出雅克比矩阵(3*9或者9*3),输入数组对输出数组的偏导数。

2.python

# -*- coding: UTF-8 -*-
import os
import cv2
import numpy as np
T = np.zeros((1,3), np.float32)
a = (0.2,0.4,0.8)
print a
R = cv2.Rodrigues(a);
print R[0]
v3 = (R[0][2,1],R[0][0,2],R[0][1,0])
print v3
c = cv2.Rodrigues(v3)
print c[0]
b = cv2.Rodrigues(R[0])
print b[0][0]

3.在做SLAM时,希望用到深度图来辅助生成场景,所以要构建立体视觉,在这里使用OpenCV的Stereo库和python来进行双目立体视觉的图像处理。http://www.mamicode.com/info-detail-1610612.html

猜你喜欢

转载自blog.csdn.net/weixin_37251044/article/details/80444552
今日推荐