活动介绍

std::vector<std::vector<Point2f>> trajectories; 这个嵌套vector通过resize 如何预分配内存?

时间: 2025-06-17 18:00:08 浏览: 11
为了使用 `resize` 方法为嵌套的 `std::vector<std::vector<Point2f>>` 数据结构预分配内存,需要理解两层向量的行为及其初始化方式。以下是具体实现和说明: ### 1. 预分配内存的核心概念 `std::vector<T>::resize(n)` 函数的作用是调整向量的大小为 `n`,如果当前容量小于 `n` 则会扩展并向新位置填充默认值[^1]。对于嵌套的 `std::vector<std::vector<Point2f>>` 来说,这意味着不仅外部向量会被调整大小,内部的每一个子向量也需要单独设置其大小。 --- ### 2. 实现细节 假设我们需要创建一个二维向量矩阵,其中外层数组有 `rows` 行,每行包含 `cols` 列,默认值为 `(0.0f, 0.0f)` 的 `Point2f` 类型元素,则可以按照以下方式进行初始化: ```cpp #include <iostream> #include <vector> using namespace std; struct Point2f { float x; float y; }; int main() { size_t rows = 5; // 外部向量的大小 size_t cols = 3; // 每个内部向量的大小 // 创建并预分配内存给嵌套向量 std::vector<std::vector<Point2f>> matrix(rows, std::vector<Point2f>(cols, {0.0f, 0.0f})); // 或者通过 resize 方法手动调整大小 std::vector<std::vector<Point2f>> nested_vector; nested_vector.resize(rows); // 调整外部向量大小 for (auto& row : nested_vector) { row.resize(cols, {0.0f, 0.0f}); // 调整每个内部向量大小 } // 输出验证 for (const auto& row : nested_vector) { for (const auto& point : row) { cout << "(" << point.x << ", " << point.y << ") "; } cout << endl; } return 0; } ``` 在此代码中: - 第一种方法利用了构造函数直接初始化整个嵌套向量。 - 第二种方法展示了如何通过 `resize` 手动控制每一层的大小,并提供初始值 `{0.0f, 0.0f}` 给所有的 `Point2f` 元素[^2]。 --- ### 3. 关键点分析 - **内外层关系**:外部向量决定了有多少个子向量(即多少行),而每个子向量决定了一行中有多少列。 - **默认值设定**:无论是通过构造函数还是 `resize` 方法,都需要明确指定内部元素的默认状态以保持一致性。 - **性能优化**:提前调用 `reserve` 可进一步提升效率,尤其是在知道确切数量前提下避免多次重新分配带来的开销。 ---
阅读全文

相关推荐

