pip install open3d

pip install open3d-0.18.0-cp39-cp39-manylinux_2_27_x86_64.whl

pip install open3d-cpu

open3d.io.write_point_cloud(
    filename: os.PathLike, 
    pointcloud: open3d.geometry.PointCloud, 
    format: str = 'auto', 
    write_ascii: bool = False, 
    compressed: bool = False, 
    print_progress: bool = False
) -> bool

open3d.io.read_point_cloud(
    filename: os.PathLike, 
    format: str = 'auto', 
    remove_nan_points: bool = False, 
    remove_infinite_points: bool = False, 
    print_progress: bool = False
) -> open3d.geometry.PointCloud

open3d.visualization.draw_geometries(
    geometry_list: list[open3d.geometry.Geometry], 
    window_name: str = 'Open3D', 
    width: int = 1920, 
    height: int = 1080, 
    left: int = 50, 
    top: int = 50, 
    point_show_normal: bool = False, 
    mesh_show_wireframe: bool = False, 
    mesh_show_back_face: bool = False, 
    lookat: numpy.ndarray[numpy.float64[3, 1]] | None = None, 
    up: numpy.ndarray[numpy.float64[3, 1]] | None = None, 
    front: numpy.ndarray[numpy.float64[3, 1]] | None = None, 
    zoom: float | None = None
) -> None

import open3d as o3d
import numpy as np

# 生成一个简单的点云（例如，一个立方体的顶点）
points = np.array([
    [0, 0, 0],
    [1, 0, 0],
    [1, 1, 0],
    [0, 1, 0],
    [0, 0, 1],
    [1, 0, 1],
    [1, 1, 1],
    [0, 1, 1],
])

# 创建 PointCloud 对象
pcd = o3d.geometry.PointCloud()

# 将点添加到 PointCloud 对象中
pcd.points = o3d.utility.Vector3dVector(points)

# 保存点云到文件
o3d.io.write_point_cloud("generated_point_cloud.ply", pcd)

# 读取点云文件
load_pcd = o3d.io.read_point_cloud("generated_point_cloud.ply")

# 可视化点云
o3d.visualization.draw_geometries([load_pcd])

static const std::unordered_map<
        std::string,
        std::function<bool(const std::string &,
                           geometry::TriangleMesh &,
                           const ReadTriangleMeshOptions &)>>
        file_extension_to_trianglemesh_read_function{
                {"ply", ReadTriangleMeshFromPLY},
                {"stl", ReadTriangleMeshUsingASSIMP},
                {"obj", ReadTriangleMeshUsingASSIMP},
                {"off", ReadTriangleMeshFromOFF},
                {"gltf", ReadTriangleMeshUsingASSIMP},
                {"glb", ReadTriangleMeshUsingASSIMP},
                {"fbx", ReadTriangleMeshUsingASSIMP},
        };

static const std::unordered_map<
        std::string,
        std::function<bool(const std::string &,
                           const geometry::TriangleMesh &,
                           const bool,
                           const bool,
                           const bool,
                           const bool,
                           const bool,
                           const bool)>>
        file_extension_to_trianglemesh_write_function{
                {"ply", WriteTriangleMeshToPLY},
                {"stl", WriteTriangleMeshToSTL},
                {"obj", WriteTriangleMeshToOBJ},
                {"off", WriteTriangleMeshToOFF},
                {"gltf", WriteTriangleMeshToGLTF},
                {"glb", WriteTriangleMeshToGLTF},
        };

}  // unnamed namespace

open3d.io.read_triangle_mesh(
filename: os.PathLike, 
enable_post_processing: 
bool = False, 
print_progress: bool = False
) → open3d.geometry.TriangleMesh

import open3d as o3d

# 定义文件路径
filename = "doll.stl"

try:
    # 尝试读取三角网格
    mesh = o3d.io.read_triangle_mesh(filename, enable_post_processing=True, print_progress=True)
    
    # 检查网格是否成功读取
    if mesh.is_empty():
        print("Failed to read the mesh. The file format may not be supported.")
    else:
        print("Successfully read the mesh.")
        # 可视化三角网格
        o3d.visualization.draw_geometries([mesh])
except Exception as e:
    print(f"An error occurred: {e}")

import open3d as o3d
import numpy as np

# 定义文件路径
filename = "doll.stl"

