1 ICP算法的推导
- Reference Paper： LiDAR-Camera Calibration using 3D-3D Point correspondences
目标函数， Q Q Q为target cloud， P P P为source cloud
F
(
t
)
=
∑
i
=
1
n
∥
(
R
P
i
+
t
)
−
Q
i
∥
2
F(t)=\sum_{i=1}^{n}\left\|\left(R P_{i}+t\right)-Q_{i}\right\|^{2}
F(t)=i=1∑n∥(RPi+t)−Qi∥2
首先对
t
t
t进行求导，
∂
F
(
t
)
∂
t
=
2
∑
i
=
1
n
(
R
P
i
+
t
)
−
Q
i
=
0
∂
F
(
t
)
∂
t
=
2
R
∑
i
=
1
n
P
i
+
2
t
∑
i
=
1
n
1
−
2
∑
i
=
1
n
Q
i
\begin{array}{c} \frac{\partial F(t)}{\partial t}=2 \sum_{i=1}^{n}\left(R P_{i}+t\right)-Q_{i}=0 \\ \frac{\partial F(t)}{\partial t}=2 R \sum_{i=1}^{n} P_{i}+2 t \sum_{i=1}^{n} 1-2 \sum_{i=1}^{n} Q_{i} \end{array}
∂t∂F(t)=2∑i=1n(RPi+t)−Qi=0∂t∂F(t)=2R∑i=1nPi+2t∑i=1n1−2∑i=1nQi
发现求出
R
R
R之后再去求解
t
t
t会非常简单，
t
=
1
n
∑
i
=
1
n
Q
i
−
R
1
n
∑
i
=
1
n
P
i
t=\frac{1}{n} \sum_{i=1}^{n} Q_{i}-R \frac{1}{n} \sum_{i=1}^{n} P_{i}
t=n1i=1∑nQi−Rn1i=1∑nPi
t
=
Q
ˉ
−
R
P
ˉ
t=\bar{Q}-R \bar{P}
t=Qˉ−RPˉ
将优化问题转化为只和
R
R
R有关
R
=
arg
min
R
∈
S
O
(
3
)
∥
(
R
(
P
i
−
P
ˉ
)
−
(
Q
i
−
Q
ˉ
)
∥
2
R=\underset{R \in S O(3)}{\arg \min }\left\|\left(R\left(P_{i}-\bar{P}\right)-\left(Q_{i}-\bar{Q}\right) \|^{2}\right.\right.
R=R∈SO(3)argmin∥∥(R(Pi−Pˉ)−(Qi−Qˉ)∥2
X
i
=
(
P
i
−
P
ˉ
)
,
Y
i
=
(
Q
i
−
Q
ˉ
)
X_i=\left(P_{i}-\bar{P}\right), Y_i=\left(Q_{i}-\bar{Q}\right)
Xi=(Pi−Pˉ),Yi=(Qi−Qˉ)
对式子进行展开，发现
R
=
arg
min
R
∈
S
O
(
3
)
∑
∥
R
X
i
−
Y
i
∥
2
=
T
r
(
∑
∥
R
X
i
−
Y
i
∥
2
)
=
∑
T
r
(
∥
R
X
i
−
Y
i
∥
2
)
=
∑
(
T
r
(
X
i
T
R
T
R
X
i
)
+
T
r
(
Y
i
T
Y
i
)
−
2
T
r
(
Y
i
T
R
X
i
)
)
R = \underset{R \in S O(3)}{\arg \min } \sum \| RX_i -Y_i\|^2 \\ = Tr(\sum \| RX_i -Y_i\|^2)\\ =\sum Tr(\| RX_i -Y_i\|^2)\\ =\sum (Tr(X_i^TR^TRX_i)+Tr(Y_i^TY_i)-2Tr(Y_i^TRX_i))
R=R∈SO(3)argmin∑∥RXi−Yi∥2=Tr(∑∥RXi−Yi∥2)=∑Tr(∥RXi−Yi∥2)=∑(Tr(XiTRTRXi)+Tr(YiTYi)−2Tr(YiTRXi))
发现前两项和
R
R
R没有关系
优化问题转化为
R
=
arg
max
R
∈
S
O
(
3
)
∑
T
r
(
Y
i
T
R
X
i
)
)
=
∑
T
r
(
R
X
i
Y
i
T
)
)
=
T
r
(
R
∑
X
i
Y
i
T
)
=
T
r
(
R
X
3
×
n
Y
3
×
n
T
)
R = \underset{R \in S O(3)}{\arg \max } \sum Tr(Y_i^TRX_i))\\ =\sum Tr(RX_iY_i^T))\\ =Tr(R \sum X_iY_i^T)\\ =Tr(RX_{3×n}Y_{3×n}^T)
R=R∈SO(3)argmax∑Tr(YiTRXi))=∑Tr(RXiYiT))=Tr(R∑XiYiT)=Tr(RX3×nY3×nT)
做svd分解
X
Y
T
=
U
D
V
T
XY^T=UDV^T
XYT=UDVT
R
=
arg
max
R
∈
S
O
(
3
)
T
r
(
R
X
Y
T
)
=
T
r
(
R
U
D
V
T
)
=
T
r
(
V
T
R
U
D
)
R = \underset{R \in S O(3)}{\arg \max }Tr(RXY^T)\\ =Tr(RUDV^T)\\ =Tr(V^TRUD)
R=R∈SO(3)argmaxTr(RXYT)=Tr(RUDVT)=Tr(VTRUD)
let
M
=
V
T
R
U
M=V^TRU
M=VTRU
有
T
r
(
M
D
)
=
∑
M
i
i
d
i
<
=
∑
d
i
Tr(MD)=\sum M_{ii}d_i<=\sum d_i
Tr(MD)=∑Miidi<=∑di
此时
M
=
I
=
V
T
R
U
M=I=V^TRU
M=I=VTRU
得到
R
=
V
U
T
R=VU^T
R=VUT
R
R
R应该满足正交矩阵的条件，并且
d
e
t
(
R
)
=
1
det(R)=1
det(R)=1
如果
d
e
t
(
R
)
<
0
det(R)<0
det(R)<0的时候选择第二大的值进行计算
T
r
(
M
D
)
=
d
1
M
11
+
d
2
M
22
+
d
3
M
33
Tr(MD) = d_{1} M_{11}+d_{2} M_{22}+d_{3} M_{33}
Tr(MD)=d1M11+d2M22+d3M33
中
M
11
=
M
22
=
1
，
M
33
=
−
1
M_{11}=M_{22}=1， M_{33}=-1
M11=M22=1，M33=−1
此时
R
=
U
C
V
T
R=U C V^{T}
R=UCVT
C
=
[
1
0
0
0
1
0
0
0
sign
(
det
(
U
V
T
)
)
⋅
1
]
C=\left[\begin{array}{cc} 1& 0 & 0 \\ 0& 1 & 0 \\ 0 & 0& \operatorname{sign}\left(\operatorname{det}\left(U V^{T}\right)\right) \cdot 1 \end{array}\right]
C=⎣⎡10001000sign(det(UVT))⋅1⎦⎤
2 PCL中ICP算法的实现
PCL中配准算法的标准流程，对于ICP来讲，输入是两团xyz点云，输出是source变换到target的刚体变换。其实核心求解过程都是上述公式，没啥可说的。在icp的实现里面默认是没有Reject bad correspondences这一步的
Iterative Closest Point Pipeline
- Search for correspondences（使用KdTree进行搜索最近点作为correspondent point）
- Reject bad correspondences（默认icp设置没有这一个）
- Estimate a transformation using the good correspondences（使用上述推导过程进行迭代式求解）
- Iterate（多次循环直到满足收敛条件）
关于上述推导公式的核心求解代码在pcl/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp文件下的estimateRigidTransformation函数中，具体实现为
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
Matrix4 &transformation_matrix) const
{
// Convert to Eigen format
const int npts = static_cast <int> (source_it.size ());
if (use_umeyama_)
{
Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src (3, npts);
Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt (3, npts);
for (int i = 0; i < npts; ++i)
{
cloud_src (0, i) = source_it->x;
cloud_src (1, i) = source_it->y;
cloud_src (2, i) = source_it->z;
++source_it;
cloud_tgt (0, i) = target_it->x;
cloud_tgt (1, i) = target_it->y;
cloud_tgt (2, i) = target_it->z;
++target_it;
}
// Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false);
}
else
{
source_it.reset (); target_it.reset ();
// <cloud_src,cloud_src> is the source dataset
transformation_matrix.setIdentity ();
Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (source_it, centroid_src);
compute3DCentroid (target_it, centroid_tgt);
source_it.reset (); target_it.reset ();
// Subtract the centroids from source, target
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
demeanPointCloud (source_it, centroid_src, cloud_src_demean);
demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);
getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
}
}
// 求解方式1:（默认求解方式)
//
template <typename Derived, typename OtherDerived>
typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
pcl::umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling)
{
typedef typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType;
typedef typename Eigen::internal::traits<TransformationMatrixType>::Scalar Scalar;
typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
typedef typename Derived::Index Index;
EIGEN_STATIC_ASSERT (!Eigen::NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL)
EIGEN_STATIC_ASSERT ((Eigen::internal::is_same<Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::value),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
enum { Dimension = PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC (Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;
typedef Eigen::Matrix<Scalar, Dimension, Dimension> MatrixType;
typedef typename Eigen::internal::plain_matrix_type_row_major<Derived>::type RowMajorMatrixType;
const Index m = src.rows (); // dimension
const Index n = src.cols (); // number of measurements
// required for demeaning ...
const RealScalar one_over_n = 1 / static_cast<RealScalar> (n);
// computation of mean
const VectorType src_mean = src.rowwise ().sum () * one_over_n;
const VectorType dst_mean = dst.rowwise ().sum () * one_over_n;
// demeaning of src and dst points
const RowMajorMatrixType src_demean = src.colwise () - src_mean;
const RowMajorMatrixType dst_demean = dst.colwise () - dst_mean;
// Eq. (36)-(37)
const Scalar src_var = src_demean.rowwise ().squaredNorm ().sum () * one_over_n;
// Eq. (38)
const MatrixType sigma (one_over_n * dst_demean * src_demean.transpose ());
Eigen::JacobiSVD<MatrixType> svd (sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
// Initialize the resulting transformation with an identity matrix...
TransformationMatrixType Rt = TransformationMatrixType::Identity (m + 1, m + 1);
// Eq. (39)
VectorType S = VectorType::Ones (m);
if (sigma.determinant () < 0)
S (m - 1) = -1;
// Eq. (40) and (43)
const VectorType& d = svd.singularValues ();
Index rank = 0;
for (Index i = 0; i < m; ++i)
if (!Eigen::internal::isMuchSmallerThan (d.coeff (i), d.coeff (0)))
++rank;
if (rank == m - 1)
{
//如果三个值相差很大，判断UV行列式是否为+
if (svd.matrixU ().determinant () * svd.matrixV ().determinant () > 0)
Rt.block (0, 0, m, m).noalias () = svd.matrixU () * svd.matrixV ().transpose ();
else
{
const Scalar s = S (m - 1);
S (m - 1) = -1;
Rt.block (0, 0, m, m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose ();
S (m - 1) = s;
}
}
else
{
Rt.block (0, 0, m, m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose ();
}
// Eq. (42)
if (with_scaling)
{
// Eq. (42)
const Scalar c = 1 / src_var * svd.singularValues ().dot (S);
// Eq. (41)
Rt.col (m).head (m) = dst_mean;
Rt.col (m).head (m).noalias () -= c * Rt.topLeftCorner (m, m) * src_mean;
Rt.block (0, 0, m, m) *= c;
}
else
{
Rt.col (m).head (m) = dst_mean;
Rt.col (m).head (m).noalias () -= Rt.topLeftCorner (m, m) * src_mean;
}
return (Rt);
}
// 求解方式2
///
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
const Eigen::Matrix<Scalar, 4, 1> ¢roid_src,
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
const Eigen::Matrix<Scalar, 4, 1> ¢roid_tgt,
Matrix4 &transformation_matrix) const
{
transformation_matrix.setIdentity ();
// Assemble the correlation matrix H = source * target'
Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3);
// Compute the Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU ();
Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV ();
// Compute R = V * U'
if (u.determinant () * v.determinant () < 0)
{
for (int x = 0; x < 3; ++x)
v (x, 2) *= -1;
}
Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose ();
// Return the correct transformation
transformation_matrix.topLeftCorner (3, 3) = R;
const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3));
transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.head (3) - Rc;
}
扫一扫
859
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
