由旋转矩阵到Rodrigues

由Rodrigues公式有：
R=Icosθ+(1−cosθ)uuT+u×sinθR=Icos\theta+(1-cos\theta)uu^T+u_{\times}sin\thetaR=Icosθ+(1−cosθ)uuT+u×sinθ
其中，Icosθ+(1−cosθ)uuTIcos\theta+(1-cos\theta)uu^TIcosθ+(1−cosθ)uuT是对称矩阵，u×sinθu_{\times}sin\thetau×sinθ是反对称矩阵。
所以，
R−RT=2u×sinθR-R^T = 2u_{\times}sin\thetaR−RT=2u×sinθ
因为R(n^,θ)=PR(z^,θ)P−1R(\hat{n},\theta)=PR(\hat{z},\theta)P^{-1}R(n^,θ)=PR(z^,θ)P−1,
所以tr(R)=2cosθ−1tr(R)=2cos\theta-1tr(R)=2cosθ−1
结合∥R−RT∥=2sinθ\|R-R^T\|=2sin\theta∥R−RT∥=2sinθ可以求解u、θu、\thetau、θ
参考代码：https://github.com/zjulion/handeyecat

Geo3d rodrigues2(const RotMat& matrix)
{Eigen::JacobiSVD<Eigen::Matrix3f> svd(matrix, Eigen::ComputeFullV | Eigen::ComputeFullU);RotMat R = svd.matrixU() * svd.matrixV().transpose();double rx = R(2, 1) - R(1, 2);double ry = R(0, 2) - R(2, 0);double rz = R(1, 0) - R(0, 1);double s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);double c = (R.trace() - 1) * 0.5; //c=cos(theta)c = c > 1. ? 1. : c < -1. ? -1. : c;double theta = acos(c);if (s < XEPS)//sin s =0{double t;if (c > 0) //cos s=1rx = ry = rz = 0;else{t = (R( 0, 0) + 1)*0.5;rx = sqrt(std::max(t, 0.0));t = (R(1, 1) + 1)*0.5;ry = sqrt(std::max(t, 0.0)) * (R(0, 1) < 0 ? -1.0 : 1.0);t = (R(2, 2) + 1)*0.5;rz = sqrt(std::max(t, 0.0)) * (R(0, 2) < 0 ? -1.0 : 1.0);if (fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R(1, 2) > 0) != (ry*rz > 0))//这里不懂rz = -rz;theta /= sqrt(rx*rx + ry*ry + rz*rz);rx *= theta;ry *= theta;rz *= theta;}}else{double vth = 1 / (2 * s);vth *= theta;rx *= vth; ry *= vth; rz *= vth;}return Eigen::Vector3d(rx, ry, rz).cast<float>();
}

参考源：
https://www2.cs.duke.edu/courses/fall13/compsci527/notes/rodrigues.pdf
http://web.cs.iastate.edu/~cs577/handouts/rotation.pdf

由旋转矩阵到Rodrigues相关推荐

ORB_SLAM2中的Sim3变换
对于双目.RGB-D相机,可获得深度,因此不存在尺度问题,因此Sim3中的尺度s=1. (1)通过词袋加速算法实现当前帧.闭环帧的特征点的匹配,建立闭环帧的路标点和当前帧的特征点间的联系. (2)使用 ...
《Learning OpenCV3》ch18：相机模型与标定
省赛期间用到双目视觉的时候,只是很粗浅地调用了下API,毕竟初学而且时间紧迫.最近打算跟一个硕士生再做一个相关项目,而且也想用Stereo Vision作为毕业设计,所以很有必要了解一下其中的原理.最 ...
OpenCV实现立体视觉的案例
尝试用OpenCV来实现立体视觉也有一段时间了,主要的参考资料就是Learning OpenCV十一.十二章和OpenCV论坛上一些前辈的讨论.过程中磕磕碰碰,走了不少弯路,终于在前不久解决了最头大的 ...
张正友畸变矫正C++代码
1,在自己的项目下创建文件夹sample->left 2,创建两个txt文件,caliberation_result.txt,filename.txt 3,将拍的标定图名称写到filename. ...
OpenCV-Python (官方)中文教程(部分三)
[部分二]:https://blog.csdn.net/Thomson617/article/details/103961274 第七章.相机标定与3D重构 42.摄像头标定在图像测量过程以及机器视 ...
(01)ORB-SLAM2源码无死角解析-(58) 闭环线程→计算Sim3: 源码Sim3Solver::iterate()讲解
讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (0 ...
cv::omni::StereoCalibrate 源码解析（一） —— 单目标定
cv::omni::StereoCalibrate 的代码逻辑和cv::StereoCalibrate相似. 在opencv库基础上稍微改动. //omni单目标定输入calibrate(世界坐标系 ...
摄像机标定和立体标定
尝试用OpenCV来实现立体视觉也有一段时间了,主要的参考资料就是Learning OpenCV十一.十二章和OpenCV论坛上一些前辈的讨论.过程中磕磕碰碰,走了不少弯路,终于在前不久解决了最头大的 ...
【算法】多视图几何三维重建+增量式SfM
多视图几何三维重建的基本原理: 从两个或者多个视点观察同一景物,已获得在多个不同的视角下对景物的多张感知图像,运用三角测量的基本原理计算图像像素间位置偏差,获得景物的三维深度信息,这一个过程与人类观察 ...

由旋转矩阵到Rodrigues

由旋转矩阵到Rodrigues相关推荐

最新文章

热门文章