# 读取三角网格
mesh = o3d.io.read_triangle_mesh(filename, enable_post_processing=True, print_progress=True)

# 检查网格是否成功读取
if mesh.is_empty():
    print("Failed to read the mesh. The file format may not be supported.")
else:
    print("Successfully read the mesh.")
    
    # 计算法线
    mesh.compute_vertex_normals()
    
    # 设置网格的颜色为红色
    mesh.paint_uniform_color([1, 0, 0])  # 设置为红色

    # 创建一个可视化窗口
    vis = o3d.visualization.Visualizer()
    vis.create_window()

    # 添加网格到可视化窗口
    vis.add_geometry(mesh)

    # 更新几何体和渲染器
    vis.update_geometry(mesh)
    vis.poll_events()
    vis.update_renderer()

    # 渲染
    vis.run()
    vis.destroy_window()

import open3d as o3d
import numpy as np

# 定义文件路径
filename = "doll.stl"

# 读取三角网格
mesh = o3d.io.read_triangle_mesh(filename)
if mesh.is_empty():
    print("Failed to read the mesh. The file format may not be supported.")
else:
    print("Successfully read the mesh.")

    # 计算法线
    mesh.compute_vertex_normals()

    # 设置材质
    mat_box = o3d.visualization.rendering.MaterialRecord()
    mat_box.shader = 'defaultLitSSR'
    mat_box.base_color = [0.467, 0.467, 0.467, 0.2]  # 设置透明度为0.2
    mat_box.base_roughness = 0.0
    mat_box.base_reflectance = 0.0
    mat_box.base_clearcoat = 1.0
    mat_box.thickness = 1.0
    mat_box.transmission = 1.0
    mat_box.absorption_distance = 10
    mat_box.absorption_color = [0.5, 0.5, 0.5]

    # 使用draw函数渲染
    o3d.visualization.draw(
        [{'name': 'box', 'geometry': mesh, 'material': mat_box}],
        show_skybox=False,
        width=800,
        height=600,
        bg_color=[0.5, 0.5, 0.5, 0.8]  # 设置背景颜色为灰色
    )

import open3d as o3d
import numpy as np

# 定义文件路径
filename = "doll.stl"

# 读取三角网格
mesh = o3d.io.read_triangle_mesh(filename)
if mesh.is_empty():
    print("Failed to read the mesh. The file format may not be supported.")
else:
    print("Successfully read the mesh.")

    # 计算法线
    mesh.compute_vertex_normals()

    # 从mesh提取点云
    point_cloud = mesh.sample_points_uniformly(number_of_points=10000)

    # 设置材质
    mat_box = o3d.visualization.rendering.MaterialRecord()
    mat_box.shader = 'defaultLitSSR'
    mat_box.base_color = [0.467, 0.467, 0.467, 0.2]  # 设置透明度为0.2
    mat_box.base_roughness = 0.0
    mat_box.base_reflectance = 0.0
    mat_box.base_clearcoat = 1.0
    mat_box.thickness = 1.0
    mat_box.transmission = 1.0
    mat_box.absorption_distance = 10
    mat_box.absorption_color = [0.5, 0.5, 0.5]

    # 使用draw函数渲染
    o3d.visualization.draw(
        [{'name': 'box', 'geometry': mesh, 'material': mat_box},
         {'name': 'point_cloud', 'geometry': point_cloud}],
        show_skybox=False,
        width=800,
        height=600,
        bg_color=[0.5, 0.5, 0.5, 0.8]  # 设置背景颜色为灰色
    )

#include <iostream>
#include <vector>
#include <algorithm>

struct Point {
    std::vector<double> coords;
    Point(std::initializer_list<double> init) : coords(init) {}
};

struct KDNode {
    Point point;
    KDNode* left;
    KDNode* right;
    KDNode(Point p) : point(p), left(nullptr), right(nullptr) {}
};

class KDTree {
public:
    KDTree(const std::vector<Point>& points) {
        root = build(points, 0);
    }

