visualize.py

# [file name]: visualize.py
# [file content begin]
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from matplotlib.patches import Rectangle, Circle
from matplotlib.lines import Line2D
import argparse
import os
import sys
import glob
import platform


class SDCardReader:
    """SD卡读取器，自动检测SD卡路径"""

    def __init__(self):
        self.system = platform.system()
        self.sd_card_paths = self._get_sd_card_paths()

    def _get_sd_card_paths(self):
        """根据操作系统获取可能的SD卡路径"""
        if self.system == "Windows":
            # Windows系统，检查可能的盘符
            drives = []
            for drive in range(ord('F'), ord('Z') + 1):
                drive_letter = f"{chr(drive)}:\\"
                if os.path.exists(drive_letter):
                    drives.append(drive_letter)
            return drives

        elif self.system == "Linux":
            # Linux系统，检查常见的挂载点
            linux_paths = [
                "/media/*/",  # Ubuntu等系统
                "/mnt/sd*/",  # 嵌入式系统
                "/run/media/*/",  # 一些现代Linux发行版
                "/mnt/external_sd/",
            ]

            # 展开通配符路径
            expanded_paths = []
            for path in linux_paths:
                expanded_paths.extend(glob.glob(path))
            return expanded_paths

        elif self.system == "Darwin":  # macOS
            return ["/Volumes/"]

        else:
            return ["/mnt/", "/media/"]  # 其他系统

    def find_trajectory_files(self, filename="route.txt"):
        """在SD卡中查找轨迹文件"""
        found_files = []

        for sd_path in self.sd_card_paths:
            # 查找指定文件
            search_paths = [
                os.path.join(sd_path, filename),  # 根目录
                os.path.join(sd_path, "trajectory", filename),  # trajectory目录
                os.path.join(sd_path, "*", filename),  # 任何子目录
            ]

            for search_path in search_paths:
                # 处理通配符
                if "*" in search_path:
                    matches = glob.glob(search_path)
                    found_files.extend(matches)
                elif os.path.exists(search_path):
                    found_files.append(search_path)

        # 去重并返回
        return list(set(found_files))

    def get_sd_card_info(self):
        """获取SD卡信息"""
        info = {
            "system": self.system,
            "sd_card_paths": self.sd_card_paths,
            "detected_files": self.find_trajectory_files()
        }
        return info


