all_theta.extend(theta)

这行代码的作用是将一个列表 theta 中的元素添加到另一个列表 all_theta 的末尾。具体来说，它会将 theta 中的每个元素添加到 all_theta 中，扩展 all_theta 的长度。注意，extend() 方法不会创建新的列表，它只是修改现有的列表。因此，如果你希望保留原始列表，可以在调用 extend() 方法之前先创建 all_theta 的副本。

相关问题

智能小车opencv巡线代码

以下是智能小车OpenCV巡线代码的示例：

import cv2
import numpy as np

# 设置ROI区域
def roi(img, vertices):
    mask = np.zeros_like(img)
    cv2.fillPoly(mask, vertices, 255)
    masked = cv2.bitwise_and(img, mask)
    return masked

# 处理图像
def process_img(original_image):
    # 转换为灰度图像
    processed_img = cv2.cvtColor(original_image, cv2.COLOR_BGR2GRAY)
    # 高斯模糊
    processed_img = cv2.GaussianBlur(processed_img, (5, 5), 0)
    # 边缘检测
    processed_img = cv2.Canny(processed_img, threshold1=50, threshold2=150)
    # 设置ROI区域
    vertices = np.array([[0, 480], [0, 280], [150, 200], [490, 200], [640, 280], [640, 480]], np.int32)
    processed_img = roi(processed_img, [vertices])
    # 返回处理后的图像
    return processed_img

# 找到直线
def find_lines(img, lines):
    left_line_x = []
    left_line_y = []
    right_line_x = []
    right_line_y = []
    for line in lines:
        for x1, y1, x2, y2 in line:
            slope = (y2 - y1) / (x2 - x1)
            if slope < 0:
                left_line_x.extend([x1, x2])
                left_line_y.extend([y1, y2])
            else:
                right_line_x.extend([x1, x2])
                right_line_y.extend([y1, y2])
    min_y = img.shape[0] * 0.6
    max_y = img.shape[0]
    poly_left = np.poly1d(np.polyfit(left_line_y, left_line_x, deg=1))
    left_x_start = int(poly_left(max_y))
    left_x_end = int(poly_left(min_y))
    poly_right = np.poly1d(np.polyfit(right_line_y, right_line_x, deg=1))
    right_x_start = int(poly_right(max_y))
    right_x_end = int(poly_right(min_y))
    return [[left_x_start, int(max_y), left_x_end, int(min_y)]], [[right_x_start, int(max_y), right_x_end, int(min_y)]]

# 绘制直线
def draw_lines(img, lines):
    line_img = np.zeros((img.shape[0], img.shape[1], 3), dtype=np.uint8)
    for line in lines:
        for x1, y1, x2, y2 in line:
            cv2.line(line_img, (x1, y1), (x2, y2), [0, 255, 0], thickness=5)
    return line_img

# 主函数
def main():
    cap = cv2.VideoCapture(0)
    while True:
        _, frame = cap.read()
        processed_img = process_img(frame)
        lines = cv2.HoughLinesP(processed_img, rho=6, theta=np.pi / 60, threshold=160, lines=np.array([]), minLineLength=40, maxLineGap=25)
        left_line, right_line = find_lines(processed_img, lines)
        line_img = draw_lines(frame, [left_line, right_line])
        result = cv2.addWeighted(frame, 0.9, line_img, 1, 1)
        cv2.imshow("result", result)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
    cap.release()
    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()

rrt算法平滑处理 ，路径以图片形式输出python

好的，以下是一个简单的 Python 实现 RRT 算法并对路径进行平滑处理的示例代码，同时将最终路径输出为一张图片：

import numpy as np
import random
from scipy.spatial.distance import euclidean
from scipy.interpolate import splprep, splev
import matplotlib.pyplot as plt
from PIL import Image