    KDNode* build(const std::vector<Point>& points, int depth) {
        if (points.empty()) return nullptr;
        int k = points[0].coords.size();
        int axis = depth % k;
        std::vector<Point> sorted_points = points;
        std::sort(sorted_points.begin(), sorted_points.end(), [axis](const Point& a, const Point& b) {
            return a.coords[axis] < b.coords[axis];
        });
        int median = sorted_points.size() / 2;
        KDNode* node = new KDNode(sorted_points[median]);
        std::vector<Point> left_points(sorted_points.begin(), sorted_points.begin() + median);
        std::vector<Point> right_points(sorted_points.begin() + median + 1, sorted_points.end());
        node->left = build(left_points, depth + 1);
        node->right = build(right_points, depth + 1);
        return node;
    }

    void nearestNeighborSearch(const Point& query, Point& best, double& best_dist, KDNode* node, int depth) {
        if (!node) return;
        int k = query.coords.size();
        int axis = depth % k;
        double dist = distance(query, node->point);
        if (dist < best_dist) {
            best_dist = dist;
            best = node->point;
        }
        double diff = query.coords[axis] - node->point.coords[axis];
        KDNode* near = diff <= 0 ? node->left : node->right;
        KDNode* far = diff <= 0 ? node->right : node->left;
        nearestNeighborSearch(query, best, best_dist, near, depth + 1);
        if (std::abs(diff) < best_dist) {
            nearestNeighborSearch(query, best, best_dist, far, depth + 1);
        }
    }

    Point nearestNeighbor(const Point& query) {
        Point best = root->point;
        double best_dist = distance(query, best);
        nearestNeighborSearch(query, best, best_dist, root, 0);
        return best;
    }

private:
    KDNode* root;

    double distance(const Point& a, const Point& b) {
        double dist = 0;
        for (size_t i = 0; i < a.coords.size(); ++i) {
            dist += (a.coords[i] - b.coords[i]) * (a.coords[i] - b.coords[i]);
        }
        return dist;
    }
};

int main() {
    std::vector<Point> points = {{2.0, 3.0}, {5.0, 4.0}, {9.0, 6.0}, {4.0, 7.0}, {8.0, 1.0}, {7.0, 2.0}};
    KDTree tree(points);
    Point query = {9.0, 2.0};
    Point nearest = tree.nearestNeighbor(query);
    std::cout << "最近邻点: (" << nearest.coords[0] << ", " << nearest.coords[1] << ")\n";
    return 0;
}

import open3d as o3d
import numpy as np

# 创建一个随机点云
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(np.random.rand(5000, 3))

# 为点云设置颜色
colors = np.random.rand(5000, 3)  # 随机颜色
pcd.colors = o3d.utility.Vector3dVector(colors)

# 构建k-d tree
kdtree = o3d.geometry.KDTreeFlann(pcd)

# 查询k-d tree中的最近邻
query_point = np.random.rand(3)
[k, idx, dist] = kdtree.search_knn_vector_3d(query_point, 10)
print("查询点:", query_point)
print("k-d tree最近邻索引:", idx)
print("k-d tree最近邻距离:", dist)

# 提取最近邻点
nearest_points = np.asarray(pcd.points)[idx, :]

# 创建查询点和最近邻点的点云
query_pcd = o3d.geometry.PointCloud()
query_pcd.points = o3d.utility.Vector3dVector([query_point])
query_pcd.paint_uniform_color([1, 0, 0])  # 将查询点设置为红色

nearest_pcd = o3d.geometry.PointCloud()
nearest_pcd.points = o3d.utility.Vector3dVector(nearest_points)
nearest_pcd.paint_uniform_color([0, 1, 0])  # 将最近邻点设置为绿色

# 可视化点云、查询点和最近邻点
vis = o3d.visualization.Visualizer()
vis.create_window()
vis.add_geometry(pcd)
vis.add_geometry(query_pcd)
vis.add_geometry(nearest_pcd)

# 调整点云大小
opt = vis.get_render_option()
opt.point_size = 2.0  # 设置原始点云大小
opt.background_color = np.asarray([0.8, 0.8, 0.8])  # 设置背景颜色

# 放大最近邻点的大小
for i in range(len(nearest_pcd.points)):
    sphere = o3d.geometry.TriangleMesh.create_sphere(radius=0.02)
    sphere.translate(nearest_pcd.points[i])
    sphere.paint_uniform_color([0, 1, 0])
    vis.add_geometry(sphere)

# 更新可视化
vis.poll_events()
vis.update_renderer()
vis.run()
vis.destroy_window()

import open3d as o3d

# 创建一个 Octree，指定最大深度
max_depth = 4
octree = o3d.geometry.Octree(max_depth)