class TrajectoryVisualizer:
    def __init__(self, matrix_file=None):
        """
        初始化轨迹可视化器

        Args:
            matrix_file: 轨迹矩阵文件路径，如果为None则自动查找SD卡
        """
        self.sd_reader = SDCardReader()

        if matrix_file is None:
            # 自动查找SD卡中的轨迹文件
            self.matrix_file = self._auto_find_trajectory_file()
        else:
            self.matrix_file = matrix_file

        self.matrix = None
        self.height = 200  # 固定高度为200
        self.width = 200   # 固定宽度为200
        self.trajectory_points = []  # 存储轨迹点的坐标
        self.connections = []  # 存储连接的线段

        if self.matrix_file is None:
            print("未找到轨迹文件，请检查SD卡是否正确插入")

    def _auto_find_trajectory_file(self):
        """自动查找SD卡中的轨迹文件"""
        print("正在搜索SD卡中的轨迹文件...")

        # 首先查找常见的轨迹文件名
        possible_filenames = [
            # "route.txt",
            # "trajectory.txt",
            # "matrix.txt",
            # "*.txt",   # 调试用
            "route_*.txt",  # 查找所有txt文件
            "traj_*.txt"
        ]

        found_files = []
        for filename in possible_filenames:
            files = self.sd_reader.find_trajectory_files(filename)
            if files:
                found_files.extend(files)

        # 去重
        found_files = list(set(found_files))

        if not found_files:
            print("未找到任何轨迹文件")
            return None

        if len(found_files) == 1:
            print(f"找到轨迹文件: {found_files[0]}")
            return found_files[0]
        else:
            # 多个文件，让用户选择
            print("找到多个轨迹文件:")
            for i, file_path in enumerate(found_files, 1):
                print(f"  {i}. {file_path}")

            choice = input("请选择要读取的文件编号 (1-{}): ".format(len(found_files)))
            try:
                index = int(choice) - 1
                if 0 <= index < len(found_files):
                    return found_files[index]
            except ValueError:
                pass

            # 默认选择第一个
            print(f"使用第一个文件: {found_files[0]}")
            return found_files[0]

    def load_matrix(self):
        """从文件加载坐标点"""
        if self.matrix_file is None:
            print("错误: 未指定轨迹文件!")
            return False

        if not os.path.exists(self.matrix_file):
            print(f"错误: 文件 {self.matrix_file} 不存在!")
            return False

        print(f"正在加载坐标文件: {self.matrix_file}")

        try:
            with open(self.matrix_file, 'r') as f:
                lines = f.readlines()

            self.trajectory_points = []
            
            for i, line in enumerate(lines):
                line = line.strip()
                if not line:  # 跳过空行
                    continue
                    
                # 解析坐标点，支持空格或逗号分隔
                if ',' in line:
                    parts = line.split(',')
                else:
                    parts = line.split()
                
                if len(parts) >= 2:
                    try:
                        x = float(parts[0].strip())
                        y = float(parts[1].strip())
                        # 确保坐标在200x200范围内
                        if 0 <= x < self.width and 0 <= y < self.height:
                            self.trajectory_points.append((x, y))
                        else:
                            print(f"警告: 第{i+1}行坐标({x}, {y})超出范围(0-{self.width-1}, 0-{self.height-1})")
                    except ValueError:
                        print(f"警告: 第{i+1}行格式错误")
                else:
                    print(f"警告: 第{i+1}行格式错误")

            if len(self.trajectory_points) == 0:
                print("错误: 文件中没有有效的坐标点!")
                return False

            print(f"成功加载 {len(self.trajectory_points)} 个坐标点")
            
            # 创建200x200的矩阵，将轨迹点标记为1
            self.matrix = np.zeros((self.height, self.width), dtype=int)
            for x, y in self.trajectory_points:
                # 将坐标转换为整数索引
                ix = int(round(x))
                iy = int(round(y))
                if 0 <= ix < self.width and 0 <= iy < self.height:
                    self.matrix[iy][ix] = 1

            print(f"矩阵尺寸: {self.width} x {self.height}")
            print("坐标点加载完成!")
            return True

        except Exception as e:
            print(f"加载坐标文件时发生错误: {e}")
            return False

    def extract_trajectory(self):
        """从坐标点中提取轨迹"""
        if not self.trajectory_points:
            print("错误: 没有轨迹点数据!")
            return False

        print("正在提取轨迹...")

        # 坐标点已经是按顺序排列的，直接使用
        print(f"使用 {len(self.trajectory_points)} 个坐标点作为轨迹")

        # 提取连接线段
        self._extract_connections()
        
        return True

    def _extract_connections(self):
        """从轨迹点中提取连接线段"""
        if len(self.trajectory_points) < 2:
            print("警告: 轨迹点不足2个，无法生成连接线段")
            return

        self.connections = []
        for i in range(len(self.trajectory_points) - 1):
            p1 = self.trajectory_points[i]
            p2 = self.trajectory_points[i + 1]
            self.connections.append((p1, p2))
            
        print(f"生成 {len(self.connections)} 条连接线段")

    def visualize_static(self, output_file=None, show_points=True, show_line=True):
        """静态可视化轨迹"""
        if not self.trajectory_points:
            if not self.load_matrix() or not self.extract_trajectory():
                print("错误: 无法提取轨迹!")
                return

        print("正在生成静态可视化...")

        fig, ax = plt.subplots(figsize=(12, 12))

        # 设置坐标轴
        ax.set_xlim(0, self.width)
        ax.set_ylim(0, self.height)
        ax.set_aspect('equal')
        ax.invert_yaxis()  # 反转y轴，使原点在左上角

        # 设置标题和标签
        ax.set_title(f"Route of the Car (Size: {self.width} x {self.height})", fontsize=16)
        ax.set_xlabel("X", fontsize=12)
        ax.set_ylabel("Y", fontsize=12)

        # 绘制轨迹线
        if show_line and self.connections:
            for connection in self.connections:
                p1, p2 = connection
                x_vals = [p1[0], p2[0]]
                y_vals = [p1[1], p2[1]]
                ax.plot(x_vals, y_vals, 'b-', linewidth=2, alpha=0.7)

        # 绘制轨迹点
        if show_points and self.trajectory_points:
            x_vals = [p[0] for p in self.trajectory_points]
            y_vals = [p[1] for p in self.trajectory_points]
            ax.scatter(x_vals, y_vals, c='red', s=20, alpha=0.8, zorder=5)

        # 添加起始点和终点标记
        if len(self.trajectory_points) >= 2:
            start_point = self.trajectory_points[0]
            end_point = self.trajectory_points[-1]

            # 起始点
            ax.plot(start_point[0], start_point[1], 'go', markersize=12,
                    label=f'start ({start_point[0]}, {start_point[1]})', zorder=10)

            # 终点
            ax.plot(end_point[0], end_point[1], 'ro', markersize=12,
                    label=f'end ({end_point[0]}, {end_point[1]})', zorder=10)

        # 添加图例
        ax.legend(loc='best', fontsize=10)

        # 添加网格
        ax.grid(True, alpha=0.3, linestyle='--')

        # 调整布局
        plt.tight_layout()

        # 保存或显示图像
        if output_file:
            plt.savefig(output_file, dpi=150, bbox_inches='tight')
            print(f"可视化已保存到: {output_file}")

        plt.show()

    def visualize_animated(self, output_file=None, speed=10):
        """动画可视化轨迹（模拟小车运动）"""
        if not self.trajectory_points:
            if not self.load_matrix() or not self.extract_trajectory():
                print("错误: 无法提取轨迹!")
                return

        print("正在生成动画可视化...")

        fig, ax = plt.subplots(figsize=(12, 12))

        # 设置坐标轴
        ax.set_xlim(0, self.width)
        ax.set_ylim(0, self.height)
        ax.set_aspect('equal')
        ax.invert_yaxis()  # 反转y轴，使原点在左上角

        # 设置标题和标签
        ax.set_title(f"Route of the Car (Size: {self.width} x {self.height})", fontsize=16)
        ax.set_xlabel("X", fontsize=12)
        ax.set_ylabel("Y", fontsize=12)

        # 初始化图形元素
        line, = ax.plot([], [], 'b-', linewidth=2, alpha=0.7, label='已行驶路径')
        car = ax.plot([], [], 'ro', markersize=10, zorder=10, label='小车')[0]
        start_point = ax.plot(self.trajectory_points[0][0], self.trajectory_points[0][1],
                              'go', markersize=12, label='起点')[0]

        # 如果轨迹点足够多，标记终点
        if len(self.trajectory_points) > 1:
            end_point = ax.plot(self.trajectory_points[-1][0], self.trajectory_points[-1][1],
                                'yo', markersize=12, label='终点')[0]

        # 添加图例
        ax.legend(loc='best', fontsize=10)

        # 添加网格
        ax.grid(True, alpha=0.3, linestyle='--')

        # 初始化动画数据
        x_data = []
        y_data = []
        current_index = 0

        def init():
            line.set_data([], [])
            car.set_data([], [])
            return line, car

        def update(frame):
            nonlocal current_index, x_data, y_data

            # 每帧更新一个点
            if current_index < len(self.trajectory_points):
                point = self.trajectory_points[current_index]
                x_data.append(point[0])
                y_data.append(point[1])

                # 更新路径线
                line.set_data(x_data, y_data)

                # 更新小车位置
                car.set_data([point[0]], [point[1]])

                current_index += 1

            return line, car

        # 计算帧数
        total_frames = len(self.trajectory_points)

        # 创建动画
        ani = animation.FuncAnimation(
            fig, update, frames=total_frames,
            init_func=init, blit=True, interval=1000 / speed
        )

        # 调整布局
        plt.tight_layout()

        # 保存或显示动画
        if output_file:
            # 保存为GIF
            ani.save(output_file, writer='pillow', fps=speed, dpi=100)
            print(f"动画已保存到: {output_file}")

        plt.show()

    def visualize_matrix(self, output_file=None):
        """可视化原始01矩阵"""
        if self.matrix is None:
            if not self.load_matrix():
                return

        print("正在可视化原始矩阵...")

        fig, ax = plt.subplots(figsize=(12, 12))

        # 显示矩阵
        im = ax.imshow(self.matrix, cmap='binary', interpolation='none')

        # 设置标题
        ax.set_title(f"Route of the Car (Size: {self.width} x {self.height})", fontsize=16)

        # 添加颜色条
        plt.colorbar(im, ax=ax, label='值 (0/1)')

        # 调整布局
        plt.tight_layout()

        # 保存或显示图像
        if output_file:
            plt.savefig(output_file, dpi=150, bbox_inches='tight')
            print(f"矩阵可视化已保存到: {output_file}")

        plt.show()

    def analyze_trajectory(self):
        """分析轨迹的统计信息"""
        if not self.trajectory_points:
            if not self.load_matrix() or not self.extract_trajectory():
                print("错误: 无法提取轨迹!")
                return

        print("\n" + "=" * 50)
        print("轨迹分析报告")
        print("=" * 50)

        print(f"轨迹点数: {len(self.trajectory_points)}")
        print(f"连接线段数: {len(self.connections)}")

        if len(self.trajectory_points) >= 2:
            # 计算轨迹长度
            total_length = 0
            for i in range(len(self.trajectory_points) - 1):
                x1, y1 = self.trajectory_points[i]
                x2, y2 = self.trajectory_points[i + 1]
                distance = np.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
                total_length += distance

            print(f"轨迹总长度: {total_length:.2f} 单位")

            # 起点和终点
            start = self.trajectory_points[0]
            end = self.trajectory_points[-1]
            print(f"起点坐标: ({start[0]}, {start[1]})")
            print(f"终点坐标: ({end[0]}, {end[1]})")

            # 计算起点到终点的直线距离
            direct_distance = np.sqrt((end[0] - start[0]) ** 2 + (end[1] - start[1]) ** 2)
            print(f"起点到终点直线距离: {direct_distance:.2f} 单位")

            # 计算路径效率（直线距离/实际距离）
            if total_length > 0:
                efficiency = direct_distance / total_length
                print(f"路径效率: {efficiency:.2%}")

        # 计算矩阵中1的比例
        if self.matrix is not None:
            ones_count = np.sum(self.matrix)
            total_cells = self.width * self.height
            coverage = ones_count / total_cells
            print(f"矩阵中1的比例: {coverage:.6f} ({ones_count}/{total_cells})")

        print("=" * 50)


