Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
肥鼠路易
基于g2o的面特征优化
提交
49498095
基
基于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 搜索 >>
提交
49498095
编写于
6月 22, 2021
作者:
肥鼠路易
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
上传新文件
上级
ecd93d52
变更
1
隐藏空白更改
内联
并排
Showing
1 changed file
with
108 addition
and
0 deletion
+108
-0
src/g2o_opt_type.cpp
src/g2o_opt_type.cpp
+108
-0
未找到文件。
src/g2o_opt_type.cpp
0 → 100644
浏览文件 @
49498095
/*create by user 18/06/2021 Jacobian error*/
#include "mylib/g2o_opt_type.h"
const
double
SMALL_EPS
=
1e-5
;
/**
* \brief Plane vertex, omg(XYZ)
*/
namespace
g2o
{
// virtual bool read(std::istream &is);
// virtual bool write(std::ostream &os) const;
void
VertexSBAPlaneomg
::
setToOriginImpl
()
{
_estimate
.
fill
(
0.1
);
}
void
VertexSBAPlaneomg
::
oplusImpl
(
const
double
*
update
)
{
//Eigen::Map<const Vector3d> omg(update);
Eigen
::
Vector3d
omg
;
omg
<<
update
[
0
],
update
[
1
],
update
[
2
];
/* double theta, half_theta;
theta = omg.norm();//M_PI / 6;
omg = omg/theta;
half_theta = 0.5 * theta;
EIGEN_USING_STD(sin)
EIGEN_USING_STD(cos)*/
// if(theta<SMALL_EPS) TODO
Eigen
::
Quaterniond
res
;
Eigen
::
Quaterniond
q
(
1
,
0
,
0
,
0
),
q_tmp
;
q_tmp
=
QuatPlane
::
ExpUpDateQuat
(
_estimate
,
q
);
res
=
QuatPlane
::
ExpUpDateQuat
(
omg
,
q_tmp
);
Eigen
::
Vector3d
v
=
QuatPlane
::
logQuat
(
res
);
_estimate
+=
v
;
}
void
EdgeSE3ProjectPlane
::
computeError
()
{
const
VertexSE3Expmap
*
vj
=
static_cast
<
const
VertexSE3Expmap
*>
(
_vertices
[
1
]);
g2o
::
SE3Quat
T
(
vj
->
estimate
());
Eigen
::
Matrix4d
T_w_c
=
T
.
to_homogeneous_matrix
().
transpose
();
const
VertexSBAPlaneomg
*
vi
=
static_cast
<
const
VertexSBAPlaneomg
*>
(
_vertices
[
0
]);
Eigen
::
Vector3d
opt_plane
=
vi
->
estimate
();
Eigen
::
Quaterniond
q
(
1
,
0
,
0
,
0
),
qt
;
double
dism
=
_measurement
.
norm
();
Eigen
::
Vector3d
normalm
=
_measurement
/
dism
;
Eigen
::
Vector4d
vec4
;
QuatPlane
::
HesseToHomogenous
(
normalm
,
dism
,
vec4
);
Eigen
::
Vector4d
vec4_c
;
vec4_c
=
T_w_c
*
vec4
;
qt
.
vec
()
=
vec4_c
.
head
(
3
);
qt
.
w
()
=
vec4_c
(
3
);
qt
.
normalize
();
Eigen
::
Vector3d
obs_plane_w
=
QuatPlane
::
logQuat
(
qt
);
Eigen
::
Quaterniond
structure_err
=
QuatPlane
::
ExpUpDateQuat
(
obs_plane_w
.
normalized
(),
q
)
*
QuatPlane
::
ExpUpDateQuat
(
opt_plane
.
normalized
(),
q
).
inverse
();
structure_err
.
normalize
();
_error
=
QuatPlane
::
logQuat
(
structure_err
);
std
::
cout
<<
"error is : "
<<
_error
.
transpose
()
<<
std
::
endl
;
/*****************************************************************************/
}
void
EdgeSE3ProjectPlane
::
linearizeOplus
()
{
VertexSE3Expmap
*
vj
=
static_cast
<
VertexSE3Expmap
*>
(
_vertices
[
1
]);
SE3Quat
T
(
vj
->
estimate
());
VertexSBAPlaneomg
*
vi
=
static_cast
<
VertexSBAPlaneomg
*>
(
_vertices
[
0
]);
Eigen
::
Vector3d
plane
=
vi
->
estimate
();
Eigen
::
Quaterniond
q
(
1
,
0
,
0
,
0
);
Eigen
::
Quaterniond
plane_obs_c
=
QuatPlane
::
ExpUpDateQuat
(
_measurement
,
q
).
normalized
();
// the observation
Eigen
::
Vector4d
homo_c_4d
;
homo_c_4d
<<
plane_obs_c
.
x
()
,
plane_obs_c
.
y
()
,
plane_obs_c
.
z
()
,
plane_obs_c
.
w
();
Eigen
::
Vector4d
homo_w_4d
=
T
.
to_homogeneous_matrix
().
transpose
()
*
homo_c_4d
;
Eigen
::
Vector4d
q_y
=
homo_w_4d
/
homo_w_4d
.
norm
();
Eigen
::
Quaterniond
plane_obs_w
(
q_y
[
3
]
,
q_y
[
0
]
,
q_y
[
1
]
,
q_y
[
2
]);
Eigen
::
Quaterniond
plane_opt_w
=
QuatPlane
::
ExpUpDateQuat
(
plane
,
q
);
Eigen
::
Quaterniond
structure_error
=
plane_obs_w
*
plane_opt_w
.
inverse
();
Eigen
::
Vector3d
u
=
QuatPlane
::
logQuat
(
structure_error
);
Eigen
::
Matrix3d
jacobian_E_u
=
QuatPlane
::
JacobianLInv
(
u
);
Eigen
::
Matrix
<
double
,
3
,
4
>
jacobian_u_phi
;
double
q_n
=
plane_obs_w
.
norm
();
double
q_n_3
=
q_n
*
q_n
*
q_n
;
jacobian_u_phi
.
block
<
3
,
3
>
(
0
,
0
)
=
2
*
acos
(
plane_obs_w
.
w
())
*
(
Eigen
::
Matrix3d
::
Identity
()
/
plane_obs_w
.
vec
().
norm
()
-
plane_obs_w
.
vec
()
*
plane_obs_w
.
vec
().
transpose
()
/
sqrt
(
q_n_3
));
jacobian_u_phi
.
block
<
3
,
1
>
(
0
,
3
)
=
-
1
*
2.0
/
(
plane_obs_w
.
vec
().
norm
()
*
sqrt
(
1.0
-
plane_obs_w
.
w
()
*
plane_obs_w
.
w
()
+
0.0001
))
*
plane_obs_w
.
vec
();
double
q_y_n
=
q_y
.
norm
();
double
q_y_n3
=
q_y_n
*
q_y_n
*
q_y_n
;
Eigen
::
Matrix4d
jacobian_phi_y
=
Eigen
::
Matrix4d
::
Identity
()
/
q_y
.
norm
()
-
q_y
*
q_y
.
transpose
()
/
sqrt
(
q_y_n3
);
Eigen
::
Matrix
<
double
,
4
,
6
>
jacobian_y_pose
=
Eigen
::
Matrix
<
double
,
4
,
6
>::
Zero
();
jacobian_y_pose
.
block
<
3
,
3
>
(
0
,
0
)
=
T
.
rotation
().
toRotationMatrix
().
transpose
()
*
g2o
::
hat
(
plane_obs_w
.
vec
());
jacobian_y_pose
.
block
<
1
,
3
>
(
3
,
0
)
=
T
.
translation
().
transpose
()
*
g2o
::
hat
(
plane_obs_w
.
vec
());
jacobian_y_pose
.
block
<
1
,
3
>
(
3
,
3
)
=
plane_obs_w
.
vec
().
transpose
();
_jacobianOplusXi
=
QuatPlane
::
JacobianRInv
(
u
);
_jacobianOplusXj
=
jacobian_E_u
*
jacobian_u_phi
*
jacobian_phi_y
*
jacobian_y_pose
;
// 3 x 6 matrix
}
Eigen
::
Matrix3d
hat
(
const
Eigen
::
Vector3d
&
v
)
{
Eigen
::
Matrix3d
m
;
m
<<
0.0
,
-
v
[
2
],
v
[
1
],
v
[
2
],
0.0
,
-
v
[
0
],
-
v
[
1
],
v
[
0
],
0.0
;
return
m
;
}
};
\ No newline at end of file
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录