# 创建一个随机点云
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(np.random.rand(5000, 3))

# 从点云构建 Octree
octree.convert_from_point_cloud(pcd, size_expand=0.01)

point = [0.5, 0.5, 0.5]
octree.insert_point(point)

query_point = [0.5, 0.5, 0.5]
success, node_info = octree.locate_leaf_node(query_point)
print("查询成功:", success)
print("节点信息:", node_info)

o3d.visualization.draw_geometries([octree])

import open3d as o3d
import numpy as np

# 创建一个随机点云
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(np.random.rand(5000, 3))

# 构建 Octree
max_depth = 4
octree = o3d.geometry.Octree(max_depth)
octree.convert_from_point_cloud(pcd, size_expand=0.01)

# 插入多个点
points = np.random.rand(100, 3)  # 生成 100 个随机点

def leaf_node_init():
    return o3d.geometry.OctreeColorLeafNode()

def leaf_node_update(node):
    pass

def internal_node_init():
    return o3d.geometry.OctreeInternalNode()

def internal_node_update(node):
    pass

for point in points:
    octree.insert_point(point, leaf_node_init, leaf_node_update, internal_node_init, internal_node_update)

# 查询 Octree 中的点
query_point = np.array([0.5, 0.5, 0.5])
success, node_info = octree.locate_leaf_node(query_point)
print("查询点:", query_point)
print("查询成功:", success)
print("节点信息:", node_info)

# 可视化 Octree 节点
def create_pointcloud_from_octree(octree):
    points = []
    colors = []
    def traverse(node, node_info):
        if isinstance(node, o3d.geometry.OctreeColorLeafNode):
            origin = node_info.origin
            size = node_info.size
            depth = node_info.depth
            # 根据深度设置颜色
            if depth == 0:
                color = [1, 0, 0]  # 红色
            elif depth == 1:
                color = [0, 1, 0]  # 绿色
            elif depth == 2:
                color = [0, 0, 1]  # 蓝色
            elif depth == 3:
                color = [1, 1, 0]  # 黄色
            else:
                color = [0, 1, 1]  # 青色
            points.append(origin + size / 2)
            colors.append(color)
        return False
    octree.traverse(traverse)
    pointcloud = o3d.geometry.PointCloud()
    pointcloud.points = o3d.utility.Vector3dVector(np.array(points))
    pointcloud.colors = o3d.utility.Vector3dVector(np.array(colors))
    return pointcloud

octree_pointcloud = create_pointcloud_from_octree(octree)

# 创建 Octree 的线框表示
def create_lineset_from_octree(octree):
    lines = []
    colors = []
    points = []
    def traverse(node, node_info):
        if isinstance(node, o3d.geometry.OctreeColorLeafNode) or isinstance(node, o3d.geometry.OctreeInternalNode):
            origin = node_info.origin
            size = node_info.size
            # 添加立方体的 12 条边
            cube_lines = [
                [0, 1], [1, 3], [3, 2], [2, 0],  # 底面
                [4, 5], [5, 7], [7, 6], [6, 4],  # 顶面
                [0, 4], [1, 5], [2, 6], [3, 7]   # 侧面
            ]
            cube_points = [
                origin,
                origin + [size, 0, 0],
                origin + [0, size, 0],
                origin + [size, size, 0],
                origin + [0, 0, size],
                origin + [size, 0, size],
                origin + [0, size, size],
                origin + [size, size, size]
            ]
            base_index = len(points)
            points.extend(cube_points)
            lines.extend([[base_index + start, base_index + end] for start, end in cube_lines])
            colors.extend([[0, 0, 0] for _ in range(len(cube_lines))])  # 黑色
        return False
    octree.traverse(traverse)
    lineset = o3d.geometry.LineSet()
    lineset.points = o3d.utility.Vector3dVector(np.array(points))
    lineset.lines = o3d.utility.Vector2iVector(np.array(lines))
    lineset.colors = o3d.utility.Vector3dVector(np.array(colors))
    return lineset

octree_lineset = create_lineset_from_octree(octree)

# 创建找到的立方体
if success:
    origin = node_info.origin
    size = node_info.size
    cube = o3d.geometry.TriangleMesh.create_box(width=size, height=size, depth=size)
    cube.translate(origin)
    cube.paint_uniform_color([1, 0, 0])  # 红色