def main():
    """主函数"""
    parser = argparse.ArgumentParser(description='小车轨迹可视化工具 - SD卡版')

    # 添加文件参数作为可选参数
    parser.add_argument('file_name', type=str, nargs='?', default=None,
                        help='轨迹矩阵文件路径 (如果未指定则自动从SD卡查找)')

    parser.add_argument('--mode', type=str, default='static',
                        choices=['static', 'animated', 'matrix', 'all'],
                        help='可视化模式: static(静态), animated(动画), matrix(矩阵), all(全部)')
    parser.add_argument('--output', type=str, default=None,
                        help='输出文件路径 (不指定则仅显示)')
    parser.add_argument('--speed', type=int, default=10,
                        help='动画速度 (帧/秒，默认: 10)')
    parser.add_argument('--analyze', action='store_true',
                        help='显示轨迹分析报告')
    parser.add_argument('--no-points', action='store_true',
                        help='静态可视化时不显示轨迹点')
    parser.add_argument('--no-line', action='store_true',
                        help='静态可视化时不显示轨迹线')

    parser.add_argument('--list-sd', action='store_true',
                        help='列出SD卡中的轨迹文件')
    parser.add_argument('--sd-info', action='store_true',
                        help='显示SD卡信息')

    args = parser.parse_args()

    # 显示SD卡信息
    if args.sd_info:
        reader = SDCardReader()
        info = reader.get_sd_card_info()
        print("=== SD卡信息 ===")
        print(f"操作系统: {info['system']}")
        print(f"检测到的SD卡路径: {info['sd_card_paths']}")
        print(f"找到的轨迹文件: {info['detected_files']}")
        return

    # 列出SD卡中的轨迹文件
    if args.list_sd:
        reader = SDCardReader()
        files = reader.find_trajectory_files()
        print("=== SD卡中的轨迹文件 ===")
        if files:
            for i, file_path in enumerate(files, 1):
                print(f"{i}. {file_path}")
        else:
            print("未找到任何轨迹文件")
        return

    # 如果指定了文件，直接使用该文件
    if args.file_name is not None:
        # 创建可视化器
        visualizer = TrajectoryVisualizer(args.file_name)

        # 如果未找到文件，退出
        if visualizer.matrix_file is None:
            sys.exit(1)

        # 加载矩阵
        if not visualizer.load_matrix():
            sys.exit(1)

        # 提取轨迹
        visualizer.extract_trajectory()

        # 分析轨迹
        if args.analyze:
            visualizer.analyze_trajectory()

        # 根据模式进行可视化
        if args.mode == 'static' or args.mode == 'all':
            output_file = None
            if args.output:
                if args.mode == 'all':
                    base_name = os.path.splitext(args.output)[0]
                    output_file = f"{base_name}_static.png"
                else:
                    output_file = args.output

            visualizer.visualize_static(
                output_file=output_file,
                show_points=not args.no_points,
                show_line=not args.no_line
            )

        if args.mode == 'animated' or args.mode == 'all':
            output_file = None
            if args.output:
                if args.mode == 'all':
                    base_name = os.path.splitext(args.output)[0]
                    output_file = f"{base_name}_animated.gif"
                else:
                    output_file = args.output

            visualizer.visualize_animated(
                output_file=output_file,
                speed=args.speed
            )

        if args.mode == 'matrix' or args.mode == 'all':
            output_file = None
            if args.output:
                if args.mode == 'all':
                    base_name = os.path.splitext(args.output)[0]
                    output_file = f"{base_name}_matrix.png"
                else:
                    output_file = args.output

            visualizer.visualize_matrix(output_file=output_file)
            
        return

    # 如果未指定文件，进入交互式循环模式
    reader = SDCardReader()
    
    while True:
        print("\n" + "=" * 50)
        print("请选择要操作的轨迹文件 (输入 'q' 退出程序)")
        print("=" * 50)
        
        # 查找所有轨迹文件
        files = reader.find_trajectory_files()
        
        if not files:
            print("未找到任何轨迹文件，请检查SD卡是否正确插入")
            choice = input("按回车键重新搜索，或输入 'q' 退出: ")
            if choice.lower() == 'q':
                break
            continue
        
        # 显示文件列表
        print("找到的轨迹文件:")
        for i, file_path in enumerate(files, 1):
            print(f"  {i}. {file_path}")
        
        print("  q. 退出程序")
        print("-" * 50)
        
        # 获取用户选择
        choice = input("请选择文件编号 (1-{}) 或输入 'q': ".format(len(files)))
        
        if choice.lower() == 'q':
            print("程序退出")
            break
        
        try:
            index = int(choice) - 1
            if 0 <= index < len(files):
                selected_file = files[index]
                
                # 创建可视化器
                visualizer = TrajectoryVisualizer(selected_file)
                
                # 加载矩阵
                if not visualizer.load_matrix():
                    print("加载文件失败，请重新选择")
                    continue
                
                # 提取轨迹
                visualizer.extract_trajectory()
                
                # 分析轨迹
                if args.analyze:
                    visualizer.analyze_trajectory()
                
                # 根据模式进行可视化
                if args.mode == 'static' or args.mode == 'all':
                    output_file = None
                    if args.output:
                        if args.mode == 'all':
                            base_name = os.path.splitext(args.output)[0]
                            output_file = f"{base_name}_static.png"
                        else:
                            output_file = args.output
                    
                    visualizer.visualize_static(
                        output_file=output_file,
                        show_points=not args.no_points,
                        show_line=not args.no_line
                    )
                
                if args.mode == 'animated' or args.mode == 'all':
                    output_file = None
                    if args.output:
                        if args.mode == 'all':
                            base_name = os.path.splitext(args.output)[0]
                            output_file = f"{base_name}_animated.gif"
                        else:
                            output_file = args.output
                    
                    visualizer.visualize_animated(
                        output_file=output_file,
                        speed=args.speed
                    )
                
                if args.mode == 'matrix' or args.mode == 'all':
                    output_file = None
                    if args.output:
                        if args.mode == 'all':
                            base_name = os.path.splitext(args.output)[0]
                            output_file = f"{base_name}_matrix.png"
                        else:
                            output_file = args.output
                    
                    visualizer.visualize_matrix(output_file=output_file)
            else:
                print("无效的编号，请重新选择")
        except ValueError:
            print("无效的输入，请重新选择")

if __name__ == "__main__":
    main()
# [file content end]