罗德里格斯公式 理解、推导

  罗德里格斯公式（Rodriguez formula）是计算机视觉中的一大经典公式，在描述相机位姿的过程中很常用。公式：

$$R=I+sin(\theta )K+(1-cos(\theta ))K^2$$

  在三维空间中，旋转矩阵$R$可以对坐标系（基向量组）进行刚性的旋转变换。

$$R=\left[\begin{array}{ccc}{r}_{xx}& r_{xy}& r_{xz}\\ r_{yx}& r_{yy}& r_{yz}\\ r_{zx}& r_{zy}& r_{zz}\end{array}\right]$$

  通常为了方便计算，基向量组中的向量是相互正交的且都为单位向量，那么$R$就是一个标准正交矩阵。两个重要性质：

• $R^{T}R=R^{-1}R=E$
• $|R|=1$

  假设原坐标系基向量矩阵为$B$，旋转后的坐标系基向量矩阵为$C$。

$$B=\left[\begin{array}{ccc}{b}_x& b_y& b_z\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

$$C=RB$$

  其变换过程如图所示：

$$C=\left[\begin{array}{ccc}{r}_{xx}& r_{xy}& r_{xz}\\ r_{yx}& r_{yy}& r_{yz}\\ r_{zx}& r_{zy}& r_{zz}\end{array}\right]\left[\begin{array}{ccc}{b}_x& b_y& b_z\end{array}\right]$$
  根据线性代数的定义，旋转矩阵$R$就是从基向量矩阵$B$到基向量矩阵$C$的过渡矩阵。由于旋转矩阵$R$是标准3阶正交矩阵，故旋转矩阵$R$的自由度为3，这说明最少可以用三个变量来表示旋转矩阵$R$，这就是**罗德里格斯公式（Rodriguez formula）**存在的基础。

  **罗德里格斯公式（Rodriguez formula）**首先要确定一个三维的单位向量$k={\left[\begin{array}{ccc}{k}_x& k_y& k_z\end{array}\right]}^T$（两个自由度）和一个标量 $\theta$ （一个自由度）。

**证明方法一：**

（图片摘自Wiki）

  先考虑对一个向量作旋转，其中 $v$ 是原向量，三维的单位向量 $k={\left[\begin{array}{ccc}{k}_x& k_y& k_z\end{array}\right]}^T$是旋转轴， $\theta$ 是旋转角度，$v_{rot}$是旋转后的向量。

  先通过点积得到 $v$ 在 $k$ 方向的平行分量 $v_{\parallel }$。
$$v_{\parallel }=(v\cdot k)k$$

  再通过叉乘得到与 $k$ 正交的两个向量 $v_{\perp }$ 和 $w$ 。
$$v_{\perp }=v-v_{\parallel }=v-(v\cdot k)k=-k\times (k\times v)\cdot \cdot \cdot \cdot \cdot \cdot (1)$$ $$w=k\times v$$

  这样，我们就得到了3个相互正交的向量。不难得出：
$$v_{rot}=v_{\parallel }+cos(\theta )v_{\perp }+sin(\theta )w$$

  再引入叉积矩阵的概念：记 $K$ 为 $k={\left[\begin{array}{ccc}{k}_x& k_y& k_z\end{array}\right]}^T$ 的叉积矩阵。显然 $K$ 是一个反对称矩阵。
$$K=\left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]$$

  他有如下性质：
$$k\times v=Kv$$

  为了利用该性质，需要将 $v_{rot} $ 代换为 $v $ 与 $k $ 的叉积关系，先根据（1）式做代换：
$$v_{\parallel }=v+k\times (k\times v)$$
  然后得到：
$$v_{rot}=v+k\times (k\times v)-cos(\theta )k\times (k\times v)+sin(\theta )k\times v$$

  根据叉积矩阵性质：
$$v_{rot}=v+(1-cos(\theta ))K^{2}v+sin(\theta )Kv$$ $$v_{rot}=(I+(1-cos(\theta ))K^2+sin(\theta )K)v$$

  最后将 $v、v_{rot}$ 换为 $B、C$，就是罗德里格斯公式的标准形式。

$$B=(I+(1-cos(\theta ))K^2+sin(\theta )K)C\iff R=I+(1-cos(\theta ))K^2+sin(\theta )K$$

  这里我取 $k={\left[\begin{array}{ccc}{3}^{-0.5}& 3^{-0.5}& 3^{-0.5}\end{array}\right]}^T$ ，可以看到通过罗德里格斯公式作基变换$C=RB$的过程。


绘图用的Matlab代码：

在这里插入代码片


% Rodriguez formula 
% 作者：龚冰剑
% 时间：2017年12月29日 16:39:14


clc
clear all
close all