# 使用默认的绘制函数来显示点云和 Octree
geometries = [octree_pointcloud, octree_lineset]
if success:
    geometries.append(cube)
o3d.visualization.draw_geometries(geometries)

import open3d as o3d
import numpy as np

# 创建多个点云簇
def create_clustered_point_cloud(num_clusters=5, points_per_cluster=2000, cluster_radius=0.05):
    points = []
    for i in range(num_clusters):
        cluster_center = np.random.rand(3)
        cluster_points = cluster_center + cluster_radius * np.random.randn(points_per_cluster, 3)
        points.append(cluster_points)
    return np.vstack(points)

# 生成点云
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(create_clustered_point_cloud())

# 构建 Octree
max_depth = 4  # 调整 Octree 的深度
octree = o3d.geometry.Octree(max_depth)
octree.convert_from_point_cloud(pcd, size_expand=0.01)

# 分割点云并为每个部分赋予不同的颜色
def segment_point_cloud(octree):
    segments = []
    colors = np.random.rand(100, 3)  # 生成随机颜色
    color_index = 0

    def traverse(node, node_info):
        nonlocal color_index
        if isinstance(node, o3d.geometry.OctreeLeafNode):
            # 获取叶节点中的点
            segment = pcd.select_by_index(node.indices)
            segment.paint_uniform_color(colors[color_index % len(colors)])
            segments.append(segment)
            color_index += 1
        return False

    octree.traverse(traverse)
    return segments

# 获取分割后的点云部分
segments = segment_point_cloud(octree)

# 检查是否有分割后的点云部分
if len(segments) == 0:
    print("没有分割后的点云部分，请检查 Octree 构建和遍历逻辑。")
else:
    # 可视化分割后的点云
    o3d.visualization.draw_geometries(segments)

import open3d as o3d
import numpy as np

# 创建一个随机点云
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(np.random.rand(10000, 3))

# 构建 Octree
max_depth = 4  # 调整 Octree 的深度
octree = o3d.geometry.Octree(max_depth)
octree.convert_from_point_cloud(pcd, size_expand=0.01)

# 定义体素滤波函数
def voxel_filter(octree, voxel_size):
    filtered_points = []

    def traverse(node, node_info):
        if isinstance(node, o3d.geometry.OctreeLeafNode):
            # 计算叶节点的中心点
            voxel_center = node_info.origin + node_info.size / 2
            filtered_points.append(voxel_center)
        return False

    octree.traverse(traverse)
    return filtered_points

# 设置体素大小
voxel_size = 0.05

# 获取滤波后的点云
filtered_points = voxel_filter(octree, voxel_size)
filtered_pcd = o3d.geometry.PointCloud()
filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points)

# 可视化原始点云和滤波后的点云
print("原始点云点数:", len(pcd.points))
print("滤波后点云点数:", len(filtered_pcd.points))
o3d.visualization.draw_geometries([pcd], window_name="原始点云")
o3d.visualization.draw_geometries([filtered_pcd], window_name="滤波后点云")

import open3d as o3d
import numpy as np

# 创建一个随机点云
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(np.random.rand(10000, 3))

# 统计滤波
pcd_statistical, ind_statistical = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=1.0)
filtered_pcd_statistical = pcd.select_by_index(ind_statistical)

# 半径滤波
pcd_radius, ind_radius = pcd.remove_radius_outlier(nb_points=10, radius=0.1)
filtered_pcd_radius = pcd.select_by_index(ind_radius)

# 体素下采样
voxel_size = 0.05
downsampled_pcd_voxel = pcd.voxel_down_sample(voxel_size)

# Uniform 下采样
every_k_points = 10
downsampled_pcd_uniform = pcd.uniform_down_sample(every_k_points)

# 可视化原始点云和过滤后的点云
print("原始点云点数:", len(pcd.points))
print("统计滤波后的点云点数:", len(filtered_pcd_statistical.points))
print("半径滤波后的点云点数:", len(filtered_pcd_radius.points))
print("体素下采样后的点云点数:", len(downsampled_pcd_voxel.points))
print("Uniform下采样后的点云点数:", len(downsampled_pcd_uniform.points))

