1. 问题
有时我们需要测量或者标定两个相机之间的关系, 在3D空间中，存在一些特征点，而这些特征点在两个相机像素平面我们均可以拿到，这时可以根据这些特征点对，求出两个相机间的相对测量关系。

2. 理论
假设相机1的位姿为T1(世界坐标系到相机坐标系的表示方式), 假设相机2的位姿为T2(世界坐标系到相机坐标系的表示方式), 这时我们建立如下的相对测量关系
T = T1 * {Inv<矩阵运算\Inv>}(T2)
上面的相对测量关系T不随着相机1与相机2的刚体变换而改变, 换一句话说, 只要相机1与相机2它们相对固定住, 不管怎么移动它们, 它们之间的相对测量T不会改变, 而这个T往往是我们需要标定的量。

3. 求解
我们在已知相机内参以及多个匹配对后，想求得相机间的相对测量T，则使用如下方式

下面的方式求解的变量为6维度, 前3个维度对应旋转, 后3个维度对应为t.

3.1 创建对象
//使用{SLAM_PC2DTo2D_Create<矩阵运算\SLAM_PC2DTo2D_Create>}函数创建的2D-2D对极约束对象，比如执行类似如下代码
pc = {SLAM_PC2DTo2D_Create<矩阵运算\SLAM_PC2DTo2D_Create>}(K1, K2, T, Pt1, Pt2, w)

//设置求解返回, 前3个反映旋转R, 后3个参数反映t的范围,这里t的范围可看情况更改
a  = [0,0,0,--1000, -1000, -1000];
b  = [6.284, 6.284, 6.284, 1000, 1000, 1000];

3.2 【启发式优化】求解
(1)【变量个数】设置为6
(2)【变量上限】设置为b
(3)【变量下限】设置为a
(4)【梯度数据】设置为0
(5)将如下代码复制到代码框里,下面的代码计算完毕后,会将最佳T保存到对象pc中. 因此执行完之后也可以使用 {SLAM_PC2DTo2D_GetT<矩阵运算\SLAM_PC2DTo2D_GetT>}(pc)函数获取矩阵形式结果
(6)点击【计算】即可
/******************************************************* 
Public Function TargetFun(ByRef var() As Double, ByRef obj As List(Of Object)) As Double
    Dim a As Object = obj(0)
    a.SetT(var)
    Return a.CalcError()
End Function

Public Sub RunStart(ByRef obj As List(Of Object))
    Dim a As Object = API_GetObject("pc")
    obj.Add(a)
End Sub

Public Sub RunEnd(ByVal er As Double, ByRef var() As Double, ByRef obj As List(Of Object))
    Dim a As Object = obj(0)
    a.SetT(var)
End Sub
*******************************************************/

3.3 【常规优化】求解
【Parameter】下的参数如下设置,注意data后为空, errorCount后的m为匹配点个数, 即{SLAM_PC2DTo2D_Create<矩阵运算\SLAM_PC2DTo2D_Create>}第4个参数的行数。
data       =   
varCount   = 6
varInital  = 0   
varMax     = b 
varMin     = a
errorCount = m    
autoDiff   = 0  
autoUpdate = 0   

【Code】里使用如下代码,下面的代码计算完毕后,会将最佳T保存到对象pc中. 因此执行完之后也可以使用 {SLAM_PC2DTo2D_GetT<矩阵运算\SLAM_PC2DTo2D_GetT>}(pc)函数获取矩阵形式结果
(6)点击【计算】即可
/******************************************************* 
Public Sub ErrorFun(ByRef var() As Double,
			ByRef data() As Double,
			ByRef obj  As Object,
			ByRef f() As Double)
    Dim a As Object = obj(0)
    a.SetT(var)
    a.CalcOptError(f)
End Sub
	
Public Sub ErrorJacobi(ByRef var() As Double,
			   ByRef data() As Double,
			   ByRef obj  As Object,
			   ByRef jacob(,) As Double)
    Dim a As Object = obj(0)
    a.SetT(var)
    a.CalcOptJacobi(jacob)
End Sub
	
Public Sub Update(ByRef var() As Double,
			ByRef dvar() As Double,
			ByRef max() As Double,
			ByRef min() As Double,
			ByRef vartype() As Byte,
			ByRef r As System.Random,
			ByRef var1() As Double)
    Slam_UpdateLeftDx(var, dvar, var1)
End Sub

Public Sub NormVar( ByRef var() As Double,
			ByRef data(,) As Double,
			ByRef other As Object)
End Sub

Public Sub TargetFun(  ByRef x As Double,
			ByRef r As Double,
			ByRef dx As Double)
	r = x * x
	dx= x + x
End Sub

Public Sub RunStart(ByRef data(,) As Double, ByRef obj  As Object)
	Dim a As Object = API_GetObject("pc")
    obj.Add(a)
End Sub

Public Sub RunEnd(ByRef optVar() As Double，
                ByRef minError As Double,
                ByRef loopSum As Integer,
                ByRef costTime As Integer,
                ByRef data(,) As Double, 
                ByRef obj  As Object)
    Dim a As Object = obj(0)
    a.SetT(var)
End Sub
*******************************************************/