#include "dml-module.h" #include "cap.h" #include "cap_win.h" #include <nms.h> #include <opencv2/opencv.hpp> #include "AIMBOT.cpp" #include "mouse_controller.h" #include <windows.h> #include <chrono> #include <string> #include <filesystem> #include <iostream> #include <functional> #include <atomic> #include <mutex> #include <vector> #include <algorithm> #include <optional> #include "gbil_mouse_controller.h" #include "config.h" // ===== 新增:ADRC控制器结构体(自抗扰控制,解决动态目标扰动问题)===== struct ADRCController { float w0; // 观测器带宽 float b0; // 控制增益 float w_n; // 闭环带宽 float sigma; // 阻尼系数 float time_delta; // 时间步长(ms转换为秒) // 扩张状态观测器变量 float z1{ 0 }, z2{ 0 }, z3{ 0 }; // 状态观测值(位置、速度、扰动) float u_p{ 0 }; // 上一时刻控制量 std::mutex mtx; // 初始化ADRC参数(从配置文件读取) ADRCController(float w0_, float b0_, float w_n_, float sigma_, float dt) : w0(w0_), b0(b0_), w_n(w_n_), sigma(sigma_), time_delta(dt) { } // *核心方法:根据目标值和当前值计算补偿后的控制量 float update(float target, float current) { std::lock_guard<std::mutex> lock(mtx); float err = current - z1; // 跟踪误差 // 扩张状态观测器(ESO)更新 float fhan = this->fhan(err, z2, w_n, sigma); z1 += time_delta * (z2 + w0 * w0 * err); // 位置观测 z2 += time_delta * (z3 + 2 * w0 * z2 + w0 * w0 * fhan); // 速度观测 z3 += time_delta * (-3 * w0 * z3 + w0 * w0 * w0 * (current - z1)); // 扰动观测 // 控制律计算(补偿扰动) float u0 = w_n * w_n * (target - z1) - 2 * sigma * w_n * z2; float u = (u0 - z3) / b0; u_p = u; // 保存当前控制量 return u; } // 快速跟踪微分器(处理非线性特性) float fhan(float x1, float x2, float r, float h0) { float d = r * h0 * h0; float d0 = h0 * d; float y = x1 + h0 * x2; float a0 = sqrt(d * (d + 8 * abs(y))); float a; if (abs(y) > d0) { a = x2 + (a0 - d) / 2 * (y > 0 ? 1 : -1); } else { a = x2 + y / h0; } if (abs(a) > d) { return -r * (a > 0 ? 1 : -1); } else { return -r * a / d; } } void reset() { std::lock_guard<std::mutex> lock(mtx); z1 = z2 = z3 = 0; u_p = 0; } }; // ===== PID控制器结构体(保留原有功能,与ADRC级联)===== struct PIDController { float kp, ki, kd; float integral{ 0 }, last_error{ 0 }; std::mutex mtx; PIDController(float p, float i, float d) : kp(p), ki(i), kd(d) {} float compute(float error) { std::lock_guard<std::mutex> lock(mtx); integral += error; float derivative = error - last_error; last_error = error; return kp * error + ki * integral + kd * derivative; } void reset() { std::lock_guard<std::mutex> lock(mtx); integral = 0; last_error = 0; } }; // ===== 卡尔曼滤波类(参数可配置)===== class KalmanFilter { public: KalmanFilter(float process_noise, float obs_noise) { kf.init(4, 2, 0); kf.transitionMatrix = (cv::Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); cv::setIdentity(kf.measurementMatrix); cv::setIdentity(kf.processNoiseCov, cv::Scalar::all(process_noise)); cv::setIdentity(kf.measurementNoiseCov, cv::Scalar::all(obs_noise)); cv::setIdentity(kf.errorCovPost, cv::Scalar::all(1)); } cv::Point2f predict(float x, float y) { cv::Mat measurement = (cv::Mat_<float>(2, 1) << x, y); kf.predict(); cv::Mat estimated = kf.correct(measurement); return cv::Point2f(estimated.at<float>(0), estimated.at<float>(1)); } private: cv::KalmanFilter kf; }; // ===== 新增:贝塞尔曲线生成工具(解决移动拖影,实现类人平滑路径)===== class BezierCurve { public: // 生成从起点到终点的贝塞尔曲线路径点 static std::vector<cv::Point2f> generatePoints( cv::Point2f start, cv::Point2f end, int num_points = 10) { // 路径点数量(越多越平滑)数值越大越慢 std::vector<cv::Point2f> points; // 生成随机内点(增加路径自然性) std::vector<cv::Point2f> control_points; control_points.push_back(start); // 添加2个随机内点(在起点和终点范围内) control_points.push_back(cv::Point2f( start.x + (end.x - start.x) * 0.3f + rand() % 5 - 2, // 微小随机偏移 start.y + (end.y - start.y) * 0.3f + rand() % 5 - 2 )); control_points.push_back(cv::Point2f( start.x + (end.x - start.x) * 0.7f + rand() % 5 - 2, start.y + (end.y - start.y) * 0.7f + rand() % 5 - 2 )); control_points.push_back(end); // 贝塞尔曲线采样 for (int i = 0; i < num_points; ++i) { float t = (float)i / (num_points - 1); points.push_back(bezier(t, control_points)); } return points; } private: // 贝塞尔曲线计算 static cv::Point2f bezier(float t, const std::vector<cv::Point2f>& points) { int n = points.size() - 1; cv::Point2f result(0, 0); for (int i = 0; i <= n; ++i) { float bernstein = binomial(n, i) * pow(t, i) * pow(1 - t, n - i); result.x += points[i].x * bernstein; result.y += points[i].y * bernstein; } return result; } // 二项式系数计算 static float binomial(int n, int k) { if (k < 0 || k > n) return 0; if (k == 0 || k == n) return 1; k = std::min(k, n - k); float res = 1; for (int i = 1; i <= k; ++i) { res = res * (n - k + i) / i; } return res; } }; // ===== 目标选择函数(安全封装)===== std::optional<Box> selectTarget( std::vector<Box>& filtered, int& tracked_id, int& tracking_counter, float anchor_x, float anchor_y, int width, int height ) { // 优先选择上次追踪目标 for (auto& box : filtered) { if (box.id == tracked_id) { tracking_counter++; return box; } } // 未找到则选择距离中心最近的目标 if (!filtered.empty()) { float min_dist = FLT_MAX; Box* best_target = nullptr; for (auto& box : filtered) { float cx = box.left + (box.right - box.left) * anchor_x; float cy = box.top + (box.bottom - box.top) * anchor_y; float dist = std::hypot(cx - width / 2.0f, cy - height / 2.0f); if (dist < min_dist) { min_dist = dist; best_target = &box; } } if (best_target) { tracked_id = best_target->id; tracking_counter = 1; return *best_target; } } return std::nullopt; } int main() { // ------ 新增:检查自身实例是否已在运行 开始 ------ // 通过创建全局命名互斥锁(Named Mutex)来判断是否已有一个实例在运行 HANDLE hInstanceMutex = CreateMutexW(nullptr, FALSE, L"Global\\GblidDemoVC_Mutex"); if (!hInstanceMutex || GetLastError() == ERROR_ALREADY_EXISTS) { MessageBoxW(NULL, L"请勿重复运行", L"提示", MB_OK | MB_ICONWARNING); if (hInstanceMutex) { CloseHandle(hInstanceMutex); } ExitProcess(0); // 已有实例,退出当前进程 } // ------ 新增:检查自身实例是否已在运行 结束 ------ // 隐藏控制台窗口 HWND consoleWindow = GetConsoleWindow(); // 获取当前控制台窗口句柄 ShowWindow(consoleWindow, SW_HIDE); // 隐藏控制台窗口(如果需要调试可以注释掉这行)//==========需要调试可以注释================== // ==== 加载配置 ==== Config config; if (!config.loadFromFile("./config.ini")) { printf("主程序-配置文件加载失败!\n"); return -1; } bool baim = (config.aim_part == 0); timeBeginPeriod(1); // 设置定时器精度为1ms // === 自动加载模型 === std::string model_dir = "onnx"; std::string model_path; for (const auto& entry : std::filesystem::directory_iterator(model_dir)) { if (entry.path().extension() == ".onnx") { model_path = entry.path().string(); break; } } if (model_path.empty()) { printf("主程序-未在 'onnx' 文件夹中找到任何 .onnx 模型文件!\n"); return -1; } printf("主程序-加载模型路径: %s\n", model_path.c_str()); // 初始化模型类 auto* frame = new IDML(); if (!frame->AnalyticalModel(model_path)) { printf("主程序-模型加载失败。\n"); return -2; } const auto& input_dims = frame->getInputDims(); if (input_dims.size() != 4) { printf("主程序-模型输入维度异常,当前维度数量: %llu\n", input_dims.size()); return -3; } int channels = static_cast<int>(input_dims[1]); int height = static_cast<int>(input_dims[2]); int width = static_cast<int>(input_dims[3]); printf("主程序-输入图像尺寸: %d x %d\n", width, height); // ==== 初始化控制器(*修改:新增ADRC控制器)==== PIDController pid_x(config.xkp, config.xki, config.xkd); PIDController pid_y(config.ykp, config.yki, config.ykd); // *ADRC参数(从配置文件读取,可根据动态目标特性调整) float adrc_dt = 0.016f; // 约60FPS的时间步长(1/60≈0.016s) ADRCController adrc_x(config.adrc_w0, config.adrc_b0, config.adrc_wn, config.adrc_sigma, adrc_dt); ADRCController adrc_y(config.adrc_w0, config.adrc_b0, config.adrc_wn, config.adrc_sigma, adrc_dt); KalmanFilter kf(config.kalman_Q, config.kalman_R); printf("主程序-控制参数: PID X[%.2f, %.3f, %.2f] Y[%.2f, %.3f, %.2f]\n", config.xkp, config.xki, config.xkd, config.ykp, config.yki, config.ykd); printf("主程序-ADRC参数: w0=%.1f, b0=%.2f, wn=%.1f, sigma=%.1f\n", config.adrc_w0, config.adrc_b0, config.adrc_wn, config.adrc_sigma); int screenW = GetSystemMetrics(SM_CXSCREEN); int screenH = GetSystemMetrics(SM_CYSCREEN); std::function<BYTE* ()> capture_func; void* cap_impl = nullptr; // 选择截图方式 if (config.screenshot_method == 0) { auto* c = new capture(screenW, screenH, width, height, "CrossFire"); cap_impl = c; capture_func = [c]() -> BYTE* { return (BYTE*)c->cap(); }; printf("主程序-截图方式: DXGI\n"); } else { auto* c = new capture_win32(screenW, screenH, width, height, "CrossFire"); cap_impl = c; capture_func = [c]() -> BYTE* { return c->cap(); }; printf("主程序-截图方式: Win32 BitBlt\n"); } // 初始化鼠标控制器 IMouseController* mouse = nullptr; if (config.move_type == 0) { mouse = new WinMouseController(); printf("主程序-使用鼠标控制器: WinMove\n"); } else if (config.move_type == 1) { mouse = new GBILMouseController(); printf("主程序-使用鼠标控制器: GBIL\n"); } else { printf("主程序-未知的 move_type 类型: %d,未启用鼠标移动!\n", config.move_type); } using Clock = std::chrono::high_resolution_clock; auto last_time = Clock::now(); float fps = 0.0f; // ==== 持续追踪状态 ==== static int tracked_id = -1; static int tracking_counter = 0; static int lost_counter = 0; // ==== 跟踪功能开关状态 ==== std::atomic<bool> tracking_enabled{ true }; bool last_tracking_toggle_state = false; bool last_exit_key_state = false; // ==== 新增:记录上一时刻鼠标位置(用于贝塞尔路径生成)==== cv::Point2f last_mouse_pos(0, 0); // ==== 主循环 ==== static bool target_locked = false; static int lock_frames_counter = 0; static float last_min_dist = FLT_MAX; while (true) { auto start = Clock::now(); // ==== 检查退出键 ==== bool current_exit_state = (GetAsyncKeyState(config.exit_key) & 0x8000) != 0; if (current_exit_state && !last_exit_key_state) { // 使用MB_SYSTEMMODAL标志使对话框置顶 [1,8](@ref) int result = MessageBoxA( NULL, "确定要退出YAK吗?", "退出确认", MB_OKCANCEL | MB_ICONQUESTION | MB_SYSTEMMODAL // 添加MB_SYSTEMMODAL标志 ); if (result == IDOK) { break; } } last_exit_key_state = current_exit_state; // ==== 检查跟踪开关键 ==== bool current_tracking_toggle = (GetAsyncKeyState(config.tracking_toggle_key) & 0x8000) != 0; if (current_tracking_toggle && !last_tracking_toggle_state) { tracking_enabled = !tracking_enabled; printf("主程序-跟踪功能已%s\n", tracking_enabled ? "开启" : "关闭"); } last_tracking_toggle_state = current_tracking_toggle; // 截图 BYTE* s = capture_func(); auto after_cap = Clock::now(); // 目标检测 float* data = frame->Detect(s); auto after_detect = Clock::now(); // 计算性能指标 float capture_ms = std::chrono::duration<float, std::milli>(after_cap - start).count(); float detect_ms = std::chrono::duration<float, std::milli>(after_detect - after_cap).count(); float frame_time = std::chrono::duration<float>(after_detect - last_time).count(); last_time = after_detect; fps = 1.0f / frame_time; std::vector<Box> oldbox; std::vector<Box> newbox = generate_yolo_proposals(data, oldbox, frame->out1, frame->out2, config.conf_threshold, config.nms_threshold); // ==== 目标过滤 ==== static std::vector<Box> filtered; filtered.clear(); for (auto& box : newbox) { if (config.valid_classes.empty() || std::find(config.valid_classes.begin(), config.valid_classes.end(), box.class_label) != config.valid_classes.end()) { filtered.push_back(box); } } // ==== 目标选择与锁定机制 ==== std::optional<Box> current_target = std::nullopt; if (tracking_enabled) { if (target_locked) { for (auto& box : filtered) { if (box.id == tracked_id) { current_target = box; tracking_counter++; lost_counter = 0; break; } } if (!current_target) { if (++lost_counter > 5) { target_locked = false; tracked_id = -1; tracking_counter = 0; lost_counter = 0; last_min_dist = FLT_MAX; } } } else if (!filtered.empty()) { float min_dist = FLT_MAX; Box* best_target = nullptr; for (auto& box : filtered) { float cx = box.left + (box.right - box.left) * config.anchor_x; float cy = box.top + (box.bottom - box.top) * config.anchor_y; float dist = std::hypot(cx - width / 2.0f, cy - height / 2.0f); if (dist < min_dist && (tracked_id == -1 || dist < last_min_dist * 0.8f)) { min_dist = dist; best_target = &box; } } if (best_target) { current_target = *best_target; tracked_id = best_target->id; tracking_counter = 1; last_min_dist = min_dist; if (min_dist < config.lock_threshold) { if (++lock_frames_counter >= config.lock_frames) { target_locked = true; lock_frames_counter = 0; } } else { lock_frames_counter = 0; } } } } // ==== 鼠标控制逻辑(*核心修改:PID+ADRC级联控制+贝塞尔路径平滑)==== bool aim_key_pressed = (GetAsyncKeyState(config.aim_key) & 0x8000) != 0; bool aim_key1_pressed = (GetAsyncKeyState(config.aim_key1) & 0x8000) != 0; bool any_aim_key_pressed = aim_key_pressed || aim_key1_pressed; if (any_aim_key_pressed && mouse && tracking_enabled) { if (current_target) { // 计算锚点位置 float center_x = current_target->left + (current_target->right - current_target->left) * config.anchor_x; float center_y = current_target->top + (current_target->bottom - current_target->top) * config.anchor_y; cv::Point2f smoothed; if (config.kalman_enable) { smoothed = kf.predict(center_x, center_y); } else { smoothed = cv::Point2f(center_x, center_y); } // 计算原始偏移(目标与屏幕中心的偏差) float raw_dx = smoothed.x - width / 2.0f; float raw_dy = smoothed.y - height / 2.0f; // *步骤1:PID计算基础移动量(比例+积分+微分) float pid_dx = pid_x.compute(raw_dx); float pid_dy = pid_y.compute(raw_dy); // *步骤2:ADRC补偿动态扰动(解决动态目标跟踪延迟) float adrc_dx = adrc_x.update(pid_dx, raw_dx); // 输入PID输出和原始偏差 float adrc_dy = adrc_y.update(pid_dy, raw_dy); // *步骤3:贝塞尔曲线生成平滑路径(解决移动拖影) cv::Point2f target_pos(last_mouse_pos.x + adrc_dx, last_mouse_pos.y + adrc_dy); std::vector<cv::Point2f> path = BezierCurve::generatePoints(last_mouse_pos, target_pos); // *步骤4:按平滑路径移动鼠标 for (const auto& p : path) { mouse->updateTarget(p.x - last_mouse_pos.x, p.y - last_mouse_pos.y); last_mouse_pos = p; // 更新当前鼠标位置 Sleep(1); // 微小延迟,确保平滑效果 } } else { // 无目标时重置 mouse->updateTarget(0.0f, 0.0f); pid_x.reset(); pid_y.reset(); adrc_x.reset(); // *重置ADRC adrc_y.reset(); last_mouse_pos = cv::Point2f(0, 0); target_locked = false; tracked_id = -1; lock_frames_counter = 0; } } else { // 未按触发键时重置 if (mouse) mouse->updateTarget(0.0f, 0.0f); target_locked = false; tracked_id = -1; tracking_counter = 0; lock_frames_counter = 0; pid_x.reset(); pid_y.reset(); adrc_x.reset(); // *重置ADRC adrc_y.reset(); last_mouse_pos = cv::Point2f(0, 0); } // ==== 调试窗口显示 ==== if (config.show_window) { cv::Mat a(height, width, CV_8UC3, s); if (!newbox.empty()) { for (const Box& detection : newbox) { cv::Scalar color = (current_target && detection.id == current_target->id) ? cv::Scalar(0, 0, 255) : cv::Scalar(0, 255, 0); int thickness = (current_target && detection.id == current_target->id) ? 2 : 1; cv::rectangle(a, cv::Point((int)detection.left, (int)detection.top), cv::Point((int)detection.right, (int)detection.bottom), color, thickness ); std::string class_str = std::to_string(detection.class_label); char conf_str[16]; snprintf(conf_str, sizeof(conf_str), "%.2f", detection.confidence); int fontFace = cv::FONT_HERSHEY_SIMPLEX; double fontScale = 0.4; int textThickness = 1; cv::Point textOrg((int)detection.left, (int)detection.top - 4); if (textOrg.y < 10) textOrg.y = 10; cv::putText(a, class_str, textOrg, fontFace, fontScale, cv::Scalar(0, 0, 255), textThickness); cv::Size textSize = cv::getTextSize(class_str, fontFace, fontScale, textThickness, nullptr); cv::Point confOrg = textOrg + cv::Point(textSize.width + 4, 0); cv::putText(a, conf_str, confOrg, fontFace, fontScale, cv::Scalar(0, 255, 0), textThickness); } } // 绘制锚点和追踪线 if (current_target && any_aim_key_pressed && tracking_enabled) { int anchor_x_pos = current_target->left + (current_target->right - current_target->left) * config.anchor_x; int anchor_y_pos = current_target->top + (current_target->bottom - current_target->top) * config.anchor_y; cv::circle(a, cv::Point(anchor_x_pos, anchor_y_pos), 5, cv::Scalar(255, 0, 255), -1); cv::line(a, cv::Point(width / 2, height / 2), cv::Point(anchor_x_pos, anchor_y_pos), cv::Scalar(0, 255, 255), 1); } // 性能指标显示 int base_y = 20; int line_height = 18; double font_scale = 0.5; int thickness = 1; auto draw_metric = [&](const std::string& label, const std::string& value, int line) { int y = base_y + line * line_height; cv::putText(a, label, cv::Point(10, y), cv::FONT_HERSHEY_SIMPLEX, font_scale, cv::Scalar(0, 0, 255), thickness); cv::putText(a, value, cv::Point(80, y), cv::FONT_HERSHEY_SIMPLEX, font_scale, cv::Scalar(0, 255, 0), thickness); }; draw_metric("FPS:", std::to_string((int)fps), 0); draw_metric("CAPTURE:", std::to_string((int)capture_ms) + "ms", 1); draw_metric("DETECT:", std::to_string((int)detect_ms) + "ms", 2); draw_metric("TRACK:", tracking_enabled ? "ON" : "OFF", 3); // 状态信息显示 if (current_target) { std::string track_info = "TRACK: " + std::to_string(tracking_counter); cv::putText(a, track_info, cv::Point(width - 120, 30), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(200, 200, 0), 1); } if (target_locked) { std::string lock_info = "LOCKED: " + std::to_string(tracked_id); cv::putText(a, lock_info, cv::Point(width - 120, 60), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 200, 255), 1); } if (lost_counter > 0) { std::string lost_info = "LOST: " + std::to_string(lost_counter); cv::putText(a, lost_info, cv::Point(width - 120, 90), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 200, 200), 1); } // 窗口置顶设置 static bool topmost_set = false; if (!topmost_set) { HWND hwnd = FindWindowA(NULL, "c"); if (hwnd) { SetWindowPos(hwnd, HWND_TOPMOST, 0, 0, 0, 0, SWP_NOMOVE | SWP_NOSIZE | SWP_SHOWWINDOW); } topmost_set = true; } cv::imshow("c", a); cv::waitKey(1); } } // 退出前释放资源 if (mouse) { mouse->stop(); delete mouse; mouse = nullptr; } delete frame; if (cap_impl) { if (config.screenshot_method == 0) { delete static_cast<capture*>(cap_impl); } else { delete static_cast<capture_win32*>(cap_impl); } } timeEndPeriod(1); return 0; }以上代码你详细的分析一下,为什么会跟不上目标,跟踪的时候还有拖影

cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( const int trajectory_id) { cartographer_ros_msgs::StatusResponse status_response; if (trajectories_scheduled_for_finish_.count(trajectory_id)) { status_response.message = absl::StrCat("Trajectory ", trajectory_id, " already pending to finish."); status_response.code = cartographer_ros_msgs::StatusCode::OK; LOG(INFO) << status_response.message; return status_response; } // First, check if we can actually finish the trajectory. status_response = TrajectoryStateToStatus( trajectory_id, {TrajectoryState::ACTIVE} /* valid states */); if (status_response.code != cartographer_ros_msgs::StatusCode::OK) { LOG(ERROR) << "Can't finish trajectory: " << status_response.message; return status_response; } // Shutdown the subscribers of this trajectory. // A valid case with no subscribers is e.g. if we just visualize states. if (0) { if (subscribers_.count(trajectory_id)) { for (auto& entry : subscribers_[trajectory_id]) { entry.subscriber.shutdown(); subscribed_topics_.erase(entry.topic); LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]"; } CHECK_EQ(subscribers_.erase(trajectory_id), 1); } } else { // for debug if (subscribers_.count(trajectory_id)) { for (auto& entry : subscribers_[trajectory_id]) { try { LOG(INFO) << "Attempting to shutdown subscriber of [" << entry.topic << "]"; entry.subscriber.shutdown(); LOG(INFO) << "Successfully shutdown subscriber of [" << entry.topic << "]"; } catch (const std::exception& e) { LOG(ERROR) << "Error shutting down subscriber of [" << entry.topic << "]: " << e.what(); } try { LOG(INFO) << "Attempting to erase topic [" << entry.topic << "] from subscribed_topics_"; auto it = subscribed_topics_.find(entry.topic); if (it != subscribed_topics_.end()) { subscribed_topics_.erase(it); LOG(INFO) << "Successfully erased topic [" << entry.topic << "] from subscribed_topics_"; } else { LOG(WARNING) << "Topic [" << entry.topic << "] not found in subscribed_topics_"; } } catch (const std::exception& e) { LOG(ERROR) << "Error erasing topic [" << entry.topic << "] from subscribed_topics_: " << e.what(); } } CHECK_EQ(subscribers_.erase(trajectory_id), 1); } } map_builder_bridge_.FinishTrajectory(trajectory_id); trajectories_scheduled_for_finish_.emplace(trajectory_id); status_response.message = absl::StrCat("Finished trajectory ", trajectory_id, "."); status_response.code = cartographer_ros_msgs::StatusCode::OK; return status_response; }这个函数在shutdown 时卡死会有什么原因

下面的代码是干什么用的,请生成说明注释,: 【import rospy, math from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg from geometry_msgs.msg import PolygonStamped, Point32, Quaternion from tf.transformations import euler_from_quaternion import numpy as np import scipy.io as sio import time def feedback_callback(data): global got_data if not data.trajectories: # empty trajectory = [] return if got_data: return got_data = True # copy trajectory trajectories = [] for traj in data.trajectories: trajectory = [] # # store as struct and cell array # for point in traj.trajectory: # (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w]) # pose = {'x': point.pose.position.x, 'y': point.pose.position.y, 'theta': yaw} # velocity = {'v': point.velocity.linear.x, 'omega': point.velocity.angular.z} # time_from_start = point.time_from_start.to_sec() # trajectory.append({'pose': pose, 'velocity': velocity, 'time_from_start': time_from_start}) # store as all-in-one mat arr = np.zeros([6, len(traj.trajectory)], dtype='double'); # x, y, theta, v, omega, t for index, point in enumerate(traj.trajectory): arr[0,index] = point.pose.position.x arr[1,index] = point.pose.position.y (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w]) arr[2,index] = yaw arr[3,index] = point.velocity.linear.x arr[4,index] = point.velocity.angular.z arr[5,index] = point.time_from_start.to_sec() # trajectories.append({'raw': trajectory, 'mat': arr}) trajectories.append({'data': arr, 'legend': ['x','y','theta','v','omega','t']}) # copy obstacles obstacles = [] for obst_id, obst in enumerate(data.obstacle_msg.obstacles): #polygon = [] #for point in obst.polygon.points: # polygon.append({'x': point.x, 'y': point.y, 'z': point.z}) obst_arr = np.zeros([4, len(obst.polygon.points)], dtype='double'); # x, y for index, point in enumerate(obst.polygon.points): obst_arr[0, index] = point.x obst_arr[1, index] = point.y obst_arr[2, index] = data.obstacle_msg.velocities[obst_id].twist.linear.x obst_arr[3, index] = data.obstacle_msg.velocities[obst_id].twist.linear.y #obstacles.append(polygon) obstacles.append({'data': obst_arr, 'legend': ['x','y', 'v_x', 'v_y']}) # create main struct: mat = {'selected_trajectory_idx': data.selected_trajectory_idx, 'trajectories': trajectories, 'obstacles': obstacles} timestr = time.strftime("%Y%m%d_%H%M%S") filename = '/home/albers/MasterThesis/Matlab/Homotopie/test_optim_node/' + 'teb_data_' + timestr + '.mat' rospy.loginfo("Saving mat-file '%s'.", filename) sio.savemat(filename, mat) def feedback_exporter(): global got_data rospy.init_node("export_to_mat", anonymous=True) topic_name = "/test_optim_node/teb_feedback" # define feedback topic here! rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) rospy.loginfo("Waiting for feedback message on topic %s.", topic_name) r = rospy.Rate(2) # define rate here while not rospy.is_shutdown(): if got_data: rospy.loginfo("Data export completed.") return r.sleep() if __name__ == '__main__': try: global got_data got_data = False feedback_exporter() except rospy.ROSInterruptException: pass】

放在下面代码里: def show_images(n_clicks, floor_input, current_index, firmware, floor): if floor is None: return html.Div([html.H1(‘Please select a floorplan!’)]), 0 if current_index is None: current_index = 0 new_index = current_index + STEP_SIZE_OF_TRAJECTORY_IMAGES_TO_PLOT incomplete = False outlierdict = get_outlier_in_firmware(firmware) print(f"outlierdict={outlierdict}") trajectories = [] heatmaps = [] areas = [] cleaning_times = [] bumps = [] bumps_heavy = [] failreasons = [] tarball_links = [] if n_clicks is None and new_index > STEP_SIZE_OF_TRAJECTORY_IMAGES_TO_PLOT: raise PreventUpdate if (new_index <= len(outlierdict[floor])) or new_index < len(outlierdict[floor]) + STEP_SIZE_OF_TRAJECTORY_IMAGES_TO_PLOT: for outlier in outlierdict[floor][0:new_index]: if not incomplete: if "autonomous_dock" not in outlier["fails"]: trajectories.append(outlier["trajectory_png"]) heatmaps.append(outlier["distribution_png"]) areas.append(outlier["area"]) cleaning_times.append(outlier["total_cleaning_time"]) bumps.append(outlier["bumps"]) bumps_heavy.append(outlier["heavy_bumps"]) failreasons.append(outlier["fails"]) if "tarball_location" in outlier: tarball_links.append(outlier["tarball_location"]) else: tarball_links.append(None) else: if "autonomous_dock" in outlier["fails"]: trajectories.append(outlier["trajectory_png"]) heatmaps.append(outlier["distribution_png"]) areas.append(outlier["area"]) cleaning_times.append(outlier["total_cleaning_time"]) bumps.append(outlier["bumps"]) bumps_heavy.append(outlier["heavy_bumps"]) failreasons.append(outlier["fails"]) if "tarball_location" in outlier: tarball_links.append(outlier["tarball_location"]) else: tarball_links.append(None)

最新推荐

recommend-type

【税会实务】Excel文字输入技巧.doc

【税会实务】Excel文字输入技巧.doc
recommend-type

中职计算机教学大纲(1).docx

中职计算机教学大纲(1).docx
recommend-type

C++实现的DecompressLibrary库解压缩GZ文件

根据提供的文件信息,我们可以深入探讨C++语言中关于解压缩库(Decompress Library)的使用,特别是针对.gz文件格式的解压过程。这里的“lib”通常指的是库(Library),是软件开发中用于提供特定功能的代码集合。在本例中,我们关注的库是用于处理.gz文件压缩包的解压库。 首先,我们要明确一个概念:.gz文件是一种基于GNU zip压缩算法的压缩文件格式,广泛用于Unix、Linux等操作系统上,对文件进行压缩以节省存储空间或网络传输时间。要解压.gz文件,开发者需要使用到支持gzip格式的解压缩库。 在C++中,处理.gz文件通常依赖于第三方库,如zlib或者Boost.IoStreams。codeproject.com是一个提供编程资源和示例代码的网站,程序员可以在该网站上找到现成的C++解压lib代码,来实现.gz文件的解压功能。 解压库(Decompress Library)提供的主要功能是读取.gz文件,执行解压缩算法,并将解压缩后的数据写入到指定的输出位置。在使用这些库时,我们通常需要链接相应的库文件,这样编译器在编译程序时能够找到并使用这些库中定义好的函数和类。 下面是使用C++解压.gz文件时,可能涉及的关键知识点: 1. Zlib库 - zlib是一个用于数据压缩的软件库,提供了许多用于压缩和解压缩数据的函数。 - zlib库支持.gz文件格式,并且在多数Linux发行版中都预装了zlib库。 - 在C++中使用zlib库,需要包含zlib.h头文件,同时链接z库文件。 2. Boost.IoStreams - Boost是一个提供大量可复用C++库的组织,其中的Boost.IoStreams库提供了对.gz文件的压缩和解压缩支持。 - Boost库的使用需要下载Boost源码包,配置好编译环境,并在编译时链接相应的Boost库。 3. C++ I/O操作 - 解压.gz文件需要使用C++的I/O流操作,比如使用ifstream读取.gz文件,使用ofstream输出解压后的文件。 - 对于流操作,我们常用的是std::ifstream和std::ofstream类。 4. 错误处理 - 解压缩过程中可能会遇到各种问题,如文件损坏、磁盘空间不足等,因此进行适当的错误处理是必不可少的。 - 正确地捕获异常,并提供清晰的错误信息,对于调试和用户反馈都非常重要。 5. 代码示例 - 从codeproject找到的C++解压lib很可能包含一个或多个源代码文件,这些文件会包含解压.gz文件所需的函数或类。 - 示例代码可能会展示如何初始化库、如何打开.gz文件、如何读取并处理压缩数据,以及如何释放资源等。 6. 库文件的链接 - 编译使用解压库的程序时,需要指定链接到的库文件,这在不同的编译器和操作系统中可能略有不同。 - 通常,在编译命令中加入-l参数,比如使用g++的话可能是`g++ -o DecompressLibrary DecompressLibrary.cpp -lz`,其中`-lz`表示链接zlib库。 7. 平台兼容性 - 在不同平台上使用解压库可能需要考虑平台兼容性问题。 - Windows系统可能需要额外的配置和库文件,因为zlib或其他库可能不是默认预装的。 根据以上知识点,我们可以得出,在C++中解压.gz文件主要涉及到对zlib或类似库的使用,以及熟悉C++的I/O操作。正确使用这些库,能够有效地对压缩文件进行解压,并处理可能出现的错误情况。如果从codeproject获取到的C++解压lib确实是针对.gz文件格式的,那么它很可能已经封装好了大部分的操作细节,让开发者能够以更简单的方式实现解压功能。
recommend-type

【数据融合技术】:甘肃土壤类型空间分析中的专业性应用

# 摘要 数据融合技术作为一种集成多源数据信息的方法,在土壤类型空间分析中发挥着关键作用。本文介绍了数据融合技术的基本概念及其理论基础,阐述了数据预处理、同步整合及冲突解决等关键技术,并详细描述了甘肃土壤类型数据准备的流程,包括数据采集、质量评估、空间化处理及融合实践准备。通过具体案例分析,展示了数据融合在土壤类型空间分布分析、土壤质量评估及土壤保护规划中的应用。同时,文
recommend-type

VM ware如何查看软件版本信息

<think>首先,用户的问题是:“我想查找VMware软件版本信息的查看方法请问如何在VMware中查看软件版本信息”。用户参考了之前的引用,但那些引用主要是关于查看VMware版本的一般方法,现在用户具体想知道在VMware软件中如何查看版本信息。回顾之前的对话:-用户第一次问的是VSCode的版本查看方法。-现在用户问的是VMware的版本查看方法。-系统级指令要求使用中文回答,正确格式化数学表达式(如果需要),但这里可能不需要数学表达式。-指令还要求生成相关问题,并在回答中引用段落时添加引用标识。用户提供的引用[1]到[5]是关于VMware版本的查看方法、下载等,但用户特别强调“参考
recommend-type

数据库课程设计报告:常用数据库综述

数据库是现代信息管理的基础,其技术广泛应用于各个领域。在高等教育中,数据库课程设计是一个重要环节,它不仅是学习理论知识的实践,也是培养学生综合运用数据库技术解决问题能力的平台。本知识点将围绕“经典数据库课程设计报告”展开,详细阐述数据库的基本概念、课程设计的目的和内容,以及在设计报告中常用的数据库技术。 ### 1. 数据库基本概念 #### 1.1 数据库定义 数据库(Database)是存储在计算机存储设备中的数据集合,这些数据集合是经过组织的、可共享的,并且可以被多个应用程序或用户共享访问。数据库管理系统(DBMS)提供了数据的定义、创建、维护和控制功能。 #### 1.2 数据库类型 数据库按照数据模型可以分为关系型数据库(如MySQL、Oracle)、层次型数据库、网状型数据库、面向对象型数据库等。其中,关系型数据库因其简单性和强大的操作能力而广泛使用。 #### 1.3 数据库特性 数据库具备安全性、完整性、一致性和可靠性等重要特性。安全性指的是防止数据被未授权访问和破坏。完整性指的是数据和数据库的结构必须符合既定规则。一致性保证了事务的执行使数据库从一个一致性状态转换到另一个一致性状态。可靠性则保证了系统发生故障时数据不会丢失。 ### 2. 课程设计目的 #### 2.1 理论与实践结合 数据库课程设计旨在将学生在课堂上学习的数据库理论知识与实际操作相结合,通过完成具体的数据库设计任务,加深对数据库知识的理解。 #### 2.2 培养实践能力 通过课程设计,学生能够提升分析问题、设计解决方案以及使用数据库技术实现这些方案的能力。这包括需求分析、概念设计、逻辑设计、物理设计、数据库实现、测试和维护等整个数据库开发周期。 ### 3. 课程设计内容 #### 3.1 需求分析 在设计报告的开始,需要对项目的目标和需求进行深入分析。这涉及到确定数据存储需求、数据处理需求、数据安全和隐私保护要求等。 #### 3.2 概念设计 概念设计阶段要制定出数据库的E-R模型(实体-关系模型),明确实体之间的关系。E-R模型的目的是确定数据库结构并形成数据库的全局视图。 #### 3.3 逻辑设计 基于概念设计,逻辑设计阶段将E-R模型转换成特定数据库系统的逻辑结构,通常是关系型数据库的表结构。在此阶段,设计者需要确定各个表的属性、数据类型、主键、外键以及索引等。 #### 3.4 物理设计 在物理设计阶段,针对特定的数据库系统,设计者需确定数据的存储方式、索引的具体实现方法、存储过程、触发器等数据库对象的创建。 #### 3.5 数据库实现 根据物理设计,实际创建数据库、表、视图、索引、触发器和存储过程等。同时,还需要编写用于数据录入、查询、更新和删除的SQL语句。 #### 3.6 测试与维护 设计完成之后,需要对数据库进行测试,确保其满足需求分析阶段确定的各项要求。测试过程包括单元测试、集成测试和系统测试。测试无误后,数据库还需要进行持续的维护和优化。 ### 4. 常用数据库技术 #### 4.1 SQL语言 SQL(结构化查询语言)是数据库管理的国际标准语言。它包括数据查询、数据操作、数据定义和数据控制四大功能。SQL语言是数据库课程设计中必备的技能。 #### 4.2 数据库设计工具 常用的数据库设计工具包括ER/Studio、Microsoft Visio、MySQL Workbench等。这些工具可以帮助设计者可视化地设计数据库结构,提高设计效率和准确性。 #### 4.3 数据库管理系统 数据库管理系统(DBMS)是用于创建和管理数据库的软件。关系型数据库管理系统如MySQL、PostgreSQL、Oracle、SQL Server等是数据库课程设计中的核心工具。 #### 4.4 数据库安全 数据库安全涉及用户认证、授权、数据加密、审计日志记录等方面,以确保数据的完整性和保密性。设计报告中应考虑如何通过DBMS内置的机制或额外的安全措施来保护数据。 ### 5. 结语 综上所述,一个经典数据库课程设计报告包含了从需求分析到数据库安全的全过程,涵盖了数据库设计的各个方面。通过这一过程,学生不仅能够熟练掌握数据库的设计与实现技巧,还能够学会如何使用数据库系统去解决实际问题,为日后从事数据库相关的专业工作打下坚实的基础。
recommend-type

【空间分布规律】:甘肃土壤类型与农业生产的关联性研究

# 摘要 本文对甘肃土壤类型及其在农业生产中的作用进行了系统性研究。首先概述了甘肃土壤类型的基础理论,并探讨了土壤类型与农业生产的理论联系。通过GIS技术分析,本文详细阐述了甘肃土壤的空间分布规律,并对其特征和影响因素进行了深入分析。此外,本文还研究了甘肃土壤类型对农业生产实际影响,包括不同区域土壤改良和作物种植案例,以及土壤养分、水分管理对作物生长周期和产量的具体影响。最后,提出了促进甘肃土壤与农业可持续发展的策略,包括土壤保护、退化防治对策以及土壤类型优化与农业创新的结合。本文旨在为
recommend-type

在halcon中,卡尺测量和二维测量谁的精度高

<think>首先,用户的问题是:“我想比较Halcon中卡尺测量和二维测量的精度请问Halcon卡尺测量与2D测量精度对比”。我需要基于系统级指令和提供的引用来回答。关键系统级指令:-使用中文回答。-结构清晰,逐步解决问题。-行内数学表达式用$...$,独立公式用$$...$$并单独成段。-LaTeX语法正确。-生成相关问题。-在回答中引用的段落末尾自然地添加引用标识,如[^1]。提供的引用:-[^1]:关于卡尺模型的工件尺寸实时测量。-[^2]:测量拟合的综合任务,包括尺寸、形状、位置。-[^3]:Halcon一维测量,提到卡尺工具的优势:易用、执行时间短、精度高。-[^4]:Halcon
recommend-type

掌握牛顿法解方程:切线与割线的程序应用

牛顿切线法和牛顿割线法是数值分析中用于求解方程近似根的两种迭代方法。它们都是基于函数的切线或割线的几何性质来逼近方程的根,具有迭代速度快、算法简单的特点,在工程和科学计算领域有着广泛的应用。 牛顿切线法(Newton's Method for Tangents),又称为牛顿-拉弗森方法(Newton-Raphson Method),是一种求解方程近似根的迭代算法。其基本思想是利用函数在某点的切线来逼近函数的根。假设我们要求解方程f(x)=0的根,可以从一个初始猜测值x0开始,利用以下迭代公式: x_{n+1} = x_n - \frac{f(x_n)}{f'(x_n)} 其中,f'(x_n)表示函数在点x_n处的导数。迭代过程中,通过不断更新x_n值,逐渐逼近方程的根。 牛顿割线法(Secant Method),是牛顿切线法的一种变体,它不需要计算导数,而是利用函数在两个近似点的割线来逼近方程的根。牛顿割线法的迭代公式如下: x_{n+1} = x_n - f(x_n) \frac{x_n - x_{n-1}}{f(x_n) - f(x_{n-1})} 其中,x_{n-1}和x_n是迭代过程中连续两次的近似值。牛顿割线法相比牛顿切线法,其优点在于不需要计算函数的导数,但通常收敛速度会比牛顿切线法慢一些。 在实际应用中,这两种方法都需要注意迭代的起始点选择,否则可能会导致迭代过程不收敛。同时,这两种方法都是局部收敛方法,即它们只能保证在初始点附近有足够的近似根时才收敛。 关于例题和程序,牛顿切线法和牛顿割线法都可以通过编程实现。通常在编程实现时,需要输入函数的表达式、初始猜测值、迭代次数限制以及误差容忍度等参数。程序会根据这些输入,通过循环迭代计算,直到满足误差容忍度或达到迭代次数限制为止。 在编程实现过程中,需要注意以下几点: 1. 初始猜测值的选择对迭代的收敛性有较大影响,需要根据具体问题来合理选择。 2. 当迭代过程中遇到函数值或导数值过大、过小,或者分母趋近于零时,需要进行适当的数值处理,以避免数值不稳定或除以零的错误。 3. 对于不同的方程和函数,可能需要选择不同的迭代终止条件,如设定一个误差值或迭代次数上限。 牛顿法(包含牛顿切线法和牛顿割线法)是一类非常强大的数值解法,尤其适用于求解非线性方程,其基本原理和程序实现的知识点在理工科的许多领域都有着广泛的应用,是数值分析领域的一个基石。 请注意,本知识点仅涵盖标题和描述中提到的内容,压缩包子文件列表中的信息并未提供,因此无法提供相关内容的知识点。
recommend-type

【制图技术】:甘肃高质量土壤分布TIF图件的成图策略

# 摘要 本文针对甘肃土壤分布数据的TIF图件制作进行了系统研究。首先概述了甘肃土壤的分布情况,接着介绍了TIF图件的基础知识,包括其格式特点、空间数据表达以及质量控制方法。随后,文中构建了成图策略的理论框架,分析了土壤分布图的信息需求与数据处理流程,并探讨了成图原则与标准。在实践操作部分,详细阐述了制图软