class RRT:
    def __init__(self, start, goal, obstacle_list, rand_area, expand_dis=0.5, path_resolution=0.1, goal_sample_rate=20, max_iter=1000):
        self.start = start
        self.goal = goal
        self.obstacle_list = obstacle_list
        self.min_x, self.min_y, self.max_x, self.max_y = rand_area
        self.expand_dis = expand_dis
        self.path_resolution = path_resolution
        self.goal_sample_rate = goal_sample_rate
        self.max_iter = max_iter

    class Node:
        def __init__(self, x, y):
            self.x = x
            self.y = y
            self.parent = None

    def planning(self):
        self.node_list = [self.Node(self.start[0], self.start[1])]
        for i in range(self.max_iter):
            rnd_node = self.get_random_node()
            nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node)
            nearest_node = self.node_list[nearest_ind]
            new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
            if self.check_collision(new_node, self.obstacle_list):
                self.node_list.append(new_node)
            if self.calc_dist(new_node.x, new_node.y, self.goal[0], self.goal[1]) <= self.expand_dis:
                final_node = self.steer(new_node, self.goal, self.expand_dis)
                if self.check_collision(final_node, self.obstacle_list):
                    return self.generate_final_course(len(self.node_list) - 1)
            if i % 5 == 0:
                self.draw_graph(rnd_node)
        return None

    def steer(self, from_node, to_node, extend_length=float("inf")):
        new_node = self.Node(from_node.x, from_node.y)
        dist, theta = self.calc_distance_and_angle(new_node, to_node)
        new_node.x += extend_length * np.cos(theta)
        new_node.y += extend_length * np.sin(theta)
        new_node.parent = from_node
        return new_node

    def generate_final_course(self, goal_ind):
        path = [[self.goal[0], self.goal[1]]]
        node = self.node_list[goal_ind]
        while node.parent is not None:
            path.append([node.x, node.y])
            node = node.parent
        path.append([self.start[0], self.start[1]])
        return path

    def calc_distance_and_angle(self, from_node, to_node):
        dx = to_node.x - from_node.x
        dy = to_node.y - from_node.y
        return euclidean([0, 0], [dx, dy]), np.arctan2(dy, dx)

    def get_random_node(self):
        if random.randint(0, 100) > self.goal_sample_rate:
            rnd = [random.uniform(self.min_x, self.max_x), random.uniform(self.min_y, self.max_y)]
        else:
            rnd = [self.goal[0], self.goal[1]]
        return self.Node(rnd[0], rnd[1])

    def get_nearest_node_index(self, node_list, rnd_node):
        dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y) ** 2 for node in node_list]
        minind = dlist.index(min(dlist))
        return minind

    def check_collision(self, node, obstacle_list):
        for (ox, oy, r) in obstacle_list:
            if (node.x - ox) ** 2 + (node.y - oy) ** 2 <= r ** 2:
                return False  # collision
        return True  # safe

    def calc_dist(self, x1, y1, x2, y2):
        return np.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)

    def draw_graph(self, rnd=None):
        plt.clf()
        if rnd is not None:
            plt.plot(rnd[0], rnd[1], "^k")
        for node in self.node_list:
            if node.parent is not None:
                plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")
        for (ox, oy, r) in self.obstacle_list:
            self.plot_circle(ox, oy, r)
        plt.plot(self.start[0], self.start[1], "xr")
        plt.plot(self.goal[0], self.goal[1], "xr")
        plt.axis("equal")
        plt.axis([-2, 15, -2, 15])
        plt.grid(True)
        plt.pause(0.01)

    @staticmethod
    def plot_circle(x, y, size, color="-b"):  # pragma: no cover
        deg = list(range(0, 360, 5))
        deg.append(0)
        xl = [x + size / 2.0 * np.cos(np.deg2rad(d)) for d in deg]
        yl = [y + size / 2.0 * np.sin(np.deg2rad(d)) for d in deg]
        plt.plot(xl, yl, color)

    def smooth_path(self, path):
        path_x = [x[0] for x in path]
        path_y = [x[1] for x in path]
        tck, u = splprep([path_x, path_y], s=0.0, per=False)
        u_new = np.linspace(u.min(), u.max(), int(np.sum([euclidean(a, b) for a, b in zip(path[:-1], path[1:])]) / self.path_resolution))
        x_new, y_new = splev(u_new, tck, der=0)
        smoothed_path = [[x_new[i], y_new[i]] for i in range(len(x_new))]
        return smoothed_path

    def visualize_path(self, path):
        plt.clf()
        for node in self.node_list:
            if node.parent is not None:
                plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")
        for (ox, oy, r) in self.obstacle_list:
            self.plot_circle(ox, oy, r)
        plt.plot(self.start[0], self.start[1], "xr")
        plt.plot(self.goal[0], self.goal[1], "xr")
        path_x = [x[0] for x in path]
        path_y = [x[1] for x in path]
        plt.plot(path_x, path_y, "-r")
        plt.axis("equal")
        plt.axis([-2, 15, -2, 15])
        plt.grid(True)
        plt.pause(0.01)

if __name__ == '__main__':
    start = [0, 0]
    goal = [10, 10]
    obstacle_list = [
        (5, 5, 1),
        (3, 6, 2),
        (3, 8, 2),
        (5, 10, 2),
        (7, 9, 2)
    ]
    rand_area = [-2, 15, -2, 15]

    rrt = RRT(start, goal, obstacle_list, rand_area)
    path = rrt.planning()
    if path is None:
        print("Cannot find path")
    else:
        smoothed_path = rrt.smooth_path(path)
        rrt.visualize_path(smoothed_path)
        plt.savefig('rrt_smoothed_path.png')

上述代码中，`RRT` 类实现了 RRT 算法的核心逻辑，其中 `planning` 方法用于规划路径，返回最终路径（若找到了可行路径），否则返回 `None`。`smooth_path` 方法用于对最终路径进行平滑处理，返回平滑后的路径。`visualize_path` 方法用于将路径可视化，并将最终路径保存为一张图片。在上述代码中，将最终路径保存为 `rrt_smoothed_path.png`。你可以根据需要修改代码中的参数，例如起点位置、终点位置、障碍物信息等，以及调整图片的输出方式。