%
W_vec = [3^(-0.5)   3^(-0.5)    3^(-0.5)];
W_mat = [0          -W_vec(3)   W_vec(2);
        W_vec(3)    0           -W_vec(1);
        -W_vec(2)   W_vec(1)    0       ];
  
%
zeta = 0 : pi / 180 : pi / 3;

%
origin_point = [0   0   0];
vector_x = [1   0   0];
vector_y = [0   1   0];
vector_z = [0   0   1];

% color
color_table = linspecer(length(zeta));

for i = 1 : length(zeta)
    
    %hold off
    
    plot3([origin_point(1) W_vec(1)],[origin_point(2) W_vec(2)],[origin_point(3) W_vec(3)], 'r--o', 'linewidth',2)
    hold on

    % 绘制原基向量
    plot3([origin_point(1) vector_x(1)],[origin_point(2) vector_x(2)],[origin_point(3) vector_x(3)], 'r')
    plot3([origin_point(1) vector_y(1)],[origin_point(2) vector_y(2)],[origin_point(3) vector_y(3)], 'r')
    plot3([origin_point(1) vector_z(1)],[origin_point(2) vector_z(2)],[origin_point(3) vector_z(3)], 'r')
    
    %
    Rot_mat = diag([1 1 1]) + sin(zeta(i)) * W_mat + (1 - cos(zeta(i))) * W_mat * W_mat;
    
    vector_x_Rot = vector_x * Rot_mat;
    vector_y_Rot = vector_y * Rot_mat;
    vector_z_Rot = vector_z * Rot_mat;
    
    % 绘制变换后的基向量
    plot3([origin_point(1) vector_x_Rot(1)],[origin_point(2) vector_x_Rot(2)],[origin_point(3) vector_x_Rot(3)], '-*','color', color_table(i, :))
    plot3([origin_point(1) vector_y_Rot(1)],[origin_point(2) vector_y_Rot(2)],[origin_point(3) vector_y_Rot(3)], '-*','color', color_table(i, :))
    plot3([origin_point(1) vector_z_Rot(1)],[origin_point(2) vector_z_Rot(2)],[origin_point(3) vector_z_Rot(3)], '-*','color', color_table(i, :))
    
    axis([-0.5 1 -0.5 1 -0.5 1])
    view(100, 30);
    drawnow()
    title(sprintf('θ = %.3f 度', zeta(i) * 180 / pi))
    grid on
    pause(0.05)
end

颜色表“linspecer.m”文件（推荐使用）

