        } else if (path_points_.size() > 2) {
            // 中间点
            double dx = path_points_[i + 1].x - path_points_[i - 1].x;
            double dy = path_points_[i + 1].y - path_points_[i - 1].y;
            path_points_[i].theta = std::atan2(dy, dx);

            // 计算曲率（使用三点法）
            if (i > 0 && i < path_points_.size() - 1) {
                path_points_[i].kappa = computeCurvature(
                    path_points_[i - 1], path_points_[i], path_points_[i + 1]);
            }
        }
        // 单点情况：保持原有的theta和kappa值（通常为0）
        // 不需要额外计算，避免越界访问