o3d.visualization.draw_geometries([pcd], window_name="原始点云")
o3d.visualization.draw_geometries([filtered_pcd_statistical], window_name="统计滤波后的点云")
o3d.visualization.draw_geometries([filtered_pcd_radius], window_name="半径滤波后的点云")
o3d.visualization.draw_geometries([downsampled_pcd_voxel], window_name="体素下采样后的点云")
o3d.visualization.draw_geometries([downsampled_pcd_uniform], window_name="Uniform下采样后的点云")

import open3d as o3d
import numpy as np

# 创建一个随机点云
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(np.random.rand(1000, 3))

# 定义一个变换矩阵
transformation_matrix = np.array([[1, 0, 0, 1],
                                  [0, 1, 0, 2],
                                  [0, 0, 1, 3],
                                  [0, 0, 0, 1]])

# 应用变换矩阵到点云
pcd.transform(transformation_matrix)

# 可视化变换后的点云
o3d.visualization.draw_geometries([pcd], window_name="Transformed Point Cloud")

# 平移向量
translation_vector = np.array([1, 2, 3])

# 平移点云
pcd.translate(translation_vector)

# 可视化平移后的点云
o3d.visualization.draw_geometries([pcd], window_name="Translated Point Cloud")

# 定义一个旋转矩阵（绕 Z 轴旋转 45 度）
rotation_matrix = pcd.get_rotation_matrix_from_xyz((0, 0, np.pi / 4))

# 旋转点云
pcd.rotate(rotation_matrix, center=(0, 0, 0))

# 可视化旋转后的点云
o3d.visualization.draw_geometries([pcd], window_name="Rotated Point Cloud")

# 缩放比例
scale_factor = 2.0

# 缩放点云
pcd.scale(scale_factor, center=pcd.get_center())

# 可视化缩放后的点云
o3d.visualization.draw_geometries([pcd], window_name="Scaled Point Cloud")

import open3d as o3d
import numpy as np

# 生成点云数据
def generate_point_cloud():
    # 生成一个简单的平面点云
    mesh = o3d.geometry.TriangleMesh.create_sphere(radius=1.0)
    pcd = mesh.sample_points_poisson_disk(number_of_points=500)
    return pcd

# 生成点云
pcd = generate_point_cloud()

# 打印点云信息
print("Point cloud before normal estimation:")
print(pcd)

# 估计法线
print("Estimating normals...")
pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamKNN(knn=30))

# 打印估计法线后的点云信息
print("Point cloud after normal estimation:")
print(pcd)

# 可视化带法线的点云
o3d.visualization.draw_geometries([pcd], point_show_normal=True, window_name="Point Cloud with Normals")

# 使法线方向一致
print("Orienting normals consistently...")
pcd.orient_normals_consistent_tangent_plane(k=30)

# 打印调整法线方向后的点云信息
print("Point cloud after orienting normals:")
print(pcd)

# 可视化带一致法线的点云
o3d.visualization.draw_geometries([pcd], point_show_normal=True, window_name="Point Cloud with Oriented Normals")

import open3d as o3d
import numpy as np

def create_colored_point_cloud(color):
    """创建并上色的球体点云"""
    pcd = o3d.geometry.TriangleMesh.create_sphere(radius=1.0).sample_points_uniformly(number_of_points=1000)
    pcd.paint_uniform_color(color)
    return pcd

def visualize_point_clouds(pcd1, pcd2, window_name):
    """可视化点云"""
    o3d.visualization.draw_geometries([pcd1, pcd2], window_name=window_name)

# 生成并上色两个相似的点云
pcd1 = create_colored_point_cloud([1, 0, 0])  # 红色
pcd2 = create_colored_point_cloud([0, 1, 0])  # 绿色

# 对第二个点云进行初始变换
initial_transformation = np.array([[0.862, 0.011, -0.507, 0.5],
                                   [-0.139, 0.967, -0.215, 0.7],
                                   [0.487, 0.255, 0.835, -1.4],
                                   [0.0, 0.0, 0.0, 1.0]])
pcd2.transform(initial_transformation)

# 可视化初始点云
visualize_point_clouds(pcd1, pcd2, "Initial Point Clouds")

# 使用 ICP 进行配准
threshold = 0.7  # 增加阈值
trans_init = np.eye(4)
reg_p2p = o3d.pipelines.registration.registration_icp(
    pcd1, pcd2, threshold, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPoint()
)