% function lineStyles = linspecer(N)
% This function creates an Nx3 array of N [R B G] colors
% These can be used to plot lots of lines with distinguishable and nice
% looking colors.
% 
% lineStyles = linspecer(N);  makes N colors for you to use: lineStyles(ii,:)
% 
% colormap(linspecer); set your colormap to have easily distinguishable 
%                      colors and a pleasing aesthetic
% 
% lineStyles = linspecer(N,'qualitative'); forces the colors to all be distinguishable (up to 12)
% lineStyles = linspecer(N,'sequential'); forces the colors to vary along a spectrum 
% 
% % Examples demonstrating the colors.
% 
% LINE COLORS
% N=6;
% X = linspace(0,pi*3,1000); 
% Y = bsxfun(@(x,n)sin(x+2*n*pi/N), X.', 1:N); 
% C = linspecer(N);
% axes('NextPlot','replacechildren', 'ColorOrder',C);
% plot(X,Y,'linewidth',5)
% ylim([-1.1 1.1]);
% 
% SIMPLER LINE COLOR EXAMPLE
% N = 6; X = linspace(0,pi*3,1000);
% C = linspecer(N)
% hold off;
% for ii=1:N
%     Y = sin(X+2*ii*pi/N);
%     plot(X,Y,'color',C(ii,:),'linewidth',3);
%     hold on;
% end
% 
% COLORMAP EXAMPLE
% A = rand(15);
% figure; imagesc(A); % default colormap
% figure; imagesc(A); colormap(linspecer); % linspecer colormap
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% by Jonathan Lansey, March 2009-2013 �Lansey at gmail.com               %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
%% credits and where the function came from
% The colors are largely taken from:
% http://colorbrewer2.org and Cynthia Brewer, Mark Harrower and The Pennsylvania State University
% 
% 
% She studied this from a phsychometric perspective and crafted the colors
% beautifully.
% 
% I made choices from the many there to decide the nicest once for plotting
% lines in Matlab. I also made a small change to one of the colors I
% thought was a bit too bright. In addition some interpolation is going on
% for the sequential line styles.
% 
% 
%%

function lineStyles=linspecer(N,varargin)

if nargin==0 % return a colormap
    lineStyles = linspecer(64);
%     temp = [temp{:}];
%     lineStyles = reshape(temp,3,255)';
    return;
end

if N<=0 % its empty, nothing else to do here
    lineStyles=[];
    return;
end

% interperet varagin
qualFlag = 0;

if ~isempty(varargin)>0 % you set a parameter?
    switch lower(varargin{1})
        case {'qualitative','qua'}
            if N>12 % go home, you just can't get this.
                warning('qualitiative is not possible for greater than 12 items, please reconsider');
            else
                if N>9
                    warning(['Default may be nicer for ' num2str(N) ' for clearer colors use: whitebg(''black''); ']);
                end
            end
            qualFlag = 1;
        case {'sequential','seq'}
            lineStyles = colorm(N);
            return;
        otherwise
            warning(['parameter ''' varargin{1} ''' not recognized']);
    end
end      
      
% predefine some colormaps
  set3 = colorBrew2mat({[141, 211, 199];[ 255, 237, 111];[ 190, 186, 218];[ 251, 128, 114];[ 128, 177, 211];[ 253, 180, 98];[ 179, 222, 105];[ 188, 128, 189];[ 217, 217, 217];[ 204, 235, 197];[ 252, 205, 229];[ 255, 255, 179]}');
set1JL = brighten(colorBrew2mat({[228, 26, 28];[ 55, 126, 184];[ 77, 175, 74];[ 255, 127, 0];[ 255, 237, 111]*.95;[ 166, 86, 40];[ 247, 129, 191];[ 153, 153, 153];[ 152, 78, 163]}'));
set1 = brighten(colorBrew2mat({[ 55, 126, 184]*.95;[228, 26, 28];[ 77, 175, 74];[ 255, 127, 0];[ 152, 78, 163]}),.8);

set3 = dim(set3,.93);

switch N
    case 1
        lineStyles = { [  55, 126, 184]/255};
    case {2, 3, 4, 5 }
        lineStyles = set1(1:N);
    case {6 , 7, 8, 9}
        lineStyles = set1JL(1:N)';
    case {10, 11, 12}
        if qualFlag % force qualitative graphs
            lineStyles = set3(1:N)';
        else % 10 is a good number to start with the sequential ones.
            lineStyles = cmap2linspecer(colorm(N));
        end
otherwise % any old case where I need a quick job done.
    lineStyles = cmap2linspecer(colorm(N));
end
lineStyles = cell2mat(lineStyles);
end

% extra functions
function varIn = colorBrew2mat(varIn)
for ii=1:length(varIn) % just divide by 255
    varIn{ii}=varIn{ii}/255;
end        
end

function varIn = brighten(varIn,varargin) % increase the brightness

if isempty(varargin),
    frac = .9; 
else
    frac = varargin{1}; 
end

for ii=1:length(varIn)
    varIn{ii}=varIn{ii}*frac+(1-frac);
end        
end

function varIn = dim(varIn,f)
    for ii=1:length(varIn)
        varIn{ii} = f*varIn{ii};
    end
end

function vOut = cmap2linspecer(vIn) % changes the format from a double array to a cell array with the right format
vOut = cell(size(vIn,1),1);
for ii=1:size(vIn,1)
    vOut{ii} = vIn(ii,:);
end
end
%%
% colorm returns a colormap which is really good for creating informative
% heatmap style figures.
% No particular color stands out and it doesn't do too badly for colorblind people either.
% It works by interpolating the data from the
% 'spectral' setting on http://colorbrewer2.org/ set to 11 colors
% It is modified a little to make the brightest yellow a little less bright.
function cmap = colorm(varargin)
n = 100;
if ~isempty(varargin)
    n = varargin{1};
end

if n==1
    cmap =  [0.2005    0.5593    0.7380];
    return;
end
if n==2
     cmap =  [0.2005    0.5593    0.7380;
              0.9684    0.4799    0.2723];
          return;
end

frac=.95; % Slight modification from colorbrewer here to make the yellows in the center just a bit darker
cmapp = [158, 1, 66; 213, 62, 79; 244, 109, 67; 253, 174, 97; 254, 224, 139; 255*frac, 255*frac, 191*frac; 230, 245, 152; 171, 221, 164; 102, 194, 165; 50, 136, 189; 94, 79, 162];
x = linspace(1,n,size(cmapp,1));
xi = 1:n;
cmap = zeros(n,3);
for ii=1:3
    cmap(:,ii) = pchip(x,cmapp(:,ii),xi);
end
cmap = flipud(cmap/255);
end

参考：

1.Rodrigues’ rotation formula - Wiki

2.《视觉SLAM十四讲》P49

3.关于罗德里格斯公式的简单推导 新浪博客

4.DERIVATION OF THE EULER–RODRIGUES FORMULA FOR THREE-DIMENSIONAL ROTATIONS…