Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
肥鼠路易
基于MyG2O的例子实现
提交
abdca323
基
基于MyG2O的例子实现
项目概览
肥鼠路易
/
基于MyG2O的例子实现
通知
1
Star
0
Fork
0
代码
文件
提交
分支
Tags
贡献者
分支图
Diff
Issue
29
列表
看板
标记
里程碑
合并请求
29
DevOps
流水线
流水线任务
计划
Wiki
0
Wiki
分析
仓库
DevOps
代码片段
项目成员
Pages
基
基于MyG2O的例子实现
项目概览
项目概览
详情
发布
仓库
仓库
文件
提交
分支
标签
贡献者
分支图
比较
Issue
29
Issue
29
列表
看板
标记
里程碑
合并请求
29
合并请求
29
Pages
DevOps
DevOps
流水线
流水线任务
计划
分析
分析
仓库分析
DevOps
Wiki
0
Wiki
代码片段
代码片段
成员
成员
收起侧边栏
关闭侧边栏
动态
分支图
创建新Issue
流水线任务
提交
Issue看板
前往新版Gitcode,体验更适合开发者的 AI 搜索 >>
提交
abdca323
编写于
6月 08, 2021
作者:
肥鼠路易
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
上传新文件
上级
872fffdc
变更
1
隐藏空白更改
内联
并排
Showing
1 changed file
with
231 addition
and
0 deletion
+231
-0
include/g2o_opt_type.h
include/g2o_opt_type.h
+231
-0
未找到文件。
include/g2o_opt_type.h
0 → 100644
浏览文件 @
abdca323
#ifndef G2O_OPT_TYPE_H_
#define G2O_OPT_TYPE_H_
#include "../Thirdparty/g2o/g2o/core/base_vertex.h"
#include "../Thirdparty/g2o/g2o/types/types_sba.h"
#include "../Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
#include "../Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "../Thirdparty/g2o/g2o/core/sparse_optimizer.h"
#include "../Thirdparty/g2o/g2o/core/block_solver.h"
#include "../Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h"
class
QuatPlane
{
public:
Eigen
::
Quaterniond
_plane
;
Eigen
::
Vector3d
_normal
;
double
_dis
;
public:
QuatPlane
(
const
Eigen
::
Vector4d
&
pl
)
{
double
norm
=
pl
.
head
(
3
).
norm
();
_normal
=
pl
.
head
(
3
)
/
norm
;
_dis
=
-
pl
[
3
]
/
norm
;
_plane
=
normAndDToQuat
(
_dis
,
_normal
);
// _plane = Eigen::Quaterniond(_dis, _normal[0], _normal[1], _normal[2]);
_plane
.
normalize
();
}
QuatPlane
()
{
_plane
=
Eigen
::
Quaterniond
(
1.0
,
0.0
,
0.0
,
0.0
);
}
QuatPlane
(
const
Eigen
::
Quaterniond
&
q_
)
:
_plane
(
q_
)
{
_dis
=
q_
.
w
()
/
std
::
sqrt
(
1
-
q_
.
w
()
*
q_
.
w
());
_normal
=
q_
.
vec
()
*
std
::
sqrt
(
1
+
_dis
*
_dis
);
}
inline
Eigen
::
Vector3d
plane3d
()
const
{
return
_normal
*
_dis
;
}
QuatPlane
operator
*
(
const
Eigen
::
Quaterniond
q_
)
const
{
Eigen
::
Quaterniond
est_pt
=
this
->
_plane
;
Eigen
::
Matrix4d
m
;
m
<<
est_pt
.
w
(),
est_pt
.
z
(),
-
est_pt
.
y
(),
est_pt
.
x
(),
-
est_pt
.
z
(),
est_pt
.
w
(),
-
est_pt
.
x
(),
est_pt
.
y
(),
est_pt
.
y
(),
-
est_pt
.
x
(),
est_pt
.
w
(),
est_pt
.
z
(),
-
est_pt
.
x
(),
-
est_pt
.
y
(),
-
est_pt
.
z
(),
est_pt
.
w
();
/* m << est_pt.w(), -est_pt.z(), est_pt.y(), est_pt.x(),
est_pt.z(), est_pt.w(), -est_pt.x(), est_pt.y(),
-est_pt.y(), est_pt.x(), est_pt.w(), est_pt.z(),
-est_pt.x(), -est_pt.y(), -est_pt.z(), est_pt.w();*/
Eigen
::
Vector4d
update_to_vector
=
Eigen
::
Vector4d
(
q_
.
x
(),
q_
.
y
(),
q_
.
z
(),
q_
.
w
());
Eigen
::
Vector4d
v
=
m
*
update_to_vector
;
Eigen
::
Quaterniond
q
=
Eigen
::
Quaterniond
(
v
[
3
],
v
[
0
],
v
[
1
],
v
[
2
]);
q
.
normalize
();
return
QuatPlane
(
q
);
}
QuatPlane
&
operator
*=
(
const
Eigen
::
Quaterniond
q_
)
{
QuatPlane
est_pt
(
*
this
);
Eigen
::
Matrix4d
m
;
/*
m << q_.w(), -q_.z(), q_.y(), q_.x(),
q_.z(), q_.w(), -q_.x(), q_.y(),
-q_.y(), q_.x(), q_.w(), q_.z(),
-q_.x(), -q_.y(), -q_.z(), q_.w(); //left multi
*/
m
<<
q_
.
w
(),
q_
.
z
(),
-
q_
.
y
(),
q_
.
x
(),
-
q_
.
z
(),
q_
.
w
(),
q_
.
x
(),
q_
.
y
(),
q_
.
y
(),
-
q_
.
x
(),
q_
.
w
(),
q_
.
z
(),
-
q_
.
x
(),
-
q_
.
y
(),
-
q_
.
z
(),
q_
.
w
();
Eigen
::
Vector4d
update_to_vector
=
Eigen
::
Vector4d
(
q_
.
x
(),
q_
.
y
(),
q_
.
z
(),
q_
.
w
());
Eigen
::
Vector4d
v
=
m
*
update_to_vector
;
_plane
=
Eigen
::
Quaterniond
(
v
[
3
],
v
[
0
],
v
[
1
],
v
[
2
]);
return
*
this
;
}
Eigen
::
Quaterniond
normAndDToQuat
(
double
d
,
Eigen
::
Vector3d
norm
)
{
Eigen
::
Quaterniond
res
;
norm
.
normalize
();
res
.
x
()
=
norm
[
0
];
res
.
y
()
=
norm
[
1
];
res
.
z
()
=
norm
[
2
];
res
.
w
()
=
-
d
;
res
.
normalize
();
// Fun::normalizeAndUnify(res);
return
res
;
}
Eigen
::
Vector4d
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
;
return
homo_para
;
}
Eigen
::
Quaterniond
HomogeneousToQuaternion
(
const
Eigen
::
Vector4d
&
homo_para
){
Eigen
::
Vector4d
v
=
homo_para
/
homo_para
.
norm
();
Eigen
::
Quaterniond
q
(
v
[
3
],
v
[
0
],
v
[
1
],
v
[
2
]);
return
q
;
}
public:
// Jr, right jacobian of SO(3)
static
Eigen
::
Matrix3d
JacobianR
(
const
Eigen
::
Vector3d
&
w
);
// Jr^(-1)
static
Eigen
::
Matrix3d
JacobianRInv
(
const
Eigen
::
Vector3d
&
w
);
// Jl, left jacobian of SO(3), Jl(x) = Jr(-x)
static
Eigen
::
Matrix3d
JacobianL
(
const
Eigen
::
Vector3d
&
w
);
// Jl^(-1)
static
Eigen
::
Matrix3d
JacobianLInv
(
const
Eigen
::
Vector3d
&
w
);
static
Eigen
::
Vector3d
log
(
const
QuatPlane
&
QuatPlane
);
static
Eigen
::
Vector3d
logAndTheta
(
const
QuatPlane
&
QuatPlane
,
double
*
theta
);
static
QuatPlane
exp
(
const
Eigen
::
Vector3d
&
omega
);
static
QuatPlane
expAndTheta
(
const
Eigen
::
Vector3d
&
omega
,
double
*
theta
);
//static Eigen::Vector3d log_map(const QuatPlane q);
};
namespace
g2o
{
class
VertexQuatPlane
:
public
g2o
::
BaseVertex
<
3
,
QuatPlane
>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
;
VertexQuatPlane
()
{
}
virtual
bool
read
(
std
::
istream
&
is
)
{
}
virtual
bool
write
(
std
::
ostream
&
os
)
const
{
}
virtual
void
setToOriginImpl
()
{
_estimate
=
QuatPlane
();
}
virtual
void
oplusImpl
(
const
double
*
update_
)
{
/*
Eigen::Vector4d v;
v << 0.5 * update_[0], 0.5 * update_[1], 0.5 * update_[2], 1.0f;
Eigen::Quaterniond q(v);
QuatPlane estimation(estimate()._plane * q);
setEstimate(estimation);*/
//std::cout << "update in " << std::endl;
Eigen
::
Map
<
const
Eigen
::
Vector3d
>
update
(
update_
);
//std::cout << "update plane: " << update << std::endl;
QuatPlane
tmp_1
=
QuatPlane
::
exp
(
update
/
update
.
norm
());
// Fun::normalizeAndUnify(tmp_q);
QuatPlane
estimation
(
tmp_1
.
_plane
*
estimate
().
_plane
);
setEstimate
(
estimation
);
//std::cout << "update out " << std::endl;
}
};
class
EdgeSE3ProjectPlane
:
public
BaseBinaryEdge
<
3
,
QuatPlane
,
VertexQuatPlane
,
g2o
::
VertexSE3Expmap
>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
;
EdgeSE3ProjectPlane
()
{
}
bool
read
(
std
::
istream
&
is
)
{
}
bool
write
(
std
::
ostream
&
os
)
const
{
}
virtual
void
computeError
();
virtual
void
linearizeOplus
();
//Vector2d cam_project(const Vector3d & pc) const;
Eigen
::
Matrix3d
skew
(
const
Eigen
::
Vector3d
&
v
)
const
;
public:
double
_fx
;
double
_fy
;
double
_cx
;
double
_cy
;
};
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
;
}
}
#endif
\ No newline at end of file
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录