# 打印配准信息
print("ICP converged:", reg_p2p.inlier_rmse < threshold)
print("Fitness:", reg_p2p.fitness)
print("Inlier RMSE:", reg_p2p.inlier_rmse)
print("Transformation matrix:")
print(reg_p2p.transformation)

# 计算逆矩阵
inverse_transformation = np.linalg.inv(reg_p2p.transformation)

# 应用逆矩阵到第二个点云
pcd2.transform(inverse_transformation)

# 可视化配准后的点云
visualize_point_clouds(pcd1, pcd2, "Aligned Point Clouds")

import open3d as o3d
import numpy as np

def create_colored_sphere(radius, color, density=1000):
    mesh = o3d.geometry.TriangleMesh.create_sphere(radius=radius)
    pcd = mesh.sample_points_poisson_disk(number_of_points=density)
    pcd.paint_uniform_color(color)
    return pcd

def estimate_normals(pcd, radius=0.1, max_nn=30):
    pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=max_nn))

def execute_colored_icp(source, target, max_correspondence_distance, initial_transformation):
    criteria = o3d.pipelines.registration.ICPConvergenceCriteria(
        relative_fitness=1e-3,
        relative_rmse=1e-3,
        max_iteration=500)
    result = o3d.pipelines.registration.registration_colored_icp(
        source, target, max_correspondence_distance, initial_transformation,
        o3d.pipelines.registration.TransformationEstimationForColoredICP(),
        criteria)
    return result

# 创建两个绿色的球体
sphere1 = create_colored_sphere(1.0, [0, 1, 0], density=1000)
sphere2 = create_colored_sphere(1.0, [0, 1, 0], density=1000)

# 对第二个球体进行变换
transformation = np.array([[0.862, 0.011, -0.507, 0.5],
                           [-0.139, 0.967, -0.215, 0.7],
                           [0.487, 0.255, 0.835, -1.4],
                           [0.0, 0.0, 0.0, 1.0]])
sphere2.transform(transformation)

# 估算法线
estimate_normals(sphere1)
estimate_normals(sphere2)

# 可视化配准前的点云
print("配准前的点云：")
o3d.visualization.draw_geometries([sphere1, sphere2], window_name="Before Registration")

# 使用彩色ICP进行配准
max_correspondence_distance = 5.0  # 增大最大对应点距离
initial_transformation = np.identity(4)
result_colored_icp = execute_colored_icp(sphere1, sphere2, max_correspondence_distance, initial_transformation)

# 应用变换到第一个球体点云
sphere1.transform(result_colored_icp.transformation)

# 可视化配准后的点云
print("配准后的点云：")
o3d.visualization.draw_geometries([sphere1, sphere2], window_name="After Registration")

import open3d as o3d
import numpy as np

# 生成点云数据
def generate_point_cloud():
    # 创建一个球体点云
    mesh = o3d.geometry.TriangleMesh.create_sphere(radius=1.0)
    pcd = mesh.sample_points_poisson_disk(number_of_points=500)
    return pcd

# 生成源点云和目标点云
source = generate_point_cloud()
target = generate_point_cloud()

# 对目标点云进行随机变换
transformation = np.array([[0.862, 0.011, -0.507, 0.5],
                           [-0.139, 0.967, -0.215, 0.7],
                           [0.487, 0.255, 0.835, -1.4],
                           [0.0, 0.0, 0.0, 1.0]])
target.transform(transformation)

# 下采样点云
voxel_size = 0.05
source_down = source.voxel_down_sample(voxel_size)
target_down = target.voxel_down_sample(voxel_size)

# 估计法线
source_down.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
target_down.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))

# 计算FPFH特征
source_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
    source_down,
    o3d.geometry.KDTreeSearchParamHybrid(radius=0.25, max_nn=100))
target_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
    target_down,
    o3d.geometry.KDTreeSearchParamHybrid(radius=0.25, max_nn=100))

# 使用RANSAC进行全局配准
result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
    source_down, target_down, source_fpfh, target_fpfh,
    mutual_filter=True,
    max_correspondence_distance=0.15,
    estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
    ransac_n=4,
    checkers=[o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
              o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(0.15)],
    criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(4000000, 500))

print(result_ransac)

# 可视化配准前的点云
source_temp = source_down.transform(np.identity(4))  # 恢复原始位置
o3d.visualization.draw_geometries([source_temp, target_down], window_name="配准前")

