Open3D源码阅读与学习
Open3D 简介
Open3D是由IntelVCL小组开发的一款轻量级的三维数据处理库。其采用C++11开发，同时与Python完美适配。作者本着usefulness and ease of use 的原则，对数据结构，和算法进行了精细的选择。没有依赖大型库如boost，PCL等，是一个小巧独立的三维点云库。笔者最喜欢的是其visualizer的设计，相比而言其可视化模块易用性比PCL的要高，同时还支持动画。
Open3D的安装
Open3D的所有源码在github上，使用者只需要按照其提供的安装教程，进行编译即可。这里需要注意的是，Ubuntu系统上对Python的选择默认的是Python3，那么还是使用Python2.7的用户需要手动将这两个变量修改一下，再进行编译即可。如果还有疑惑可以参阅Document。
PYTHON_EXECUTABLE /usr/bin/python
PYTHON_LIBRARY /usr/lib/x86_64-linux-gnu/libpython2.7.so
Open3D的Python使用
由于整个项目还在持续的开发，所以作者希望大家使用其Python借口，而不是使用CPP接口。 编译完成之后，我们可以在build文件夹下面，找到lib文件夹，在下面的Tutorial文件夹有关于Python接口的全部教程。结合其官方的英文文档可以较轻松的学习整个项目的功能。笔者就不一一赘述了。下面主要介绍CPP接口的使用。
C++接口的使用
准备工作
不幸的是我们并没有一个CPP接口的官方教程，然后整个项目也没有提供make install的功能（不久以后，应该会有）。我们就需要没有条件创造条件也要上了。想要使用其CPP接口，首先需要头文件，然后我们需要库文件，.a或者.so。
头文件
我们首先提取头文件。万幸的是，Open3D的cmake文件写的很好，很有条理，我们可以发现在Core，IO，Visualization三个文件夹下面有分别关于这个模块的CMakeLists.txt文件，我们修改这些CMakeLists.txt文件。
Core模块CMakeLists.txt末尾添加
install(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/Core)
install(FILES ${CORE_UTILITY_HEADER_FILES} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/Core/Utility)
install(FILES ${CORE_GEOMETRY_HEADER_FILES} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/Core/Geometry)
install(FILES ${CORE_CAMERA_HEADER_FILES} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/Core/Camera)
install(FILES ${CORE_REGISTRATION_HEADER_FILES} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/Core/Registration)
install(FILES ${CORE_ODOMETRY_HEADER_FILES} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/Core/Odometry)
install(FILES ${CORE_INTEGRATION_HEADER_FILES} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/Core/Integration)
IO模块CMakeLists.txt末尾添加
install(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/IO)
install(FILES ${IO_CLASSIO_HEADER_FILES} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/IO/ClassIO)
install(FILES ${IO_FILEFORMAT_HEADER_FILES} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/IO/FileFormat)
Visualization模块CMakeLists.txt末尾添加
install(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/Visualization)
install(FILES ${VISUALIZATION_UTILITY_HEADER_FILES} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/Visualization/Utility)
install(FILES ${VISUALIZATION_SHADER_HEADER_FILES} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/Visualization/Shader)
install(FILES ${VISUALIZATION_VISUALIZER_HEADER_FILES} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/Visualization/Visualizer)
这样通过make install可以把所有的头文件放好。
库文件
然后关于库文件，这里我就偷了个懒，直接复制的，当然也可以通过修改CMakeLists.txt文件来实现。
构建.cmake文件
我们知道当使用一个用cmake来管理的CPP库的时候，可以使用find_package()来寻找这个库的头文件，库文件，然后生成对应的变量，用于接下来的项目编译。同样的我们希望使用find_package()来管理Open3D，这样我们就需要写一个.cmake文件。
FindOpen3d.cmake
include_directories(/usr/include/eigen3/)
include_directories(/usr/include/GL)
set(OPEN3D_INCLUDE_DIRS /home/sun/src/Open3D/install/include)
link_directories(/home/sun/src/Open3D/install/lib)
find_package(OpenMP)
if (OPENMP_FOUND)
set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
endif()
set(OPEN3D_LIBRARIES
Visualization
IO
Core
IO
pthread
tinyfiledialogs
jpeg
png
jsoncpp
zlib
/usr/lib/x86_64-linux-gnu/libglfw.so
/usr/lib/x86_64-linux-gnu/libGLEW.so
/usr/lib/x86_64-linux-gnu/libdl.so
/usr/lib/x86_64-linux-gnu/librt.so
/usr/lib/x86_64-linux-gnu/libm.so
/usr/lib/x86_64-linux-gnu/libX11.so
/usr/lib/x86_64-linux-gnu/libXrandr.so
/usr/lib/x86_64-linux-gnu/libXinerama.so
/usr/lib/x86_64-linux-gnu/libXi.so
/usr/lib/x86_64-linux-gnu/libXxf86vm.so
/usr/lib/x86_64-linux-gnu/libXcursor.so
/usr/lib/x86_64-linux-gnu/libGL.so
)
然后我们在写自己项目的CMakeLists.txt的时候将FindOpen3d.cmake所在的目录添加到CMAKE_MODULE_PATH环境变量中去。就可以使用find_package来配置Open3d库了。
Open3D之PointCloud
Open3D的CPP接口文档
点云是三维数据处理中的基本的数据结构。在经典的点云库（PCL）中，点云是一种比较复杂的数据结构，而在Open3D中点云较为简单。
由图中可以看到，点云继承自Geometry3D而Geometry3D继承自Geometry。Geometry是一个纯粹的抽象类，也就是所有的几何数据类型都是以其为模板，它有一个枚举，包含了Unspecified，PointCloud，Lineset，TriangleMesh，Image。而Geometry3D则会有一些bound，和Transform的属性，更为具体。 PointCloud由数据点，法线，和颜色构成。结构较为简单。
// ----------------------------------------------------------------------------
// - Open3D: www.open3d.org -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------
#include <iostream>
#include <memory>
#include <Eigen/Dense>
#include <Core/Core.h>
#include <IO/IO.h>
#include <Visualization/Visualization.h>
void PrintPointCloud(const three::PointCloud &pointcloud)
{
using namespace three;
bool pointcloud_has_normal = pointcloud.HasNormals();
PrintInfo("Pointcloud has %d points.\n",
(int)pointcloud.points_.size());
Eigen::Vector3d min_bound = pointcloud.GetMinBound();
Eigen::Vector3d max_bound = pointcloud.GetMaxBound();
PrintInfo("Bounding box is: (%.4f, %.4f, %.4f) - (%.4f, %.4f, %.4f)\n",
min_bound(0), min_bound(1), min_bound(2),
max_bound(0), max_bound(1), max_bound(2));
for (size_t i = 0; i < pointcloud.points_.size(); i++) {
if (pointcloud_has_normal) {
const Eigen::Vector3d &point = pointcloud.points_[i];
const Eigen::Vector3d &normal = pointcloud.normals_[i];
PrintDebug("%.6f %.6f %.6f %.6f %.6f %.6f\n",
point(0), point(1), point(2),
normal(0), normal(1), normal(2));
} else {
const Eigen::Vector3d &point = pointcloud.points_[i];
PrintDebug("%.6f %.6f %.6f\n", point(0), point(1), point(2));
}
}
PrintDebug("End of the list.\n\n");
}
int main(int argc, char *argv[])
{
using namespace three;
SetVerbosityLevel(VerbosityLevel::VerboseAlways);
if(argc < 2)
{
PrintError("请给定点云文件！");
}
auto pcd = CreatePointCloudFromFile(argv[1]); //! 工厂函数
{
ScopeTimer timer("FPFH estimation with Radius 0.25"); //! 块计时器，在析构的时候打印耗时
//for (int i = 0; i < 20; i++) {
ComputeFPFHFeature(*pcd,
three::KDTreeSearchParamRadius(0.25));
//}
}
{
ScopeTimer timer("Normal estimation with KNN20");
for (int i = 0; i < 20; i++) {
EstimateNormals(*pcd,
three::KDTreeSearchParamKNN(20)); //! 已20个点来估算一个点的normal
}
}
std::cout << pcd->normals_[0] << std::endl;
std::cout << pcd->normals_[10] << std::endl;
{
ScopeTimer timer("Normal estimation with Radius 0.01666");
for (int i = 0; i < 20; i++) {
EstimateNormals(*pcd,
three::KDTreeSearchParamRadius(0.01666)); //! 半径来估算
}
}
std::cout << pcd->normals_[0] << std::endl;
std::cout << pcd->normals_[10] << std::endl;
{
ScopeTimer timer("Normal estimation with Hybrid 0.01666, 60");
for (int i = 0; i < 20; i++) {
EstimateNormals(*pcd,
three::KDTreeSearchParamHybrid(0.01666, 60)); //! 混合估算，第一个参数为半径，后面一个参数为数量
}
}
std::cout << pcd->normals_[0] << std::endl;
std::cout << pcd->normals_[10] << std::endl;
auto downpcd = VoxelDownSample(*pcd, 0.05); //! 体素降采样,0.05代表最小的一个box长度为0.05
// 1. test basic pointcloud functions.
PointCloud pointcloud;
PrintPointCloud(pointcloud);
//! https://blog.csdn.net/xiaolewennofollow/article/details/52559364
//! https://blog.csdn.net/yockie/article/details/52674366
//! https://blog.csdn.net/kaitiren/article/details/22302579
//! emplace_back只调用构造函数，不调用拷贝构造，和转移构造函数，速度很快
pointcloud.points_.emplace_back(0.0, 0.0, 0.0);
pointcloud.points_.emplace_back(1.0, 0.0, 0.0);
pointcloud.points_.emplace_back(0.0, 1.0, 0.0);
pointcloud.points_.emplace_back(0.0, 0.0, 1.0);
PrintPointCloud(pointcloud);
// 2. test pointcloud IO.
const std::string filename_xyz("test.xyz");
const std::string filename_ply("test.ply");
if (ReadPointCloud(argv[1], pointcloud)) {
PrintWarning("Successfully read %s\n", argv[1]);
/*
PointCloud pointcloud_copy;
pointcloud_copy.CloneFrom(pointcloud);
if (WritePointCloud(filename_xyz, pointcloud)) {
PrintWarning("Successfully wrote %s\n\n", filename_xyz.c_str());
} else {
PrintError("Failed to write %s\n\n", filename_xyz.c_str());
}
if (WritePointCloud(filename_ply, pointcloud_copy)) {
PrintWarning("Successfully wrote %s\n\n", filename_ply.c_str());
} else {
PrintError("Failed to write %s\n\n", filename_ply.c_str());
}
*/
} else {
PrintError("Failed to read %s\n\n", argv[1]);
}
// 3. test pointcloud visualization
Visualizer visualizer;
std::shared_ptr<PointCloud> pointcloud_ptr(new PointCloud);
*pointcloud_ptr = pointcloud;
pointcloud_ptr->NormalizeNormals();
BoundingBox bounding_box;
bounding_box.FitInGeometry(*pointcloud_ptr);
std::shared_ptr<PointCloud> pointcloud_transformed_ptr(new PointCloud);
*pointcloud_transformed_ptr = *pointcloud_ptr;
Eigen::Matrix4d trans_to_origin = Eigen::Matrix4d::Identity();
trans_to_origin.block<3, 1>(0, 3) = bounding_box.GetCenter() * -1.0;
Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity();
transformation.block<3, 3>(0, 0) = static_cast<Eigen::Matrix3d>(
Eigen::AngleAxisd(M_PI / 4.0, Eigen::Vector3d::UnitX()));
pointcloud_transformed_ptr->Transform(
trans_to_origin.inverse() * transformation * trans_to_origin);
//! 先把所有的点云归到原点，然后，绕着x轴旋转45度，然后再平移回去，总的来说就是绕着点云的中心点绕着x轴旋转45度
visualizer.CreateWindow("Open3D", 1600, 900);
visualizer.AddGeometry(pointcloud_ptr);
visualizer.AddGeometry(pointcloud_transformed_ptr);
visualizer.Run();
visualizer.DestroyWindow();
auto coord = CreateMeshCoordinateFrame(0.2);
// 4. test operations
*pointcloud_transformed_ptr += *pointcloud_ptr;
DrawGeometries({pointcloud_transformed_ptr,coord}, "Combined Pointcloud");
// 5. test downsample
auto downsampled = VoxelDownSample(*pointcloud_ptr, 0.05);
DrawGeometries({downsampled,coord}, "Down Sampled Pointcloud");
// 6. test normal estimation
//! https://segmentfault.com/a/1190000006212012
//! [&] 表达式的意思是所有的局部变量将按照引用的方式进行传递
DrawGeometriesWithKeyCallbacks({pointcloud_ptr},
},
"Press Space to Estimate Normal", 1600, 900);
// n. test end
PrintAlways("End of the test.\n");
}
Open3d之Lineset
Lineset顾名思义就是线段的集合，我们在显示的时候经常要连接匹配点对，以表示他们的关系。在open3d中我们可以方便的使用lineset来构建这些，无需复杂的调用绘制函数。
// ----------------------------------------------------------------------------
// - Open3D: www.open3d.org -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------
#include <cstdio>
#include <vector>
#include <Eigen/Dense>
#include <Core/Core.h>
#include <IO/IO.h>
#include <Visualization/Visualization.h>
int main(int argc, char **argv)
{
using namespace three;
using namespace flann;
SetVerbosityLevel(VerbosityLevel::VerboseAlways);
if (argc < 2) {
PrintInfo("Usage:\n");
PrintInfo(" > TestFlann [filename]\n");
PrintInfo(" The program will :\n");
PrintInfo(" 1. load the pointcloud in [filename].\n");
PrintInfo(" 2. use KDTreeFlann to compute 50 nearest neighbors of point0.\n");
PrintInfo(" 3. convert the correspondences to LineSet and render it.\n");
PrintInfo(" 4. rotate the point cloud slightly to get another point cloud.\n");
PrintInfo(" 5. find closest point of the original point cloud on the new point cloud, mark as correspondences.\n");
PrintInfo(" 6. convert to LineSet and render it.\n");
PrintInfo(" 6. distance below 0.05 are rendered as red, others as black.\n");
return 0;
}
auto cloud_ptr = CreatePointCloudFromFile(argv[1]);
std::vector<std::pair<int, int>> correspondences;
const int nn = 50;
KDTreeFlann kdtree; //! 构建kdtree
kdtree.SetGeometry(*cloud_ptr);
std::vector<int> indices_vec(nn);
std::vector<double> dists_vec(nn);
kdtree.SearchKNN(cloud_ptr->points_[0], nn, indices_vec, dists_vec);
for (int i = 0; i < nn; i++) {
correspondences.emplace_back(0, indices_vec[i]);
}
//! 构建线集合，两个点云之间的对应点连线
auto lineset_ptr = CreateLineSetFromPointCloudCorrespondences(
*cloud_ptr, *cloud_ptr, correspondences);
DrawGeometries({cloud_ptr, lineset_ptr});
//! http://en.cppreference.com/w/cpp/memory/shared_ptr/make_shared
//! 用make_shared要比使用new来得快
//! https://www.cnblogs.com/egmkang/archive/2013/04/28/3049102.html
//! http://tech-foo.blogspot.com/2012/04/experimenting-with-c-stdmakeshared.html
auto new_cloud_ptr = std::make_shared<PointCloud>();
*new_cloud_ptr = *cloud_ptr;
BoundingBox bounding_box;
bounding_box.FitInGeometry(*new_cloud_ptr); //! 计算bounding box
//! 作者用了一个比较有趣的构建方法
Eigen::Matrix4d trans_to_origin = Eigen::Matrix4d::Identity();
trans_to_origin.block<3, 1>(0, 3) = bounding_box.GetCenter() * -1.0;
Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity();
transformation.block<3, 3>(0, 0) = static_cast<Eigen::Matrix3d>(
Eigen::AngleAxisd(M_PI / 6.0, Eigen::Vector3d::UnitX()));
new_cloud_ptr->Transform(
trans_to_origin.inverse() * transformation * trans_to_origin);
correspondences.clear();
//! 对原始点云的所有的点都找一下，原来点云旋转过后的最近的点
for (size_t i = 0; i < new_cloud_ptr->points_.size(); i++) {
kdtree.SearchKNN(new_cloud_ptr->points_[i], 1, indices_vec, dists_vec);
correspondences.emplace_back(indices_vec[0], (int)i);
}
auto new_lineset_ptr = CreateLineSetFromPointCloudCorrespondences(
*cloud_ptr, *new_cloud_ptr, correspondences);
new_lineset_ptr->colors_.resize(new_lineset_ptr->lines_.size());
//! 上色，点点间距小于0.05×包围盒尺寸的上红色，其他的上黑色。
for (size_t i = 0; i < new_lineset_ptr->lines_.size(); i++) {
auto point_pair = new_lineset_ptr->GetLineCoordinate(i);
if ((point_pair.first - point_pair.second).norm() < 0.05 *
bounding_box.GetSize()) {
new_lineset_ptr->colors_[i] = Eigen::Vector3d(1.0, 0.0, 0.0);
} else {
new_lineset_ptr->colors_[i] = Eigen::Vector3d(0.0, 0.0, 0.0);
}
}
//! 由于对3维空间的元素很好的抽象了，所以点云与线集都可以直接放到visualizer中显示。
DrawGeometries({cloud_ptr, new_cloud_ptr, new_lineset_ptr});
return 0;
}
Open3D之Image
在Open3D中，图像是以std::vector<uint8_t> data_;存储的，作者使用了一些指针的方法可以将其存储float也可以将其存储uint_8等等。我们来看测试代码
// ----------------------------------------------------------------------------
// - Open3D: www.open3d.org -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------
#include <cstdio>
#include <Core/Core.h>
#include <IO/IO.h>
int main(int argc, char **argv)
{
using namespace three;
SetVerbosityLevel(VerbosityLevel::VerboseAlways);
if (argc != 3) {
PrintInfo("Usage:\n");
PrintInfo(" > TestImage [image filename] [depth filename]\n");
PrintInfo(" The program will :\n");
PrintInfo(" 1) Read 8bit RGB and 16bit depth image\n");
PrintInfo(" 2) Convert RGB image to single channel float image\n");
PrintInfo(" 3) 3x3, 5x5, 7x7 Gaussian filters are applied\n");
PrintInfo(" 4) 3x3 Sobel filter for x-and-y-directions are applied\n");
PrintInfo(" 5) Make image pyramid that includes Gaussian blur and downsampling\n");
PrintInfo(" 6) Will save all the layers in the image pyramid\n");
return 0;
}
const std::string filename_rgb(argv[1]);
const std::string filename_depth(argv[2]);
//! 彩色图像都是8位的
Image color_image_8bit;
if (ReadImage(filename_rgb, color_image_8bit)) {
PrintDebug("RGB image size : %d x %d\n",
color_image_8bit.width_, color_image_8bit.height_);
//! 灰度化 通过灰度化算法讲8位的彩色图像变成float图像
auto gray_image = CreateFloatImageFromImage(color_image_8bit);
WriteImage("gray.png",//! 存储的时候再次恢复位图
*CreateImageFromFloatImage<uint8_t>(*gray_image));
//! https://www.cnblogs.com/qiqibaby/p/5289977.html
//! 高斯滤波，3,5,7代表的是模板的大小,模板越大那么图像就越模糊
//! 需要对灰度图像进行操作
PrintDebug("Gaussian Filtering\n");
auto gray_image_b3 = FilterImage(*gray_image,
Image::FilterType::Gaussian3);
WriteImage("gray_blur3.png",
*CreateImageFromFloatImage<uint8_t>(*gray_image_b3));
auto gray_image_b5 = FilterImage(*gray_image,
Image::FilterType::Gaussian5);
WriteImage("gray_blur5.png",
*CreateImageFromFloatImage<uint8_t>(*gray_image_b5));
auto gray_image_b7 = FilterImage(*gray_image,
Image::FilterType::Gaussian7);
WriteImage("gray_blur7.png",
*CreateImageFromFloatImage<uint8_t>(*gray_image_b7));
//! https://en.wikipedia.org/wiki/Sobel_operator
//! Sobel算子用来近似计算一阶梯度。
PrintDebug("Sobel Filtering\n");
auto gray_image_dx = FilterImage(*gray_image,
Image::FilterType::Sobel3Dx);
// make [-1,1] to [0,1].
LinearTransformImage(*gray_image_dx, 0.5, 0.5);
ClipIntensityImage(*gray_image_dx);
WriteImage("gray_sobel_dx.png",
*CreateImageFromFloatImage<uint8_t>(*gray_image_dx));
auto gray_image_dy = FilterImage(*gray_image,
Image::FilterType::Sobel3Dy);
LinearTransformImage(*gray_image_dy, 0.5, 0.5);
ClipIntensityImage(*gray_image_dy);
WriteImage("gray_sobel_dy.png",
*CreateImageFromFloatImage<uint8_t>(*gray_image_dy));
//! https://en.wikipedia.org/wiki/Pyramid_(image_processing)
//! 创建了一个四层的图像金字塔
PrintDebug("Build Pyramid\n");
auto pyramid = CreateImagePyramid(*gray_image, 4);
for (int i = 0; i < 4; i++) {
auto level = pyramid[i];
auto level_8bit = CreateImageFromFloatImage<uint8_t>(*level);
std::string outputname =
"gray_pyramid_level" + std::to_string(i) + ".png";
WriteImage(outputname, *level_8bit);
}
} else {
PrintError("Failed to read %s\n\n", filename_rgb.c_str());
}
//! 深度图像是16位的
Image depth_image_16bit;
if (ReadImage(filename_depth, depth_image_16bit)) {
PrintDebug("Depth image size : %d x %d\n",
depth_image_16bit.width_, depth_image_16bit.height_);
auto depth_image = CreateFloatImageFromImage(depth_image_16bit);
WriteImage("depth.png",
*CreateImageFromFloatImage<uint16_t>(*depth_image));
PrintDebug("Gaussian Filtering\n");
auto depth_image_b3 = FilterImage(*depth_image,
Image::FilterType::Gaussian3);
WriteImage("depth_blur3.png",
*CreateImageFromFloatImage<uint16_t>(*depth_image_b3));
auto depth_image_b5 = FilterImage(*depth_image,
Image::FilterType::Gaussian5);
WriteImage("depth_blur5.png",
*CreateImageFromFloatImage<uint16_t>(*depth_image_b5));
auto depth_image_b7 = FilterImage(*depth_image,
Image::FilterType::Gaussian7);
WriteImage("depth_blur7.png",
*CreateImageFromFloatImage<uint16_t>(*depth_image_b7));
PrintDebug("Sobel Filtering\n");
auto depth_image_dx = FilterImage(*depth_image,
Image::FilterType::Sobel3Dx);
// make [-65536,65536] to [0,13107.2]. // todo: need to test this
LinearTransformImage(*depth_image_dx, 0.1, 6553.6);
ClipIntensityImage(*depth_image_dx, 0.0, 13107.2);
WriteImage("depth_sobel_dx.png",
*CreateImageFromFloatImage<uint16_t>(*depth_image_dx));
auto depth_image_dy = FilterImage(*depth_image,
Image::FilterType::Sobel3Dy);
LinearTransformImage(*depth_image_dy, 0.1, 6553.6);
ClipIntensityImage(*depth_image_dx, 0.0, 13107.2);
WriteImage("depth_sobel_dy.png",
*CreateImageFromFloatImage<uint16_t>(*depth_image_dy));
PrintDebug("Build Pyramid\n");
auto pyramid = CreateImagePyramid(*depth_image, 4);
for (int i = 0; i < 4; i++) {
auto level = pyramid[i];
auto level_16bit = CreateImageFromFloatImage<uint16_t>(*level);
std::string outputname =
"depth_pyramid_level" + std::to_string(i) + ".png";
WriteImage(outputname, *level_16bit);
}
}
else {
PrintError("Failed to read %s\n\n", filename_depth.c_str());
}
return 0;
}
Open3d之TriangleMesh
mesh就是将点云中的点三角化连接起来了。
// ----------------------------------------------------------------------------
// - Open3D: www.open3d.org -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------
#include <iostream>
#include <Core/Core.h>
#include <IO/IO.h>
#include <Visualization/Visualization.h>
void PrintHelp()
{
using namespace three;
PrintInfo("Usage :\n");
PrintInfo(" > TestTriangleMesh sphere\n");
PrintInfo(" > TestTriangleMesh merge <file1> <file2>\n");
PrintInfo(" > TestTriangleMesh normal <file1> <file2>\n");
}
void PaintMesh(three::TriangleMesh &mesh, const Eigen::Vector3d &color)
{
mesh.vertex_colors_.resize(mesh.vertices_.size());
for (size_t i = 0; i < mesh.vertices_.size(); i++) {
mesh.vertex_colors_[i] = color;
}
}
int main(int argc, char *argv[])
{
using namespace three;
SetVerbosityLevel(VerbosityLevel::VerboseAlways);
if (argc < 2) {
PrintHelp();
return 0;
}
//! 这边我们学习mesh
//! 作者为我们内置了一些简单的几何体，如球，圆柱，圆锥，箭头，坐标系
std::string option(argv[1]);
if (option == "sphere") {
auto mesh = CreateMeshSphere(0.05);
mesh->ComputeVertexNormals();
DrawGeometries({mesh});
WriteTriangleMesh("sphere.ply", *mesh, true, true);
} else if (option == "cylinder") {
auto mesh = CreateMeshCylinder(0.5, 2.0);
mesh->ComputeVertexNormals();
DrawGeometries({mesh});
WriteTriangleMesh("cylinder.ply", *mesh, true, true);
} else if (option == "cone") {
auto mesh = CreateMeshCone(0.5, 2.0, 20, 3);
mesh->ComputeVertexNormals();
DrawGeometries({mesh});
WriteTriangleMesh("cone.ply", *mesh, true, true);
} else if (option == "arrow") {
auto mesh = CreateMeshArrow();
mesh->ComputeVertexNormals();
DrawGeometries({mesh});
WriteTriangleMesh("arrow.ply", *mesh, true, true);
} else if (option == "frame") {
if (argc < 3) {
auto mesh = CreateMeshCoordinateFrame();
DrawGeometries({mesh});
WriteTriangleMesh("frame.ply", *mesh, true, true);
} else {
//! 导入mesh，并为mesh计算normal
auto mesh = CreateMeshFromFile(argv[2]);
mesh->ComputeVertexNormals();
BoundingBox boundingbox(*mesh);
auto mesh_frame = CreateMeshCoordinateFrame(
boundingbox.GetSize() * 0.2, boundingbox.min_bound_);
//! 绘制mesh加上坐标系
DrawGeometries({mesh, mesh_frame});
}
} else if (option == "merge") {
auto mesh1 = CreateMeshFromFile(argv[2]);
auto mesh2 = CreateMeshFromFile(argv[3]);
PrintInfo("Mesh1 has %d vertices, %d triangles.\n",
mesh1->vertices_.size(), mesh1->triangles_.size());
PrintInfo("Mesh2 has %d vertices, %d triangles.\n",
mesh2->vertices_.size(), mesh2->triangles_.size());
//! 重载操作符+=后，可以合并mesh
*mesh1 += *mesh2;
PrintInfo("After merge, Mesh1 has %d vertices, %d triangles.\n",
mesh1->vertices_.size(), mesh1->triangles_.size());
/// Function to remove duplicated and non-manifold vertices/triangles
mesh1->Purge();
PrintInfo("After purge vertices, Mesh1 has %d vertices, %d triangles.\n",
mesh1->vertices_.size(), mesh1->triangles_.size());
DrawGeometries({mesh1});
WriteTriangleMesh("temp.ply", *mesh1, true, true);
} else if (option == "normal") {
auto mesh = CreateMeshFromFile(argv[2]);
mesh->ComputeVertexNormals();
WriteTriangleMesh(argv[3], *mesh, true, true);
/// 对mesh放大
} else if (option == "scale") {
auto mesh = CreateMeshFromFile(argv[2]);
double scale = std::stod(argv[4]);
Eigen::Matrix4d trans = Eigen::Matrix4d::Identity();
trans(0, 0) = trans(1, 1) = trans(2, 2) = scale;
mesh->Transform(trans);
WriteTriangleMesh(argv[3], *mesh);
/// 将mesh归一到一个统一的boundingbox，同时把中心也移动过去
} else if (option == "unify") {
/// unify into (0, 0, 0) - (scale, scale, scale) box
auto mesh = CreateMeshFromFile(argv[2]);
BoundingBox bbox;
bbox.FitInGeometry(*mesh);
double scale1 = std::stod(argv[4]);
double scale2 = std::stod(argv[5]);
Eigen::Matrix4d trans = Eigen::Matrix4d::Identity();
trans(0, 0) = trans(1, 1) = trans(2, 2) = scale1 / bbox.GetSize();
mesh->Transform(trans);
trans.setIdentity();
trans.block<3, 1>(0, 3) = Eigen::Vector3d(scale2 / 2.0, scale2 / 2.0,
scale2 / 2.0) - bbox.GetCenter() * scale1 / bbox.GetSize();
mesh->Transform(trans);
WriteTriangleMesh(argv[3], *mesh);
} else if (option == "distance") {
auto mesh1 = CreateMeshFromFile(argv[2]);
auto mesh2 = CreateMeshFromFile(argv[3]);
double scale = std::stod(argv[4]);
mesh1->vertex_colors_.resize(mesh1->vertices_.size());
KDTreeFlann kdtree;
kdtree.SetGeometry(*mesh2);
std::vector<int> indices(1);
std::vector<double> dists(1);
double r = 0.0;
/// 距离越大颜色越深
for (size_t i = 0; i < mesh1->vertices_.size(); i++) {
kdtree.SearchKNN(mesh1->vertices_[i], 1, indices, dists);
double color = std::min(sqrt(dists[0]) / scale, 1.0);
mesh1->vertex_colors_[i] = Eigen::Vector3d(color, color, color);
r += sqrt(dists[0]);
}
PrintInfo("Average distance is %.6f.\n",
r / (double)mesh1->vertices_.size());
if (argc > 5) {
WriteTriangleMesh(argv[5], *mesh1);
}
DrawGeometries({mesh1});
} else if (option == "showboth") {
auto mesh1 = CreateMeshFromFile(argv[2]);
PaintMesh(*mesh1, Eigen::Vector3d(1.0, 0.75, 0.0));
auto mesh2 = CreateMeshFromFile(argv[3]);
PaintMesh(*mesh2, Eigen::Vector3d(0.25, 0.25, 1.0));
std::vector<std::shared_ptr<const Geometry>> meshes;
meshes.push_back(mesh1);
meshes.push_back(mesh2);
DrawGeometries(meshes);
} else if (option == "colormapping") {
auto mesh = CreateMeshFromFile(argv[2]);
mesh->ComputeVertexNormals();
/// 相机轨迹，外参序列
PinholeCameraTrajectory trajectory;
ReadIJsonConvertible(argv[3], trajectory);
if (filesystem::DirectoryExists("image") == false) {
PrintWarning("No image!\n");
return 0;
}
int idx = 3000;
std::vector<std::shared_ptr<const Geometry>> ptrs;
ptrs.push_back(mesh);
auto mesh_sphere = CreateMeshSphere(0.05);
Eigen::Matrix4d trans;
trans.setIdentity();
trans.block<3, 1>(0, 3) = mesh->vertices_[idx];
mesh_sphere->Transform(trans);
mesh_sphere->ComputeVertexNormals();
ptrs.push_back(mesh_sphere);
DrawGeometries(ptrs);
for (size_t i = 0; i < trajectory.extrinsic_.size(); i += 10) {
char buffer[1024];
sprintf(buffer, "image/image_%06d.png", (int)i + 1);
auto image = CreateImageFromFile(buffer);
auto fimage = CreateFloatImageFromImage(*image);
/// 将球上面的顶点变换到相机坐标系下面
Eigen::Vector4d pt_in_camera = trajectory.extrinsic_[i] *
Eigen::Vector4d(mesh->vertices_[idx](0),
mesh->vertices_[idx](1), mesh->vertices_[idx](2), 1.0);
/// 用相机内参投影
Eigen::Vector3d pt_in_plane =
trajectory.intrinsic_.intrinsic_matrix_ *
pt_in_camera.block<3, 1>(0, 0);
Eigen::Vector3d uv = pt_in_plane / pt_in_plane(2);
std::cout << pt_in_camera << std::endl;
std::cout << pt_in_plane << std::endl;
std::cout << pt_in_plane / pt_in_plane(2) << std::endl;
/// 获取图像中的点
auto result = fimage->FloatValueAt(uv(0), uv(1));
if (result.first) {
PrintWarning("%.6f\n", result.second);
}
DrawGeometries({fimage}, "Test", 1920, 1080);
}
}
}
Open3D之Flann
啥也不说了上代码，比较简单，自己对flann也没有什么研究。总共测试了原生的flann方法和Open3D实现的Kdtree
// ----------------------------------------------------------------------------
// - Open3D: www.open3d.org -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------
#include <cstdio>
#include <vector>
#include <flann/flann.hpp>
#include <Core/Core.h>
#include <IO/IO.h>
#include <Visualization/Visualization.h>
int main(int argc, char **argv)
{
using namespace three;
using namespace flann;
if (argc < 2) {
PrintInfo("Usage:\n");
PrintInfo(" > TestFlann [filename]\n");
return 0;
}
auto cloud_ptr = std::make_shared<PointCloud>();
if (ReadPointCloud(argv[1], *cloud_ptr)) {
PrintWarning("Successfully read %s\n", argv[1]);
} else {
PrintError("Failed to read %s\n\n", argv[1]);
return 0;
}
if ((int)cloud_ptr->points_.size() < 100) {
PrintError("Boring point cloud.\n");
return 0;
}
if (!cloud_ptr->HasColors()) {
cloud_ptr->colors_.resize(cloud_ptr->points_.size());
for (size_t i = 0; i < cloud_ptr->points_.size(); i++) {
cloud_ptr->colors_[i].setZero(); /// 涂黑
}
}
int nn = std::min(20, (int)cloud_ptr->points_.size() - 1);
/// https://eigen.tuxfamily.org/dox/group__QuickRefPage.html
Matrix<double> dataset((double *)cloud_ptr->points_.data(),
cloud_ptr->points_.size(), 3 );
Matrix<double> query((double *)cloud_ptr->points_.data(), 1, 3);
std::vector<int> indices_vec(nn);
std::vector<double> dists_vec(nn);
Matrix<int> indices(indices_vec.data(), query.rows, nn);
Matrix<double> dists(dists_vec.data(), query.rows, nn);
/// 使用flann原生的nn搜索
Index<L2<double>> index(dataset, KDTreeSingleIndexParams(10));
index.buildIndex();
index.knnSearch(query, indices, dists, nn, SearchParams(-1, 0.0));
for (size_t i = 0; i < indices_vec.size(); i++) {
PrintInfo("%d, %f\n", (int)indices_vec[i], sqrt(dists_vec[i]));
cloud_ptr->colors_[indices_vec[i]] = Eigen::Vector3d(1.0, 0.0, 0.0);
}
cloud_ptr->colors_[0] = Eigen::Vector3d(0.0, 1.0, 0.0);
float r = float(sqrt(dists_vec[nn - 1]) * 2.0);
Matrix<double> query1((double *)cloud_ptr->points_.data() + 3 * 99, 1, 3);
/// 使用flann的半径搜索
int k = index.radiusSearch(query1, indices, dists, r * r,
SearchParams(-1, 0.0));
PrintInfo("======== %d, %f ========\n", k, r);
for (int i = 0; i < k; i++) {
PrintInfo("%d, %f\n", (int)indices_vec[i], sqrt(dists_vec[i]));
cloud_ptr->colors_[indices_vec[i]] = Eigen::Vector3d(0.0, 0.0, 1.0);
}
cloud_ptr->colors_[99] = Eigen::Vector3d(0.0, 1.0, 1.0);
DrawGeometries({cloud_ptr}, "TestFlann", 1600, 900);
/// 使用Open3d的kdtree接口
auto new_cloud_ptr = std::make_shared<PointCloud>();
if (ReadPointCloud(argv[1], *new_cloud_ptr)) {
PrintWarning("Successfully read %s\n", argv[1]);
} else {
PrintError("Failed to read %s\n\n", argv[1]);
return 0;
}
if ((int)new_cloud_ptr->points_.size() < 100) {
PrintError("Boring point cloud.\n");
return 0;
}
if (new_cloud_ptr->HasColors() == false) {
new_cloud_ptr->colors_.resize(new_cloud_ptr->points_.size());
for (size_t i = 0; i < new_cloud_ptr->points_.size(); i++) {
new_cloud_ptr->colors_[i].setZero();
}
}
/// 构建kdtree
KDTreeFlann kdtree;
kdtree.SetGeometry(*new_cloud_ptr);
std::vector<int> new_indices_vec(nn);
std::vector<double> new_dists_vec(nn);
kdtree.SearchKNN(new_cloud_ptr->points_[0], nn,
new_indices_vec, new_dists_vec);
for (size_t i = 0; i < new_indices_vec.size(); i++) {
PrintInfo("%d, %f\n", (int)new_indices_vec[i], sqrt(new_dists_vec[i]));
new_cloud_ptr->colors_[new_indices_vec[i]] =
Eigen::Vector3d(1.0, 0.0, 0.0);
}
new_cloud_ptr->colors_[0] = Eigen::Vector3d(0.0, 1.0, 0.0);
k = kdtree.SearchRadius(new_cloud_ptr->points_[99], r, new_indices_vec,
new_dists_vec);
PrintInfo("======== %d, %f ========\n", k, r);
for (int i = 0; i < k; i++) {
PrintInfo("%d, %f\n", (int)new_indices_vec[i], sqrt(new_dists_vec[i]));
new_cloud_ptr->colors_[new_indices_vec[i]] =
Eigen::Vector3d(0.0, 0.0, 1.0);
}
new_cloud_ptr->colors_[99] = Eigen::Vector3d(0.0, 1.0, 1.0);
k = kdtree.Search(new_cloud_ptr->points_[199], KDTreeSearchParamRadius(r),
new_indices_vec, new_dists_vec);
PrintInfo("======== %d, %f ========\n", k, r);
for (int i = 0; i < k; i++) {
PrintInfo("%d, %f\n", (int)new_indices_vec[i], sqrt(new_dists_vec[i]));
new_cloud_ptr->colors_[new_indices_vec[i]] =
Eigen::Vector3d(0.0, 0.0, 1.0);
}
new_cloud_ptr->colors_[199] = Eigen::Vector3d(0.0, 1.0, 1.0);
DrawGeometries({new_cloud_ptr}, "TestKDTree", 1600, 900);
return 0;
}