# 3D Pose Estimation
This project features an object recognition pipeline to recognize and localize objects in a scene based on a variety of local features. Point clouds are given in the PCD format. By modularizing different techniques for
* Point cloud manipulation
* Feature description
* Keypoint extraction
* Transformation estimation
* Pose refinement
* Hypothesis verification,
the project aims at providing an experimentation platform that allows for fast evaluation of different 3d recognition methods.
![PC Model Pose Estimation](./documentation/pose-estimation.png)
## Requirements
* PCL >=1.8 built with C++11 support (requires VTK >=6.0.1, Eigen ~3.2.0)
* CMake >=3.1
* GCC >=4.9
* BOOST ~1.54
* NLopt
Additionally, for Unit Testing, CppUnit is required.
## Usage
`$ ./PoseEstimation (--folder foldername)|(model1.pcd model2.pcd ...) scene.pcd`
Find instances of a single model or a collection of models in a scene. Using the `--folder` CLI argument, a directory containing .pcd files representing the model candidates can be provided. Note that only the first .pcd file will be used as the input cloud for the optimization step to find good pose estimation parameters before the actual pipeline is executed on all models from the collection.
For each input model, every scene object is tested for descriptor correspondences. If the transformation estimation succeeds between an input model and a scene object, the system outputs the transformation matrix to transform the input model to the scene object, and the average of the correspondence distances ("uncertainty", between 0 and 1, the lower the more confident is the matching) between the two point clouds.
### Example
`$ ./PoseEstimation --folder objects book.pcd`
Load point clouds from directory "objects" (which contains 3 models) and find the best match with the scene point cloud.
The system prints out possible instances for each of those 3 models by showing the transformation matrices and uncertainties for each of the candidates.
#### Model 1
```
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Matching results for point cloud objects/book.pcd:
% Uncertainty: 0.343348
% Verified transformation(s):
0.999768 0.00828556 -0.0198561 0.514141
-0.00852746 0.99989 -0.0121292 0.0103949
0.0197534 0.0122957 0.999729 -0.000965119
0 0 0 1
Matching objects/book.pcd finished in 00:00:03.868636
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
```
#### Model 2
```
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Matching results for point cloud objects/mustard_centered.pcd:
% Uncertainty: 0.597509
% Verified transformation(s):
-0.726818 0.282834 0.625891 0.522188
-0.674479 -0.121883 -0.728164 0.0735832
-0.129664 -0.951393 0.279353 0.586207
0 0 0 1
Matching objects/mustard_centered.pcd finished in 00:00:00.669852
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
```
#### Model 3
```
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Matching results for point cloud objects/book2.pcd:
% Uncertainty: 0.460474
% Verified transformation(s):
0.999502 0.00724483 -0.0307228 0.519118
-0.00631464 0.999522 0.0302664 0.0159141
0.0309274 -0.0300573 0.99907 0.0193727
0 0 0 1
Matching objects/book2.pcd finished in 00:00:02.328845
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
```
#### Result
Since Model 1 had the lowest uncertainty, it is presented as the best matching candidate:
```
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The best matching point cloud is "objects/book.pcd" with an uncertainty of 0.343348.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
```
After computing possible transformations, the scene point cloud and all model point clouds (translated by 0.5 units in x, see [main.cpp:116](./main.cpp#L116)) are visualized. The blue square-shaped points represent keypoints and the lines between them symbolize the descriptor correspondences. The model transformation candidates are displayed in different colors with some transparency. Successful transformation instances which were validated by the Hypothesis Verification step are highlighted in lime green.
![PC Model Pose Estimation](./documentation/pose-estimation2.png)
## Configuration
The system can be configured via its command line interface. Issue `$ PoseEstimation -h` to get a descriptive overview of all the available CLI parameters (similar to the table below).
A more comfortable way of setting the parameters is by providing a JSON file. The system stores such a file on each run and will read from the same file again to obtain the current settings. Configuration parameters are stored in a modular fashion following the system's ensemble of components, e.g. transformation estimators, feature descriptors, etc.
The following parameters are available:
| Parameter Name | Description | Type, Default value, Constraints |
| -------------- | ----------- | -------------------------------- |
| **Downsampler** | | |
| **uniformdown** | [Downsampling using uniform filtering](http://docs.pointclouds.org/trunk/classpcl_1_1_uniform_sampling.html) | |
| `uniformdown_size` | Sample size | ([float] 1) constraints: (>= 1), (<= 10) |
| **voxelgrid** | [Downsampling using Voxel grid filtering](http://docs.pointclouds.org/trunk/classpcl_1_1_voxel_grid.html) | |
| `voxelgrid_size` | Leaf size of the voxel grid | ([float] 3) constraints: (>= 1), (<= 10) |
| **KeypointExtractor** | | |
| **uniform** | [Keypoint extraction using Uniform Sampling](http://docs.pointclouds.org/trunk/classpcl_1_1_uniform_sampling.html) | |
| `uniform_r` | Search radius for the uniform keypoint extraction | ([float] 3.294) |
| **iss** | [Keypoint extraction using Intrinsic Shape Signatures (ISS)](http://docs.pointclouds.org/trunk/classpcl_1_1_i_s_s_keypoint3_d.html) | |
| `iss_threads` | Number of threads to use for ISS keypoint extraction | ([int] 4) |
| `iss_nn` | Minimum number of neighbors to consider for ISS keypoint extraction | ([float] 3) |
| `iss_thresh21` | ISS Threshold 21 | ([float] 0.975) |
| `iss_thresh32` | ISS Threshold 32 | ([float] 0.975) |
| `iss_salient_r` | Salient radius for ISS keypoint extraction | ([float] 3) |
| `iss_nonmax_r` | Non maxima suppression radius for ISS keypoint extraction | ([float] 8) |
| **FeatureDescriptor** | | |
| **USC** | [Feature description using Unique Shape Context (USC)](http://docs.pointclouds.org/trunk/classpcl_1_1_unique_shape_context.html) | |
| `USC_search_r` | Search radius for finding neighbors | ([float] 25) constraints: (> pc_normal_nn) |
| `USC_min_r` | Minimum radius of the search sphere | ([float] 5) |
| `USC_density_r` | Points within this radius are used to calculate the local point density | ([float] 10) |
| `USC_LRF_r` | Local Reference Frame (LRF) radius of USC descriptor | ([float] 25) |
| **SI** | [Feature description using Spin Images (SI)](http://docs.pointclouds.org/trunk/classpcl_1_1_spin_image_estimation.html) | |
| `SI_search_r` | Search radius for finding neighbors | ([float] 5) constraints: (> pc_normal_nn) |
| **RSD** | [Feature description using Radius-based Surface Descriptor (RSD)](http://docs.pointclouds.or
没有合适的资源?快使用搜索试试~ 我知道了~
3D姿态估计-基于物体点云实现的物体3D姿态估计-附项目源码+流程教程-优质项目实战.zip
共48个文件
hpp:35个
cpp:5个
h:5个
1.该资源内容由用户上传,如若侵权请联系客服进行举报
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
版权申诉
0 下载量 121 浏览量
2024-05-15
13:58:16
上传
评论
收藏 118KB ZIP 举报
温馨提示
3D姿态估计_基于物体点云实现的物体3D姿态估计_附项目源码+流程教程_优质项目实战
资源推荐
资源详情
资源评论
收起资源包目录
3D姿态估计_基于物体点云实现的物体3D姿态估计_附项目源码+流程教程_优质项目实战.zip (48个子文件)
3D姿态估计_基于物体点云实现的物体3D姿态估计_附项目源码+流程教程_优质项目实战
featuredescription.hpp 659B
CMakeLists.txt 901B
keypointextractors
iss.hpp 3KB
uniform.hpp 2KB
featurematching.hpp 1KB
transformationestimators
gc.hpp 3KB
ransac.hpp 5KB
svd.hpp 2KB
hough3d.hpp 4KB
json.hpp 284KB
parameter.h 14KB
configuration.hpp 10KB
lrfestimation.hpp 413B
logger.h 1KB
transformationestimation.hpp 2KB
pointcloud.h 10KB
keypointextraction.hpp 520B
featurematcher
kdtree.hpp 4KB
parameter.cpp 22KB
types.h 446B
pipeline.hpp 20KB
visualizer.h 1KB
optimization
optimizer.hpp 15KB
main.cpp 7KB
downsampling.hpp 444B
pipelinemodule.hpp 1KB
logger.cpp 2KB
hypothesisverification.hpp 4KB
color.hpp 2KB
test
CMakeLists.txt 1KB
ParameterTest.hpp 2KB
run_tests.cpp 886B
pipelinemoduletype.hpp 2KB
visualizer.cpp 3KB
poserefinement.hpp 978B
descriptors
fpfh.hpp 2KB
spinimage.hpp 2KB
rift.hpp 4KB
rsd.hpp 3KB
shot.hpp 3KB
usc.hpp 3KB
lrfestimators
board.hpp 2KB
downsamplers
uniform.hpp 2KB
voxelgrid.hpp 2KB
poserefiners
ndt.hpp 3KB
icp.hpp 3KB
README.md 19KB
utils.hpp 4KB
共 48 条
- 1
资源评论
极智视界
- 粉丝: 2w+
- 资源: 1504
下载权益
C知道特权
VIP文章
课程特权
开通VIP
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
最新资源
- Oracle数据库工具安装包(免安装)
- Http上传文件到服务器 php文件
- 114.0.5734.1 Google Chrome谷歌浏览器下载
- meta-llama-3-8b-instruct 的 model-00002-of-00004.safetensors 的2/3
- subversion-1.14.3-6.fc41.x86-64.rpm
- GridLayoutCalculator.zip
- Release QMYSQL driver 6.2.4-msvc2019-x64
- mod-ldap-2.4.54-5.fc37.x86-64.rpm
- mod-ldap-2.4.56-1.fc38.x86-64.rpm
- mod-ldap-2.4.58-7.fc40.x86-64.rpm
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功