# 可视化配准后的点云
source_temp = source_down.transform(result_ransac.transformation)
o3d.visualization.draw_geometries([source_temp, target_down], window_name="配准后")

import open3d as o3d
import numpy as np

def create_colored_sphere(radius, color, density=1000):
    mesh = o3d.geometry.TriangleMesh.create_sphere(radius=radius)
    pcd = mesh.sample_points_poisson_disk(number_of_points=density)
    pcd.paint_uniform_color(color)
    return pcd

def preprocess_point_cloud(pcd, voxel_size):
    pcd_down = pcd.voxel_down_sample(voxel_size)
    radius_normal = voxel_size * 2
    pcd_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
    radius_feature = voxel_size * 5
    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
        pcd_down,
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
    return pcd_down, pcd_fpfh

def pairwise_registration(source, target, voxel_size):
    source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)
    target_down, target_fpfh = preprocess_point_cloud(target, voxel_size)

    distance_threshold = voxel_size * 1.5
    result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
        source_down, target_down, source_fpfh, target_fpfh, True,
        distance_threshold,
        o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
        4, [
            o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
            o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold)
        ], o3d.pipelines.registration.RANSACConvergenceCriteria(4000000, 500))
    return result

def full_registration(pcds, voxel_size):
    pose_graph = o3d.pipelines.registration.PoseGraph()
    odometry = np.identity(4)
    pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(odometry))

    for source_id in range(len(pcds)):
        for target_id in range(source_id + 1, len(pcds)):
            result = pairwise_registration(pcds[source_id], pcds[target_id], voxel_size)
            trans = result.transformation
            information = o3d.pipelines.registration.get_information_matrix_from_point_clouds(
                pcds[source_id], pcds[target_id], voxel_size * 1.5, result.transformation)
            if target_id == source_id + 1:
                odometry = np.dot(trans, odometry)
                pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(np.linalg.inv(odometry)))
                pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(source_id, target_id, trans, information, uncertain=False))
            else:
                pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(source_id, target_id, trans, information, uncertain=True))
    return pose_graph

def run_global_optimization(pose_graph):
    option = o3d.pipelines.registration.GlobalOptimizationOption(
        max_correspondence_distance=0.02,
        edge_prune_threshold=0.25,
        reference_node=0)
    o3d.pipelines.registration.global_optimization(
        pose_graph,
        o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt(),
        o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(),
        option)

def merge_point_clouds(pcds, pose_graph):
    pcd_combined = o3d.geometry.PointCloud()
    for point_id in range(len(pcds)):
        pcd_transformed = pcds[point_id].transform(pose_graph.nodes[point_id].pose)
        pcd_combined += pcd_transformed
    return pcd_combined

# 创建不同颜色和大小的球体
sphere1 = create_colored_sphere(1.0, [1, 0, 0], density=1000)  # 红色球体
sphere2 = create_colored_sphere(0.8, [0, 1, 0], density=1000)  # 绿色球体
sphere3 = create_colored_sphere(0.6, [0, 0, 1], density=1000)  # 蓝色球体

# 对球体进行随机变换
transformation1 = np.array([[0.862, 0.011, -0.507, 0.5],
                            [-0.139, 0.967, -0.215, 0.7],
                            [0.487, 0.255, 0.835, -1.4],
                            [0.0, 0.0, 0.0, 1.0]])
sphere2.transform(transformation1)

transformation2 = np.array([[0.707, -0.707, 0.0, 1.0],
                            [0.707, 0.707, 0.0, 0.5],
                            [0.0, 0.0, 1.0, -0.5],
                            [0.0, 0.0, 0.0, 1.0]])
sphere3.transform(transformation2)

pcds = [sphere1, sphere2, sphere3]

# 设置体素大小
voxel_size = 0.05

# 可视化配准前的点云
print("配准前的点云")
o3d.visualization.draw_geometries(pcds, window_name="Before Registration")

# 进行多路配准
pose_graph = full_registration(pcds, voxel_size)

# 运行全局优化
run_global_optimization(pose_graph)

# 合并点云
pcd_combined = merge_point_clouds(pcds, pose_graph)

# 可视化配准后的点云
print("配准后的点云")
o3d.visualization.draw_geometries([pcd_combined], window_name="After Registration")