Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
肥鼠路易
基于g2o的面特征优化
提交
37e988fd
基
基于g2o的面特征优化
项目概览
肥鼠路易
/
基于g2o的面特征优化
通知
1
Star
0
Fork
0
代码
文件
提交
分支
Tags
贡献者
分支图
Diff
Issue
29
列表
看板
标记
里程碑
合并请求
29
DevOps
流水线
流水线任务
计划
Wiki
0
Wiki
分析
仓库
DevOps
代码片段
项目成员
Pages
基
基于g2o的面特征优化
项目概览
项目概览
详情
发布
仓库
仓库
文件
提交
分支
标签
贡献者
分支图
比较
Issue
29
Issue
29
列表
看板
标记
里程碑
合并请求
29
合并请求
29
Pages
DevOps
DevOps
流水线
流水线任务
计划
分析
分析
仓库分析
DevOps
Wiki
0
Wiki
代码片段
代码片段
成员
成员
收起侧边栏
关闭侧边栏
动态
分支图
创建新Issue
流水线任务
提交
Issue看板
前往新版Gitcode,体验更适合开发者的 AI 搜索 >>
提交
37e988fd
编写于
6月 22, 2021
作者:
肥鼠路易
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
上传新文件
上级
74e344f0
变更
1
隐藏空白更改
内联
并排
Showing
1 changed file
with
231 addition
and
0 deletion
+231
-0
src/QuatPlane.cpp
src/QuatPlane.cpp
+231
-0
未找到文件。
src/QuatPlane.cpp
0 → 100644
浏览文件 @
37e988fd
#include "mylib/QuatPlane.h"
#include "mylib/g2o_opt_type.h"
/*
封装的含义:
1.对属性和方法进行封装
2.对属性和方法进行访问
类封装了成员变量和成员函数,使用指针的方法可以用->进行调用
*/
Eigen
::
Vector4d
QuatPlane
::
CreatePlanebyPoints
(
Eigen
::
Vector4d
pt1_
,
Eigen
::
Vector4d
pt2_
,
Eigen
::
Vector4d
pt3_
,
Eigen
::
Vector4d
pt4_
)
{
Eigen
::
Vector3d
pt1
=
pt1_
.
head
(
3
);
Eigen
::
Vector3d
pt2
=
pt2_
.
head
(
3
);
Eigen
::
Vector3d
pt3
=
pt3_
.
head
(
3
);
Eigen
::
Vector3d
pt4
=
pt4_
.
head
(
3
);
Eigen
::
Vector4d
plane_homo
;
plane_homo
.
head
(
3
)
=
(
pt1
-
pt3
).
cross
((
pt2
-
pt3
));
plane_homo
(
3
)
=
-
pt3
.
transpose
()
*
(
pt1
.
cross
(
pt2
));
Eigen
::
Vector3d
normal
;
double
dis
;
normal
=
plane_homo
.
head
(
3
)
/
plane_homo
.
head
(
3
).
norm
();
//.normalized();
dis
=
-
plane_homo
(
3
)
/
plane_homo
.
head
(
3
).
norm
();
Eigen
::
Vector4d
HessePlane
;
HessePlane
.
head
(
3
)
=
normal
;
HessePlane
(
3
)
=
dis
;
return
HessePlane
;
}
// popular Hesse notation formed by a unit normal vector n and the orthogonal distance to the origin d.
void
QuatPlane
::
HomogeneousToHesse
(
const
Eigen
::
Vector4d
&
homo_para
,
Eigen
::
Vector3d
&
norm
,
double
&
dis
){
Eigen
::
Vector3d
n
(
homo_para
[
0
],
homo_para
[
1
],
homo_para
[
2
]);
norm
[
0
]
=
homo_para
[
0
]
/
n
.
norm
();
norm
[
1
]
=
homo_para
[
1
]
/
n
.
norm
();
norm
[
2
]
=
homo_para
[
2
]
/
n
.
norm
();
dis
=
-
1
*
homo_para
[
3
]
/
n
.
norm
();
}
// 只要我们是标准化的,正规化的,我们就可以使用n_norm = 1.从Hesse形式转变为面的齐次形式
void
QuatPlane
::
HesseToHomogenous
(
const
Eigen
::
Vector3d
&
norm
,
const
double
&
dis
,
Eigen
::
Vector4d
&
homo_para
){
double
n_norm
=
1
;
norm
.
normalized
();
homo_para
[
0
]
=
norm
[
0
]
*
n_norm
;
homo_para
[
1
]
=
norm
[
1
]
*
n_norm
;
homo_para
[
2
]
=
norm
[
2
]
*
n_norm
;
homo_para
[
3
]
=
-
1
*
dis
*
n_norm
;
}
// 这里是平面的四元数表达法,只需要把Hesse形式进行一个小小的变形就可以得到单位四元数化的平面参数化形式,几何意义不明确
Eigen
::
Quaterniond
QuatPlane
::
HesseToQuaternion
(
Eigen
::
Vector3d
norm
,
double
d
)
{
Eigen
::
Quaterniond
quat
;
norm
.
normalize
();
quat
.
x
()
=
norm
[
0
];
quat
.
y
()
=
norm
[
1
];
quat
.
z
()
=
norm
[
2
];
quat
.
w
()
=
-
d
;
quat
.
normalize
();
return
quat
;
}
// 我们把Hesse单位法向量看成是一个单位圆球上的一点,可以把平面用球坐标参数化
Eigen
::
Vector3d
QuatPlane
::
SpherToHesse
(
const
double
&
phi_
,
const
double
&
theta
,
const
double
&
dis_s
){
EIGEN_USING_STD
(
sin
)
EIGEN_USING_STD
(
cos
)
dis_
=
dis_s
;
// [\cos \phi \cos \theta \quad \cos \phi \sin \theta \quad \sin \phi] \mathbf{P}_{\mathrm{f}}-d=0
normal_
[
0
]
=
cos
(
phi_
)
*
cos
(
theta
);
normal_
[
1
]
=
cos
(
phi_
)
*
sin
(
theta
);
normal_
[
2
]
=
sin
(
phi_
);
return
normal_
;
}
Eigen
::
Matrix4d
QuatPlane
::
Leftq
(
const
Eigen
::
Quaterniond
q
)
{
Eigen
::
Matrix4d
Rq_l
=
Eigen
::
Matrix4d
::
Zero
();
Rq_l
<<
q
.
w
(),
-
q
.
x
(),
-
q
.
y
(),
-
q
.
z
(),
q
.
x
(),
q
.
w
(),
-
q
.
z
(),
q
.
y
(),
q
.
y
(),
q
.
z
(),
q
.
w
(),
-
q
.
x
(),
q
.
z
(),
-
q
.
y
(),
q
.
x
(),
q
.
w
();
// Hamiltion
return
Rq_l
;
}
Eigen
::
Matrix4d
QuatPlane
::
Rightq
(
const
Eigen
::
Quaterniond
q
)
{
Eigen
::
Matrix4d
Rq_r
=
Eigen
::
Matrix4d
::
Zero
();
Rq_r
<<
q
.
w
(),
-
q
.
x
(),
-
q
.
y
(),
-
q
.
z
(),
q
.
x
(),
q
.
w
(),
-
q
.
z
(),
q
.
y
(),
q
.
y
(),
-
q
.
z
(),
q
.
w
(),
q
.
x
(),
q
.
z
(),
-
q
.
x
(),
-
q
.
x
(),
q
.
w
();
// Hamiltion qw,qx,qy,qz
return
Rq_r
;
}
Eigen
::
Quaterniond
QuatPlane
::
Conjugate
(
const
Eigen
::
Quaterniond
q
)
{
Eigen
::
Quaterniond
q_conj
;
q_conj
.
x
()
=
-
q
.
x
();
q_conj
.
y
()
=
-
q
.
y
();
q_conj
.
z
()
=
-
q
.
z
();
return
q_conj
;
}
const
double
SMALL_EPS
=
1e-5
;
Eigen
::
Quaterniond
QuatPlane
::
ExpUpDateQuat
(
const
Eigen
::
Vector3d
&
omg
,
const
Eigen
::
Quaterniond
q
)
{
Eigen
::
Quaterniond
res
;
double
theta
,
half_theta
;
theta
=
omg
.
norm
();
//M_PI / 6;
half_theta
=
0.5
*
theta
;
EIGEN_USING_STD
(
sin
)
EIGEN_USING_STD
(
cos
)
// if(theta<SMALL_EPS) TODO
if
(
theta
<
SMALL_EPS
)
{
res
.
vec
()
=
omg
;
res
.
w
()
=
1
;
}
else
{
res
.
vec
()
=
sin
(
half_theta
)
/
theta
*
omg
;
res
.
w
()
=
cos
(
half_theta
);
}
res
.
normalize
();
return
res
;
}
Eigen
::
Vector3d
QuatPlane
::
Rotation2Vector
(
Eigen
::
Matrix3d
R
)
{
double
tr
=
R
.
trace
();
Eigen
::
Vector3d
w
;
w
<<
0.5
*
(
R
(
2
,
1
)
-
R
(
1
,
2
)),
0.5
*
(
R
(
0
,
2
)
-
R
(
2
,
0
)),
0.5
*
(
R
(
1
,
0
)
-
R
(
0
,
1
));
double
cos_theta
=
(
tr
-
1
)
/
2
;
if
(
cos_theta
>
1
||
cos_theta
<
-
1
)
{
return
w
;
}
else
{
double
theta
=
acos
(
cos_theta
);
if
(
fabs
(
sin
(
theta
))
<
SMALL_EPS
)
{
return
w
;
}
else
{
return
w
/
sin
(
theta
)
*
theta
;
}
}
}
// inverse of the exponential map:log map
Eigen
::
Vector3d
QuatPlane
::
logQuat
(
Eigen
::
Quaterniond
q
)
{
/*
Eigen::Vector3d w;
Eigen::Vector3d q_vec = q.vec();
double q_norm = q.vec().norm();
double q_w = q.w();
if (q_norm < SMALL_EPS) {
w = q_vec;
} else {
w = 2 * acos(q_w) / q_norm * q_vec;
}
return w;*/
q
.
normalize
();
Eigen
::
Vector3d
w
;
double
q_norm
=
q
.
vec
().
norm
();
Eigen
::
Vector3d
q_vec
=
q
.
vec
();
double
q_w
=
q
.
w
();
if
(
q_norm
<
SMALL_EPS
)
{
w
=
q_vec
;
}
else
{
double
q_n_3
=
q_norm
*
q_norm
*
q_norm
;
double
q_w_2
=
q_w
*
q_w
;
w
=
q_vec
/
q_norm
*
(
1
-
1
/
3
*
q_n_3
/
q_w_2
);
//Taylor series
}
return
w
;
}
Eigen
::
Matrix3d
QuatPlane
::
JacobianR
(
const
Eigen
::
Vector3d
&
w
)
{
Eigen
::
Matrix3d
Jr
=
Eigen
::
Matrix3d
::
Identity
();
double
theta
=
w
.
norm
();
if
(
theta
<
0.00001
)
{
return
Jr
;
// = Matrix3d::Identity();
}
else
{
Eigen
::
Vector3d
k
=
w
.
normalized
();
// k - unit direction vector of w
Eigen
::
Matrix3d
K
=
g2o
::
hat
(
k
);
Jr
=
Eigen
::
Matrix3d
::
Identity
()
-
(
1
-
cos
(
theta
))
/
theta
*
K
+
(
1
-
sin
(
theta
)
/
theta
)
*
K
*
K
;
}
return
Jr
;
}
Eigen
::
Matrix3d
QuatPlane
::
JacobianRInv
(
const
Eigen
::
Vector3d
&
w
)
{
Eigen
::
Matrix3d
Jrinv
=
Eigen
::
Matrix3d
::
Identity
();
double
theta
=
w
.
norm
();
// very small angle
if
(
theta
<
0.00001
)
{
return
Jrinv
;
}
else
{
Eigen
::
Vector3d
k
=
w
.
normalized
();
// k - unit direction vector of w
Eigen
::
Matrix3d
K
=
g2o
::
hat
(
k
);
Jrinv
=
Eigen
::
Matrix3d
::
Identity
()
+
0.5
*
g2o
::
hat
(
w
)
+
(
1.0
-
(
1.0
+
cos
(
theta
))
*
theta
/
(
2.0
*
sin
(
theta
))
)
*
K
*
K
;
}
return
Jrinv
;
}
// left jacobian inverse
Eigen
::
Matrix3d
QuatPlane
::
JacobianLInv
(
const
Eigen
::
Vector3d
&
w
)
{
Eigen
::
Matrix3d
JlInv_old
=
JacobianRInv
(
-
w
);
// original version
Eigen
::
Matrix3d
JlInv
=
Eigen
::
Matrix3d
::
Identity
();
double
theta
=
w
.
norm
();
if
(
theta
<
0.00001
)
{
return
JlInv
;
}
else
{
Eigen
::
Vector3d
k
=
w
.
normalized
();
// k - unit direction vector of w
Eigen
::
Matrix3d
K_hat
=
g2o
::
hat
(
k
);
double
half_theta
=
theta
/
2
;
double
cot_half_theta
=
cos
(
half_theta
)
/
sin
(
half_theta
);
JlInv
=
half_theta
*
cot_half_theta
*
Eigen
::
Matrix3d
::
Identity
()
+
(
1
-
half_theta
*
cot_half_theta
)
*
k
*
k
.
transpose
()
-
half_theta
*
K_hat
;
}
return
JlInv
;
}
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录