标签：Sim3 bar const -- 单目 exp g2o optimizer vSim3
相似变换群与李代数
对于单目视觉SLAM，由于单目的不确定性，在闭环检测中为了解决尺度漂移，一般会用到相似变换群
Sim3,用来描述相似变换：
p
′
=
[
s
R
t
0
1
]
p
=
s
R
p
+
t
p'=\begin{bmatrix} s\bm R&\bm t\\ \\0&1 \end{bmatrix}p=s\bm{Rp+t}
p′=⎣⎡sR0t1⎦⎤p=sRp+t
和SO3、SE3类似，可以将其描述为群：
S
i
m
(
3
)
=
{
S
=
[
s
R
t
0
1
]
∈
R
4
×
4
}
Sim(3)=\{S= \begin{bmatrix} s\bm R&\bm t\\ \\0&1 \end{bmatrix} \in {\mathbb{R}^{4\times 4}} \}
Sim(3)={S=⎣⎡sR0t1⎦⎤∈R4×4}
对应的李代数sim(3)是一个7维的向量（7个自由度）：
s
i
m
(
3
)
=
{
ζ
=
[
ρ
ϕ
σ
]
∈
R
7
∣
ρ
∈
R
3
,
ϕ
∈
s
o
(
3
)
,
ζ
∧
=
[
σ
I
+
ϕ
∧
ρ
0
T
0
]
∈
R
4
×
4
}
sim(3) = \{ \zeta = \left[ {\begin{matrix} \rho \\ \phi \\ \sigma \end{matrix}} \right] \in {\mathbb{R}^7}|\rho \in {\mathbb{R}^3},\phi \in so(3),{\zeta ^ \wedge } = \left[ {\begin{matrix} {\sigma I+{\phi ^ \wedge }}&\rho \\ ~\\ {{0^T}}&0 \end{matrix}} \right]\in {\mathbb{R}^{4\times 4}} \}
sim(3)={ζ=⎣⎡ρϕσ⎦⎤∈R7∣ρ∈R3,ϕ∈so(3),ζ∧=⎣⎡σI+ϕ∧ 0Tρ0⎦⎤∈R4×4}
其指数映射为：
[
s
R
t
0
1
]
=
exp
(
ζ
∧
)
=
[
exp
(
σ
)
+
exp
(
ϕ
∧
)
J
s
ρ
0
T
1
]
\begin{bmatrix} s\bm R&\bm t\\ \\0&1 \end{bmatrix}=\exp(\zeta ^\wedge)=\left[ {\begin{matrix} {\exp(\sigma)+\exp({\phi ^ \wedge }})& J_s\rho \\ ~\\ {{0^T}}&1 \end{matrix}} \right]
⎣⎡sR0t1⎦⎤=exp(ζ∧)=⎣⎡exp(σ)+exp(ϕ∧) 0TJsρ1⎦⎤
其中
J
s
=
e
σ
−
1
σ
I
+
σ
e
σ
sin
θ
+
(
1
−
e
σ
cos
θ
)
θ
σ
2
+
θ
2
a
∧
+
(
e
σ
−
1
σ
−
(
e
σ
cos
θ
−
1
)
σ
+
(
e
σ
sin
θ
)
θ
σ
2
+
θ
2
)
a
∧
a
∧
J_s = \frac {e^{\sigma}-1}{\sigma}\bm I+\frac{\sigma e^\sigma \sin \theta+(1-e^\sigma\cos \theta)\theta} {\sigma^2+\theta^2}\bm a^{\wedge}+(\frac {e^{\sigma}-1} {\sigma}-\frac {(e^\sigma\cos \theta-1)\sigma+(e^\sigma \sin \theta)\theta} {\sigma^2+\theta^2})\bm a^{\wedge}\bm a^{\wedge}
Js=σeσ−1I+σ2+θ2σeσsinθ+(1−eσcosθ)θa∧+(σeσ−1−σ2+θ2(eσcosθ−1)σ+(eσsinθ)θ)a∧a∧
回环检测-Sim3求解
参考文献[1]，求解。
对于Sim3群，有三对匹配点：
[
s
R
t
0
1
]
，
x
i
=
s
R
x
ˉ
i
+
t
i
∈
(
1
,
2
,
3
)
\begin{bmatrix} s\bm R&\bm t\\ \\0&1 \end{bmatrix}，x_i = s\bm{R} \bar x_i+t \quad i \in (1,2,3)
⎣⎡sR0t1⎦⎤，xi=sRxˉi+ti∈(1,2,3)
计算质心：
c
=
1
3
(
x
1
+
x
2
+
x
3
)
,
c
ˉ
=
1
3
(
x
ˉ
1
+
x
ˉ
2
+
x
ˉ
3
)
c =\frac 1 3(x_1+x_2+x_3),\bar c = \frac 1 3 (\bar x_1+\bar x_2+\bar x_3)
c=31(x1+x2+x3),cˉ=31(xˉ1+xˉ2+xˉ3)
减去质心点：
y
i
=
x
i
−
c
,
y
ˉ
i
=
x
ˉ
i
−
c
ˉ
y_i = x_i-c,\quad \bar y_i =\bar x_i-\bar c
yi=xi−c,yˉi=xˉi−cˉ
得到映射矩阵H：
H
=
y
1
y
ˉ
1
T
+
y
2
y
ˉ
2
T
+
y
3
y
ˉ
3
T
H = y_1 \bar y_1^T + y_2 \bar y_2^T+ y_3\bar y_3^T
H=y1yˉ1T+y2yˉ2T+y3yˉ3T
奇异值SVD分解：
H
=
U
⋅
W
⋅
V
T
H = U\cdot W\cdot V^T
H=U⋅W⋅VT
求解得到R、t和尺度因子s：
R
=
V
⋅
U
T
,
t
=
c
−
s
R
c
ˉ
s
=
(
∥
y
1
∥
2
2
+
∥
y
2
∥
2
2
+
∥
y
3
∥
2
2
)
1
2
(
∥
y
ˉ
1
∥
2
2
+
∥
y
ˉ
2
∥
2
2
+
∥
y
ˉ
3
∥
2
2
)
1
2
R = V\cdot U^T,\quad t=c-s\bm R\bar c\\~\\ s=\frac {(\left\|y_1 \right\|_2^2+\left\|y_2 \right\|_2^2+\left\|y_3 \right\|_2^2)^{\frac 1 2}} {(\left\| \bar y_1 \right\|_2^2+\left\|\bar y_2 \right\|_2^2+\left\|\bar y_3 \right\|_2^2)^{\frac 1 2}}
R=V⋅UT,t=c−sRcˉ s=(∥yˉ1∥22+∥yˉ2∥22+∥yˉ3∥22)21(∥y1∥22+∥y2∥22+∥y3∥22)21
Sim3位姿图优化
我们知道SE3的位姿图优化：
e
i
j
(
ξ
i
,
ξ
j
)
=
ln
[
exp
(
−
ξ
i
j
∧
)
exp
(
−
ξ
i
∧
)
⋅
exp
(
ξ
j
∧
)
]
∨
,
e
i
j
(
ξ
i
,
ξ
j
)
=
ln
[
T
i
j
−
1
⋅
T
i
−
1
⋅
T
j
]
∨
e_{ij}(\xi_i,\xi_j)=\ln[\exp(-\xi_{ij} ^\land) \exp(-\xi_i ^\land)\cdot \exp(\xi_j ^\land)]^\vee, \\ ~\\e_{ij}(\xi_i,\xi_j)=\ln[T_{ij}^{-1}\cdot T_i^{-1}\cdot T_j]^\vee
eij(ξi,ξj)=ln[exp(−ξij∧)exp(−ξi∧)⋅exp(ξj∧)]∨, eij(ξi,ξj)=ln[Tij−1⋅Ti−1⋅Tj]∨
同理，对于Sim3的位姿图优化的误差函数可以表示为：
e
i
j
(
ζ
i
,
ζ
j
)
=
ln
[
exp
(
−
ζ
i
j
∧
)
exp
(
−
ζ
i
∧
)
⋅
exp
(
ζ
j
∧
)
]
∨
即
e
i
j
(
ζ
i
,
ζ
j
)
=
ln
[
S
i
j
−
1
⋅
S
i
−
1
⋅
S
j
]
∨
e_{ij}(\zeta_i,\zeta_j)=\ln[\exp(-\zeta_{ij} ^\land) \exp(-\zeta_i ^\land)\cdot \exp(\zeta_j ^\land)]^\vee\\ ~\\ 即\quad e_{ij}(\zeta_i,\zeta_j)=\ln[S_{ij}^{-1}\cdot S_i^{-1}\cdot S_j]^\vee
eij(ζi,ζj)=ln[exp(−ζij∧)exp(−ζi∧)⋅exp(ζj∧)]∨ 即eij(ζi,ζj)=ln[Sij−1⋅Si−1⋅Sj]∨
为了为优化作准备，我们保持每个位姿旋转和平移不变，并设置s=1，只有当回环检测的时候
s
≠
1
s\ne 1
s=1；
雅克比矩阵推导略；具体参考文献[2].
我们依然可以用g2o进行sim3的优化.
附上ORB-SLAM2中优化Sim3的源码：
int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)
{
g2o::SparseOptimizer optimizer;
g2o::BlockSolverX::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
// Calibration
const cv::Mat &K1 = pKF1->mK;
const cv::Mat &K2 = pKF2->mK;
// Camera poses
const cv::Mat R1w = pKF1->GetRotation();
const cv::Mat t1w = pKF1->GetTranslation();
const cv::Mat R2w = pKF2->GetRotation();
const cv::Mat t2w = pKF2->GetTranslation();
// Set Sim3 vertex
g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();
vSim3->_fix_scale=bFixScale;
vSim3->setEstimate(g2oS12);
vSim3->setId(0);
vSim3->setFixed(false);
vSim3->_principle_point1[0] = K1.at<float>(0,2);
vSim3->_principle_point1[1] = K1.at<float>(1,2);
vSim3->_focal_length1[0] = K1.at<float>(0,0);
vSim3->_focal_length1[1] = K1.at<float>(1,1);
vSim3->_principle_point2[0] = K2.at<float>(0,2);
vSim3->_principle_point2[1] = K2.at<float>(1,2);
vSim3->_focal_length2[0] = K2.at<float>(0,0);
vSim3->_focal_length2[1] = K2.at<float>(1,1);
optimizer.addVertex(vSim3);
// Set MapPoint vertices
const int N = vpMatches1.size();
const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;
vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21;
vector<size_t> vnIndexEdge;
vnIndexEdge.reserve(2*N);
vpEdges12.reserve(2*N);
vpEdges21.reserve(2*N);
const float deltaHuber = sqrt(th2);
int nCorrespondences = 0;
for(int i=0; i<N; i++)
{
if(!vpMatches1[i])
continue;
MapPoint* pMP1 = vpMapPoints1[i];
MapPoint* pMP2 = vpMatches1[i];
const int id1 = 2*i+1;
const int id2 = 2*(i+1);
const int i2 = pMP2->GetIndexInKeyFrame(pKF2);
if(pMP1 && pMP2)
{
if(!pMP1->isBad() && !pMP2->isBad() && i2>=0)
{
g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();
cv::Mat P3D1w = pMP1->GetWorldPos();
cv::Mat P3D1c = R1w*P3D1w + t1w;
vPoint1->setEstimate(Converter::toVector3d(P3D1c));
vPoint1->setId(id1);
vPoint1->setFixed(true);
optimizer.addVertex(vPoint1);
g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();
cv::Mat P3D2w = pMP2->GetWorldPos();
cv::Mat P3D2c = R2w*P3D2w + t2w;
vPoint2->setEstimate(Converter::toVector3d(P3D2c));
vPoint2->setId(id2);
vPoint2->setFixed(true);
optimizer.addVertex(vPoint2);
}
else
continue;
}
else
continue;
nCorrespondences++;
// Set edge x1 = S12*X2
Eigen::Matrix<double,2,1> obs1;
const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];
obs1 << kpUn1.pt.x, kpUn1.pt.y;
g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();
e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));
e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
e12->setMeasurement(obs1);
const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];
e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1);
g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber;
e12->setRobustKernel(rk1);
rk1->setDelta(deltaHuber);
optimizer.addEdge(e12);
// Set edge x2 = S21*X1
Eigen::Matrix<double,2,1> obs2;
const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];
obs2 << kpUn2.pt.x, kpUn2.pt.y;
g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::EdgeInverseSim3ProjectXYZ();
e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1)));
e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
e21->setMeasurement(obs2);
float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave];
e21->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare2);
g2o::RobustKernelHuber* rk2 = new g2o::RobustKernelHuber;
e21->setRobustKernel(rk2);
rk2->setDelta(deltaHuber);
optimizer.addEdge(e21);
vpEdges12.push_back(e12);
vpEdges21.push_back(e21);
vnIndexEdge.push_back(i);
}
// Optimize!
optimizer.initializeOptimization();
optimizer.optimize(5);
// Check inliers
int nBad=0;
for(size_t i=0; i<vpEdges12.size();i++)
{
g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
if(!e12 || !e21)
continue;
if(e12->chi2()>th2 || e21->chi2()>th2)
{
size_t idx = vnIndexEdge[i];
vpMatches1[idx]=static_cast<MapPoint*>(NULL);
optimizer.removeEdge(e12);
optimizer.removeEdge(e21);
vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);
vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);
nBad++;
}
}
int nMoreIterations;
if(nBad>0)
nMoreIterations=10;
else
nMoreIterations=5;
if(nCorrespondences-nBad<10)
return 0;
// Optimize again only with inliers
optimizer.initializeOptimization();
optimizer.optimize(nMoreIterations);
int nIn = 0;
for(size_t i=0; i<vpEdges12.size();i++)
{
g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
if(!e12 || !e21)
continue;
if(e12->chi2()>th2 || e21->chi2()>th2)
{
size_t idx = vnIndexEdge[i];
vpMatches1[idx]=static_cast<MapPoint*>(NULL);
}
else
nIn++;
}
// Recover optimized Sim3
g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));
g2oS12= vSim3_recov->estimate();
return nIn;
}
参考文献
1.Horn, Berthold, K, et al. Closed-form solution of absolute orientation using unit quaternions[J]. Journal of the Optical Society of America A, 1987.
2.Strasdat H . Local Accuracy and Global Consistency for Efficient SLAM[D]. PhD thesis, Imperial College London, 2012.
3.视觉SLAM十四讲–高翔
4.ORB-SLAM2 源码
标签：Sim3,bar,const,--,单目,exp,g2o,optimizer,vSim3 来源： https://blog.csdn.net/qq_42995327/article/details/119299158
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。