Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
za0612
pcl
提交
380ff1bd
P
pcl
项目概览
za0612
/
pcl
与 Fork 源项目一致
从无法访问的项目Fork
通知
13
Star
0
Fork
0
代码
文件
提交
分支
Tags
贡献者
分支图
Diff
Issue
0
列表
看板
标记
里程碑
合并请求
0
Wiki
0
Wiki
分析
仓库
DevOps
项目成员
Pages
P
pcl
项目概览
项目概览
详情
发布
仓库
仓库
文件
提交
分支
标签
贡献者
分支图
比较
Issue
0
Issue
0
列表
看板
标记
里程碑
合并请求
0
合并请求
0
Pages
分析
分析
仓库分析
DevOps
Wiki
0
Wiki
成员
成员
收起侧边栏
关闭侧边栏
动态
分支图
创建新Issue
提交
Issue看板
前往新版Gitcode,体验更适合开发者的 AI 搜索 >>
提交
380ff1bd
编写于
3月 13, 2014
作者:
V
Victor Lamoine
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Add threePlanesIntersection
上级
4c1675c4
变更
3
隐藏空白更改
内联
并排
Showing
3 changed file
with
104 addition
and
0 deletion
+104
-0
common/include/pcl/common/intersections.h
common/include/pcl/common/intersections.h
+20
-0
common/src/intersections.cpp
common/src/intersections.cpp
+45
-0
test/common/test_plane_intersection.cpp
test/common/test_plane_intersection.cpp
+39
-0
未找到文件。
common/include/pcl/common/intersections.h
浏览文件 @
380ff1bd
...
...
@@ -86,7 +86,27 @@ namespace pcl
const
Eigen
::
Vector4f
&
fplane_b
,
Eigen
::
VectorXf
&
line
,
double
angular_tolerance
=
0.1
);
/** \brief Determine the point of intersection of three non-parallel planes by solving the equations.
* \note If using nearly parralel planes you can lower the determinant_tolerance value. This can
* lead to inconsistent results.
* If the three planes intersects in a line the point will be anywhere on the line.
* \param[in] plane_a are the coefficients of the first plane in the form ax + by + cz + d = 0
* \param[in] plane_b are the coefficients of the second plane
* \param[in] plane_c are the coefficients of the third plane
* \param[in] determinant_tolerance is a limit to determine whether planes are parallel or not
* \param[out] intersection_point, the three coordinates x, y, z of the intersection point
* \return true if succeeded/planes aren't parallel
*/
PCL_EXPORTS
bool
threePlanesIntersection
(
const
Eigen
::
Vector4f
&
plane_a
,
const
Eigen
::
Vector4f
&
plane_b
,
const
Eigen
::
Vector4f
&
plane_c
,
Eigen
::
Vector3f
&
intersection_point
,
double
determinant_tolerance
=
1e-6
);
}
/*@}*/
#endif //#ifndef PCL_INTERSECTIONS_H_
common/src/intersections.cpp
浏览文件 @
380ff1bd
...
...
@@ -113,3 +113,48 @@ pcl::planeWithPlaneIntersection (const Eigen::Vector4f &plane_a,
line
[
5
]
=
line_direction
[
2
];
return
true
;
}
bool
pcl
::
threePlanesIntersection
(
const
Eigen
::
Vector4f
&
plane_a
,
const
Eigen
::
Vector4f
&
plane_b
,
const
Eigen
::
Vector4f
&
plane_c
,
Eigen
::
Vector3f
&
intersection_point
,
double
determinant_tolerance
)
{
// Check if some planes are parallel
Eigen
::
Matrix3f
normals_in_lines
;
for
(
int
i
=
0
;
i
<
3
;
i
++
)
{
normals_in_lines
(
i
,
0
)
=
plane_a
[
i
];
normals_in_lines
(
i
,
1
)
=
plane_b
[
i
];
normals_in_lines
(
i
,
2
)
=
plane_c
[
i
];
}
double
determinant
=
normals_in_lines
.
determinant
();
if
(
fabs
(
determinant
)
<
determinant_tolerance
)
{
// det ~= 0
PCL_ERROR
(
"Two or more planes are parralel.
\n
"
);
return
(
false
);
}
// Left part of the 3 equations
Eigen
::
Matrix3f
left_member
;
for
(
int
i
=
0
;
i
<
3
;
i
++
)
{
left_member
(
0
,
i
)
=
plane_a
[
i
];
left_member
(
1
,
i
)
=
plane_b
[
i
];
left_member
(
2
,
i
)
=
plane_c
[
i
];
}
// Right side of the 3 equations
Eigen
::
Vector3f
right_member
;
right_member
<<
-
plane_a
[
3
],
-
plane_b
[
3
],
-
plane_c
[
3
];
// Solve the system
intersection_point
=
left_member
.
fullPivLu
().
solve
(
right_member
);
return
(
true
);
}
test/common/test_plane_intersection.cpp
浏览文件 @
380ff1bd
...
...
@@ -301,6 +301,44 @@ TEST (PCL, planeWithPlaneIntersection)
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST
(
PCL
,
threePlanesIntersection
)
{
// Testing 2 parallel planes
Eigen
::
Vector4f
plane_a
(
1.0
f
,
0.0
f
,
0.0
f
,
-
0.5
f
);
Eigen
::
Vector4f
plane_b
(
1.0
f
,
0.0
f
,
0.0
f
,
0.5
f
);
Eigen
::
Vector4f
plane_c
(
0.0
f
,
0.0
f
,
1.0
f
,
-
0.5
f
);
Eigen
::
Vector3f
point
(
1.0
f
,
2.0
f
,
3.0
f
);
std
::
cout
<<
std
::
endl
;
EXPECT_FALSE
(
threePlanesIntersection
(
plane_a
,
plane_b
,
plane_c
,
point
,
1e-6
));
std
::
cout
<<
std
::
endl
;
EXPECT_FALSE
(
threePlanesIntersection
(
plane_a
,
plane_b
,
plane_c
,
point
,
1e-3
));
EXPECT_FLOAT_EQ
(
1.0
f
,
point
(
0
));
EXPECT_FLOAT_EQ
(
2.0
f
,
point
(
1
));
EXPECT_FLOAT_EQ
(
3.0
f
,
point
(
2
));
//perfect box
plane_b
<<
0.0
f
,
1.0
f
,
0.0
f
,
0.5
f
;
std
::
cout
<<
std
::
endl
;
EXPECT_TRUE
(
threePlanesIntersection
(
plane_a
,
plane_b
,
plane_c
,
point
));
EXPECT_FLOAT_EQ
(
0.5
f
,
point
(
0
));
EXPECT_FLOAT_EQ
(
-
0.5
f
,
point
(
1
));
EXPECT_FLOAT_EQ
(
0.5
f
,
point
(
2
));
//general planes
plane_a
<<
1.4564
f
,
0.5465
f
,
-
0.1325
f
,
0.4685
f
;
plane_b
<<
-
1.5619
f
,
5.5461
f
,
5.4569
f
,
2.9414
f
;
plane_c
<<
0.9852
f
,
654.55
f
,
-
0.1546
f
,
-
45.1516
f
;
std
::
cout
<<
std
::
endl
;
EXPECT_TRUE
(
threePlanesIntersection
(
plane_a
,
plane_b
,
plane_c
,
point
));
EXPECT_NEAR
(
-
0.413977
f
,
point
(
0
),
1e-4
);
EXPECT_NEAR
(
0.0694323
f
,
point
(
1
),
1e-4
);
EXPECT_NEAR
(
-
0.728082
f
,
point
(
2
),
1e-4
);
}
//* ---[ */
int
main
(
int
argc
,
char
**
argv
)
...
...
@@ -310,3 +348,4 @@ main (int argc, char** argv)
}
/* ]--- */
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录