活动介绍

std::numeric_limits<double>::epsilon()

时间: 2024-11-15 22:30:35 AIGC 浏览: 180
`std::numeric_limits<double>::epsilon()` 是C++标准库中定义的一个常量,它表示双精度浮点数(`double`)类型中的最小可表示非零差值[^1]。这个值非常小,通常用于测试数值近似相等的情况,如果两个数的差小于 `epsilon`,则通常认为它们是相等的。 然而,如果你想查看正无穷大值,你应该使用 `std::numeric_limits<T>::infinity()` 方法,这里 `T` 是类型,如 `double`[^2]。这会返回该类型的正无穷大值。 示例代码: ```cpp // 获取double类型的最小可表示非零差值 const double epsilon = std::numeric_limits<double>::epsilon(); std::cout << "Double epsilon: " << epsilon << std::endl; // 获取double类型的正无穷大值 double infinity = std::numeric_limits<double>::infinity(); std::cout << "Double positive infinity: " << infinity << std::endl; ```
相关问题

numeric_limits<数据类型>::epsilon()

在C++中,`std::numeric_limits<数据类型>::epsilon()` 是 `std::numeric_limits` 模板类的一个静态成员函数。`std::numeric_limits` 是一个模板类,在 `<limits>` 头文件中定义,它提供了查询多种算术类型属性的标准化方式,使用时需指定具体的数据类型 `T`,类中的所有成员函数和成员变量都是静态的,可通过 `std::numeric_limits<T>::member_name` 的方式访问 [^1][^3][^4]。 ### 作用 `std::numeric_limits<数据类型>::epsilon()` 表示的是该数据类型中 1 与比 1 大的最小可表示值之间的差值,也就是该数据类型能够表示的最小精度。在浮点数比较时,由于浮点数在计算机中是以二进制近似表示的,直接使用 `==` 或 `!=` 进行比较可能会因为精度问题得到不符合预期的结果,这时就可以使用 `epsilon()` 来设置一个误差范围,进行近似比较。 ### 使用方法 以下是一个简单的示例代码,展示了 `std::numeric_limits<数据类型>::epsilon()` 的使用: ```cpp #include <iostream> #include <limits> #include <cmath> // 比较两个浮点数是否近似相等 bool approximatelyEqual(double a, double b) { return std::abs(a - b) <= std::numeric_limits<double>::epsilon() * std::max(std::abs(a), std::abs(b)); } int main() { double num1 = 0.1 + 0.2; double num2 = 0.3; if (approximatelyEqual(num1, num2)) { std::cout << "num1 和 num2 近似相等" << std::endl; } else { std::cout << "num1 和 num2 不近似相等" << std::endl; } return 0; } ``` 在上述代码中,定义了一个 `approximatelyEqual` 函数,用于比较两个双精度浮点数是否近似相等。在 `main` 函数中,对 `0.1 + 0.2` 和 `0.3` 进行近似比较,并输出结果。

std::numeric_limits<double>::max() 和DBL_MAX在原理和使用上的区别

<think>我们讨论的是C++中的两个概念:`std::numeric_limits<double>::max()` 和 `DBL_MAX`。 首先,两者在功能上是等价的,都表示 double 类型的最大值。但它们在原理和使用上存在一些区别。 1. **原理**: - `std::numeric_limits<double>::max()` 是 C++ 标准模板库(STL)中的一部分,定义在 `<limits>` 头文件中。它是通过模板类 `numeric_limits` 提供的,是一个静态成员函数(实际上是静态常量表达式)。 - `DBL_MAX` 是 C 标准库中定义的宏,通常在 `<cfloat>` 或 `<float.h>` 头文件中定义。它是通过预处理器宏实现的。 2. **使用**: - `std::numeric_limits<double>::max()` 是类型安全的,并且是模板的一部分,可以用于任何数值类型(如 int, float, double 等),只需要改变模板参数即可。 - `DBL_MAX` 是一个宏,它只针对 double 类型。类似地,C 标准库还提供了 `FLT_MAX`(用于 float)和 `INT_MAX`(用于 int)等。 3. **可移植性**: - `std::numeric_limits` 是 C++ 标准的一部分,因此它在所有符合标准的 C++ 编译器中都是可用的,并且是类型安全的。 - `DBL_MAX` 是 C 标准库的一部分,因此在 C++ 中也可以使用(通过 `<cfloat>` 或 `<float.h>`),但它是宏,没有类型安全,而且在某些情况下可能不如模板灵活。 4. **功能扩展**: - `std::numeric_limits` 提供了更多的信息,比如最小值、是否有无穷大、是否是整数类型等等。而 `DBL_MAX` 只提供最大值。 5. **代码示例**: 使用 `std::numeric_limits`: ```cpp #include <iostream> #include <limits> int main() { double max_val = std::numeric_limits<double>::max(); std::cout << "Max double (using numeric_limits): " << max_val << std::endl; return 0; } ``` 使用 `DBL_MAX`: ```cpp #include <iostream> #include <cfloat> // 或者 #include <float.h> int main() { double max_val = DBL_MAX; std::cout << "Max double (using DBL_MAX): " << max_val << std::endl; return 0; } ``` 6. **比较**: - 在 C++ 代码中,推荐使用 `std::numeric_limits`,因为它更符合 C++ 的编程风格(类型安全、模板化、可扩展)。 - 在需要与 C 代码兼容或者在 C 语言环境中,使用 `DBL_MAX`。 7. **注意事项**: - 两者在数值上是相等的,都是定义在标准库中的 double 类型的最大值。 - 在 C++ 中,`<cfloat>` 头文件将 C 标准库中的 `DBL_MAX` 等宏引入到 `std` 命名空间中(但通常也会在全局命名空间中定义)。而 `<float.h>` 则是在全局命名空间中定义这些宏。 8. **性能**: - 两者在运行时都没有性能差异,因为都是编译时常量。 总结:在 C++ 中,优先使用 `std::numeric_limits<double>::max()`,因为它更安全、更通用。在需要与 C 代码交互或者维护旧代码时,可以使用 `DBL_MAX`。 §§相关问题§§: 1. 在 C++ 中,如何获取 `int` 类型的最大值?请分别用 `std::numeric_limits` 和宏的方式给出。 2. `std::numeric_limits` 除了 `max()` 和 `min()` 外,还有哪些常用的成员函数? 3. 为什么说 `std::numeric_limits` 是类型安全的,而宏不是? 4. 在 C++ 中,如何判断一个类型(比如 `T`)是否为整数类型?</think># `std::numeric_limits<double>::max()` vs `DBL_MAX`:原理与使用区别 这两者都表示 `double` 类型的最大值,但在原理和使用上有重要区别: ## 原理区别 | 特性 | `std::numeric_limits<double>::max()` | `DBL_MAX` | |------|--------------------------------------|-----------| | **来源** | C++ 标准库 (`<limits>`) | C 标准库 (`<cfloat>` 或 `<float.h>`) | | **实现方式** | 模板类静态成员 | 预处理宏 | | **类型安全** | 类型安全(编译时检查) | 无类型安全(文本替换) | | **标准化** | C++98 及更高标准 | C89/C99 标准 | | **可扩展性** | 适用于任何数值类型 | 只针对特定类型 | ## 使用区别 ### 1. 头文件包含 ```cpp // 使用 numeric_limits #include <limits> // 使用 DBL_MAX #include <cfloat> // C++ 风格(推荐) // 或 #include <float.h> // C 风格 ``` ### 2. 代码示例 ```cpp #include <iostream> #include <limits> // 对于 numeric_limits #include <cfloat> // 对于 DBL_MAX int main() { // 使用 numeric_limits double max1 = std::numeric_limits<double>::max(); // 使用 DBL_MAX double max2 = DBL_MAX; std::cout << "Using numeric_limits: " << max1 << std::endl; std::cout << "Using DBL_MAX: " << max2 << std::endl; std::cout << "Are they equal? " << (max1 == max2 ? "Yes" : "No") << std::endl; return 0; } ``` ### 3. 类型安全比较 ```cpp #include <iostream> #include <limits> #include <cfloat> template <typename T> void check_max() { // 类型安全的用法 T value = std::numeric_limits<T>::max(); std::cout << "Max value for type: " << value << std::endl; } int main() { // 安全使用不同类型 check_max<double>(); check_max<int>(); check_max<float>(); // 尝试使用 DBL_MAX 作为 float - 不会报错但值不正确 float bad_value = DBL_MAX; // 可能转换为无穷大或错误值 std::cout << "Problematic float value: " << bad_value << std::endl; return 0; } ``` ## 关键区别详解 1. **类型安全性** - `numeric_limits` 是类型安全的模板类 - `DBL_MAX` 是预处理器宏,没有类型检查 2. **可扩展性** ```cpp // 使用 numeric_limits 可以轻松处理任意类型 auto int_max = std::numeric_limits<int>::max(); auto long_max = std::numeric_limits<long>::max(); auto float_max = std::numeric_limits<float>::max(); // 使用宏需要记住不同的宏名 auto int_max_macro = INT_MAX; auto long_max_macro = LONG_MAX; auto float_max_macro = FLT_MAX; ``` 3. **C++ 标准兼容性** - `numeric_limits` 是纯 C++ 解决方案 - `DBL_MAX` 是从 C 继承的 4. **功能丰富性** ```cpp // 获取更多类型信息 bool is_signed = std::numeric_limits<double>::is_signed; int digits = std::numeric_limits<double>::digits; double epsilon = std::numeric_limits<double>::epsilon(); // 宏只提供单个值 ``` ## 推荐实践 1. **在 C++ 代码中优先使用 `std::numeric_limits`**: - 更好的类型安全性 - 更符合 C++ 编程范式 - 模板友好 2. **在需要与 C 代码交互时使用宏**: ```cpp // 混合使用示例 #ifdef __cplusplus const double max_double = std::numeric_limits<double>::max(); #else const double max_double = DBL_MAX; #endif ``` 3. **在泛型编程中必须使用 `numeric_limits`**: ```cpp template <typename T> T get_max_value() { return std::numeric_limits<T>::max(); } // 使用示例 auto d = get_max_value<double>(); auto i = get_max_value<int>(); ``` ## 性能考虑 两者在运行时性能上没有区别,因为它们都是编译时常量: ```cpp // 两种方式生成的汇编代码相同 double a = std::numeric_limits<double>::max(); double b = DBL_MAX; // 都会直接加载常量值 ```
阅读全文

相关推荐

#ifndef GUIUTIL_H #define GUIUTIL_H #include <QtGlobal> #include <QString> #include <cmath> #include <type_traits> #include #include <QMetaType> #include <QString> #include #include #include #include #include #include #include #include <vtkCamera.h> #include <vtkMath.h> #include <vtkTransform.h> #include <QMutex> #include <QMessageBox> #include <QPushButton> #include <QThread> #include <QMap> #include <QTime> #include <memory> #include <QSharedPointer> #include <QtMath> #ifndef M_PI_2_3 #define M_PI_2_3 (2 * M_PI / 3) #endif namespace Gui { namespace common { template <typename T> typename std::enable_if_t<std::is_floating_point_v<T>, bool> is_zero(T value, T epsilon = std::numeric_limits<T>::epsilon() * 100) { return std::abs(value) < epsilon; } template <typename T> typename std::enable_if_t<std::is_floating_point_v<T>, bool> is_non_zero(T value, T epsilon = std::numeric_limits<T>::epsilon() * 100) { return !is_zero(value, epsilon); } // 角度转弧度 inline double deg2rad(double deg) { return deg * (M_PI / 180.0); } // 弧度转角度 inline double rad2deg(double rad) { return rad * (180.0 / M_PI); } /** * @brief getXORSumByByte 按字节求字节数组的所有元素的异或和 * @param data * @param size * @return */ char getXORSumByByte(const char* data, unsigned int size); /** * @brief The Singleton class 模板单例类,继承该类即可实现单例 */ template<typename T> class Singleton : public QObject { public: static T& instance() { static std::unique_ptr<T> instance; static QMutex mutex; if (!instance) { QMutexLocker locker(&mutex); if (!instance) { instance.reset(new T()); } } return *instance; } protected: Singleton() = default; virtual ~Singleton() = default; // 禁止拷贝 Singleton(const Singleton&) = delete; Singleton& operator=(const Singleton&) = delete; }; int calReMainingTime(const QTime& curTime, const QTime& targetTime); } namespace Ctl { namespace rtsp { const int RTSP_STOP = 0x1; const int RTSP_CONTINU = 0x2; const int RTSP_OPENED = 0x3; // 错误状态以0x10开始 const int RTSP_NETWORK_ERROR = 0x10; const int RTSP_NOT_GET_INFO = 0x11; const int RTSP_NOT_FOUND = 0x12; const int RTSP_NOT_FOUND_DECODER = 0x13; const int RTSP_NOT_CREATE_DECODER_CONTEXT = 0x14; const int RTSP_NOT_OPEN_DECODER = 0x15; const int RTSP_NOT_ALLOCK_FRAME = 0x16; const int RTSP_FAULT = 0x17; } } 注释上述代码

TRAC_IK::TRAC_IK(const std::string& base_link, const std::string& tip_link, const std::string& URDF_param, double _maxtime, double _eps, SolveType _type) : initialized(false), eps(_eps), maxtime(_maxtime), solvetype(_type) { urdf::Model robot_model; robot_model.initString(URDF_param); KDL::Tree tree; if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) std::cerr << "Failed to extract kdl tree from xml robot description"; if (!tree.getChain(base_link, tip_link, chain)) std::cerr << "Couldn't find chain " << base_link.c_str() << " to " << tip_link.c_str(); std::vector<KDL::Segment> chain_segs = chain.segments; urdf::JointConstSharedPtr joint; std::vector<double> l_bounds, u_bounds; lb.resize(chain.getNrOfJoints()); ub.resize(chain.getNrOfJoints()); uint joint_num = 0; for (unsigned int i = 0; i < chain_segs.size(); ++i) { joint = robot_model.getJoint(chain_segs[i].getJoint().getName()); if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) { joint_num++; float lower, upper; int hasLimits; if (joint->type != urdf::Joint::CONTINUOUS) { if (joint->safety) { lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit); upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit); } else { lower = joint->limits->lower; upper = joint->limits->upper; } hasLimits = 1; } else { hasLimits = 0; } if (hasLimits) { lb(joint_num - 1) = lower; ub(joint_num - 1) = upper; } else { lb(joint_num - 1) = std::numeric_limits<float>::lowest(); ub(joint_num - 1) = std::numeric_limits<float>::max(); } // ROS_DEBUG_STREAM_NAMED("trac_ik", "IK Using joint " << joint->name << " " << lb(joint_num - 1) << " " << ub(joint_num - 1)); } } initialize(); }

#include <iostream> #include <fstream> #include <vector> #include <string> #include <cmath> #include <map> #include <set> #include <queue> #include <algorithm> #include <stdexcept> #include <iomanip> #include <sstream> #include <unordered_set> #include <stack> #include <functional> #ifdef _WIN32 #include <windows.h> // 用于GetCurrentDirectoryA #else #include <unistd.h> // 用于getcwd #endif constexpr double EPSILON = 1e-10; class Point { public: std::string id; double elevation; bool isFixed; Point(const std::string& _id, double _elevation = 0.0, bool _isFixed = false) : id(_id), elevation(_elevation), isFixed(_isFixed) { } }; class Observation { public: std::string from; std::string to; double dh; double weight; double correction; double adjustedValue; Observation(const std::string& _from, const std::string& _to, double _dh, double _weight = 1.0) : from(_from), to(_to), dh(_dh), weight(_weight), correction(0.0), adjustedValue(0.0) { } }; class Condition { public: std::vector<size_t> obsIndices; std::vector<double> coefficients; double w; Condition() : w(0.0) {} void addObservation(size_t obsIdx, double coeff) { obsIndices.push_back(obsIdx); coefficients.push_back(coeff); } void calculateW(const std::vector<Observation>& observations) { w = 0.0; for (size_t i = 0; i < obsIndices.size(); ++i) { if (obsIndices[i] < observations.size()) { w += coefficients[i] * observations[obsIndices[i]].dh; } } w = -w; } }; class Matrix { private: size_t rows; size_t cols; std::vector<double> data; public: Matrix(size_t r, size_t c) : rows(r), cols(c), data(r* c, 0.0) {} // 访问元素 double& operator()(size_t i, size_t j) { if (i >= rows || j >= cols) { throw std::out_of_range("矩阵索引越界"); } return data[i * cols + j]; } const double& operator()(size_t i, size_t j) const { if (i >= rows || j >= cols) { throw std::out_of_range("矩阵索引越界"); } return data[i * cols + j]; } size_t getRows() const { return rows; } size_t getCols() const { return cols; } // 矩阵乘法 Matrix operator*(const Matrix& other) const { if (cols != other.rows) { throw std::invalid_argument("矩阵维度不匹配"); } Matrix result(rows, other.cols); for (size_t i = 0; i < rows; ++i) { for (size_t j = 0; j < other.cols; ++j) { for (size_t k = 0; k < cols; ++k) { result(i, j) += (*this)(i, k) * other(k, j);//C(i,j)=A(i*k)*B(k*j) } } } return result; } // 矩阵转置 Matrix transpose() const { Matrix result(cols, rows); for (size_t i = 0; i < rows; ++i) { for (size_t j = 0; j < cols; ++j) { result(j, i) = (*this)(i, j);//算A^(T) } } return result; } // 更稳定的高斯消元法 static std::vector<double> solve(const Matrix& A, const std::vector<double>& b) { if (A.rows != A.cols || A.rows != b.size()) { throw std::invalid_argument("矩阵维度不匹配"); } size_t n = A.rows; Matrix augmented(n, n + 1); for (size_t i = 0; i < n; ++i) { for (size_t j = 0; j < n; ++j) { augmented(i, j) = A(i, j); } augmented(i, n) = b[i]; } // 部分选主元 for (size_t i = 0; i < n; ++i) { // 寻找主元 size_t maxRow = i; for (size_t k = i + 1; k < n; ++k) { if (std::abs(augmented(k, i)) > std::abs(augmented(maxRow, i))) { maxRow = k; } } // 交换行 if (maxRow != i) { for (size_t j = i; j <= n; ++j) { std::swap(augmented(i, j), augmented(maxRow, j)); } } // 奇异矩阵检查 if (std::abs(augmented(i, i)) < EPSILON) { throw std::runtime_error("矩阵奇异,无法求解"); } // 消元 for (size_t k = i + 1; k < n; ++k) { double factor = augmented(k, i) / augmented(i, i); for (size_t j = i; j <= n; ++j) { augmented(k, j) -= factor * augmented(i, j); } } } // 回代 std::vector<double> x(n); for (int i = static_cast<int>(n) - 1; i >= 0; --i) { x[i] = augmented(i, n); for (size_t j = i + 1; j < n; ++j) { x[i] -= augmented(i, j) * x[j]; } x[i] /= augmented(i, i); } return x; } }; class LevelingNetwork { private: std::vector points; std::vector<Observation> observations; std::vector<Condition> conditions; std::map<std::string, size_t> pointIndex; double unitWeightVariance; std::vector<double> pointPrecisions; std::vector<double> initialElevations; // 保存初始高程 // 构建图结构 struct GraphNode { std::string id; std::vector<std::pair<std::string, size_t>> neighbors; // 邻居点和观测索引 }; // 深度优先搜索识别路径 void dfs(const std::string& current, const std::string& end, std::vector<size_t>& path, std::unordered_set<std::string>& visited, std::vector<std::vector<size_t>>& allPaths, const std::map<std::string, GraphNode>& graph) { visited.insert(current); if (current == end) { allPaths.push_back(path); visited.erase(current); return; } auto it = graph.find(current); if (it != graph.end()) { for (const auto& neighbor : it->second.neighbors) { if (visited.find(neighbor.first) == visited.end()) { path.push_back(neighbor.second); dfs(neighbor.first, end, path, visited, allPaths, graph); path.pop_back(); } } } visited.erase(current); } // 识别所有可能的路径(用于附合路线) std::vector<std::vector<size_t>> findAllPaths(const std::string& start, const std::string& end, const std::map<std::string, GraphNode>& graph) { std::vector<std::vector<size_t>> allPaths; std::vector<size_t> path; std::unordered_set<std::string> visited; if (graph.find(start) != graph.end() && graph.find(end) != graph.end()) { dfs(start, end, path, visited, allPaths, graph); } return allPaths; } public: void addPoint(const Point& p) { points.push_back(p); pointIndex[p.id] = points.size() - 1; } void addObservation(const Observation& obs) { observations.push_back(obs); } // 改进的条件方程识别算法 void identifyConditions() { conditions.clear(); std::map<std::string, GraphNode> graph; // 构建图 for (size_t i = 0; i < observations.size(); ++i) { const auto& obs = observations[i]; graph[obs.from].id = obs.from; graph[obs.from].neighbors.push_back({ obs.to, i }); graph[obs.to].id = obs.to; graph[obs.to].neighbors.push_back({ obs.from, i }); } // 识别闭合环 std::vector<std::string> knownPoints; for (const auto& p : points) { if (p.isFixed) { knownPoints.push_back(p.id); } } // 1. 处理附合路线(已知点之间的所有可能路径) if (knownPoints.size() >= 2) { for (size_t i = 0; i < knownPoints.size(); ++i) { for (size_t j = i + 1; j < knownPoints.size(); ++j) { auto paths = findAllPaths(knownPoints[i], knownPoints[j], graph); for (const auto& path : paths) { if (!path.empty()) { Condition cond; for (size_t obsIdx : path) { cond.addObservation(obsIdx, 1.0); } cond.calculateW(observations); // 计算理论闭合差 double startElev = points[pointIndex[knownPoints[i]]].elevation; double endElev = points[pointIndex[knownPoints[j]]].elevation; cond.w = (endElev - startElev) - (-cond.w); conditions.push_back(cond); } } } } } // 2. 处理闭合环(改进算法) std::unordered_set<std::string> visitedNodes; for (const auto& node : graph) { if (visitedNodes.find(node.first) != visitedNodes.end()) continue; std::unordered_set<std::string> currentComponent; std::queue<std::string> bfsQueue; bfsQueue.push(node.first); currentComponent.insert(node.first); // BFS遍历连通分量 while (!bfsQueue.empty()) { std::string current = bfsQueue.front(); bfsQueue.pop(); for (const auto& neighbor : graph[current].neighbors) { if (currentComponent.find(neighbor.first) == currentComponent.end()) { currentComponent.insert(neighbor.first); bfsQueue.push(neighbor.first); } } } visitedNodes.insert(currentComponent.begin(), currentComponent.end()); // 为每个连通分量构建闭合环 for (const auto& startNode : currentComponent) { for (const auto& neighbor : graph[startNode].neighbors) { std::vector<std::vector<size_t>> paths = findAllPaths(neighbor.first, startNode, graph); for (const auto& path : paths) { if (!path.empty()) { Condition cond; std::unordered_set<size_t> obsInPath; for (size_t obsIdx : path) { if (obsInPath.find(obsIdx) == obsInPath.end()) { cond.addObservation(obsIdx, 1.0); obsInPath.insert(obsIdx); } } if (!cond.obsIndices.empty()) { cond.calculateW(observations); conditions.push_back(cond); } } } } } } // 确保条件方程个数正确 size_t necessaryObs = 0; for (const auto& p : points) { if (!p.isFixed) necessaryObs++; } if (necessaryObs == 0) necessaryObs = points.size() - 1; size_t r = observations.size() - necessaryObs; while (conditions.size() > r) { conditions.pop_back(); } std::cout << "生成 " << conditions.size() << " 个独立条件方程" << std::endl; } void calculateAdjustedElevations() { // 保存初始高程 std::vector<double> initialElevations; for (const auto& p : points) { initialElevations.push_back(p.elevation); } // 使用改正后的观测值计算高程 for (const auto& obs : observations) { size_t fromIdx = pointIndex[obs.from]; size_t toIdx = pointIndex[obs.to]; if (points[fromIdx].isFixed && !points[toIdx].isFixed) { // 已知点到未知点:H_to = H_from + dh_adjusted points[toIdx].elevation = points[fromIdx].elevation + obs.adjustedValue; } else if (!points[fromIdx].isFixed && points[toIdx].isFixed) { // 未知点到已知点:H_from = H_to - dh_adjusted points[fromIdx].elevation = points[toIdx].elevation - obs.adjustedValue; } } // 处理多个已知点的情况 bool changed; do { changed = false; for (const auto& obs : observations) { size_t fromIdx = pointIndex[obs.from]; size_t toIdx = pointIndex[obs.to]; if (points[fromIdx].isFixed && !points[toIdx].isFixed) { double newElev = points[fromIdx].elevation + obs.adjustedValue; if (fabs(newElev - points[toIdx].elevation) > EPSILON) { points[toIdx].elevation = newElev; changed = true; } } else if (!points[fromIdx].isFixed && points[toIdx].isFixed) { double newElev = points[toIdx].elevation - obs.adjustedValue; if (fabs(newElev - points[fromIdx].elevation) > EPSILON) { points[fromIdx].elevation = newElev; changed = true; } } else if (!points[fromIdx].isFixed && !points[toIdx].isFixed) { // 如果有一个点已计算,计算另一个点 if (points[fromIdx].elevation > EPSILON && points[toIdx].elevation < EPSILON) { points[toIdx].elevation = points[fromIdx].elevation + obs.adjustedValue; changed = true; } else if (points[toIdx].elevation > EPSILON && points[fromIdx].elevation < EPSILON) { points[fromIdx].elevation = points[toIdx].elevation - obs.adjustedValue; changed = true; } } } } while (changed); } void evaluatePrecision() { pointPrecisions.resize(points.size(), 0.0); double mu0 = std::sqrt(unitWeightVariance) * 1000; // 单位毫米 // 正确的高程中误差计算 for (size_t i = 0; i < points.size(); ++i) { if (!points[i].isFixed) { double weightSum = 0.0; int connectionCount = 0; // 计算与点相连的所有观测值的权之和 for (const auto& obs : observations) { if (obs.from == points[i].id || obs.to == points[i].id) { weightSum += obs.weight; connectionCount++; } } if (weightSum > EPSILON) { // 正确公式:μ_i = μ₀ / √(ΣP) double mu = mu0 / std::sqrt(weightSum); pointPrecisions[i] = mu; } } } } bool readDataFromFile(const std::string& filename) { // 显示尝试读取的文件路径 std::cout << "尝试读取文件: " << filename << std::endl; std::ifstream file(filename); if (!file.is_open()) { // 显示详细错误信息 std::cerr << "错误:无法打开文件 '" << filename << "'" << std::endl; // 显示当前工作目录 #ifdef _WIN32 char buffer[MAX_PATH]; if (GetCurrentDirectoryA(MAX_PATH, buffer)) { std::cerr << "当前工作目录: " << buffer << std::endl; } #else char buffer[1024]; if (getcwd(buffer, sizeof(buffer)) != nullptr) { std::cerr << "当前工作目录: " << buffer << std::endl; } #endif return false; } std::string line; points.clear(); observations.clear(); conditions.clear(); pointIndex.clear(); if (!std::getline(file, line)) { std::cerr << "错误:文件格式不正确,无法读取点数" << std::endl; file.close(); return false; } int numPoints; try { numPoints = std::stoi(line); } catch (const std::exception& e) { std::cerr << "错误:无效的点数格式: " << e.what() << std::endl; file.close(); return false; } std::cout << "读取 " << numPoints << " 个点数据:" << std::endl; for (int i = 0; i < numPoints; ++i) { if (!std::getline(file, line)) { std::cerr << "错误:文件格式不正确,点数据读取失败" << std::endl; file.close(); return false; } std::istringstream iss(line); std::string id; double elevation; int isFixedInt; if (!(iss >> id >> elevation >> isFixedInt)) { std::cerr << "错误:点数据格式不正确: " << line << std::endl; file.close(); return false; } addPoint(Point(id, elevation, isFixedInt != 0)); std::cout << " " << id << " : " << elevation << "m (" << (isFixedInt != 0 ? "已知点" : "待定点") << ")" << std::endl; } // 读取观测值数据 if (!std::getline(file, line)) { std::cerr << "错误:文件格式不正确,无法读取观测值数" << std::endl; file.close(); return false; } int numObs; try { numObs = std::stoi(line); } catch (const std::exception& e) { std::cerr << "错误:无效的观测值数格式: " << e.what() << std::endl; file.close(); return false; } std::cout << "读取 " << numObs << " 个观测值数据:" << std::endl; for (int i = 0; i < numObs; ++i) { if (!std::getline(file, line)) { std::cerr << "错误:文件格式不正确,观测值数据读取失败" << std::endl; file.close(); return false; } std::istringstream iss(line); std::string from, to; double dh, distance; if (!(iss >> from >> to >> dh >> distance)) { std::cerr << "错误:观测值数据格式不正确: " << line << std::endl; file.close(); return false; } double weight = 1.0 / distance; addObservation(Observation(from, to, dh, weight)); std::cout << " " << from << " -> " << to << " : " << dh << "m, 距离: " << distance << " 权: " << weight << std::endl; } // 验证观测值引用的点是否存在 for (const auto& obs : observations) { if (pointIndex.find(obs.from) == pointIndex.end()) { std::cerr << "错误:观测值起点 '" << obs.from << "' 不存在" << std::endl; return false; } if (pointIndex.find(obs.to) == pointIndex.end()) { std::cerr << "错误:观测值终点 '" << obs.to << "' 不存在" << std::endl; return false; } } file.close(); return true; } void adjust() { // 保存初始高程 initialElevations.clear(); for (const auto& p : points) { initialElevations.push_back(p.elevation); } std::cout << "\n开始水准网平差计算..." << std::endl; if (points.empty() || observations.empty()) { std::cerr << "错误:数据不完整" << std::endl; return; } // 识别条件方程 identifyConditions(); size_t n = observations.size(); size_t r = conditions.size(); if (r == 0) { std::cerr << "错误:未生成条件方程" << std::endl; return; } // 构建条件方程矩阵 Matrix A(r, n); for (size_t i = 0; i < r; ++i) { const Condition& cond = conditions[i]; for (size_t j = 0; j < cond.obsIndices.size(); ++j) { size_t obsIdx = cond.obsIndices[j]; if (obsIdx < n) { A(i, obsIdx) = cond.coefficients[j]; } } } // 输出条件方程矩阵 std::cout << "\n条件方程矩阵 A:" << std::endl; for (size_t i = 0; i < r; ++i) { for (size_t j = 0; j < n; ++j) { std::cout << std::fixed << std::setprecision(6) << A(i, j) << "\t"; } std::cout << "| " << conditions[i].w << std::endl; } // 构建权对角向量 std::vector<double> P_diag(n); for (size_t i = 0; i < n; ++i) { P_diag[i] = observations[i].weight; } // 构建法方程 N = A * diag(P) * A^T Matrix N(r, r); for (size_t i = 0; i < r; ++i) { for (size_t j = 0; j < r; ++j) { double sum = 0.0; for (size_t k = 0; k < n; ++k) { sum += A(i, k) * P_diag[k] * A(j, k); } N(i, j) = sum; } } // 构建闭合差向量 std::vector<double> w(r); for (size_t i = 0; i < r; ++i) { w[i] = conditions[i].w; } // 解法方程 N * k = -w std::vector<double> minusW(r); for (size_t i = 0; i < r; ++i) { minusW[i] = -w[i]; } std::vector<double> k; try { k = Matrix::solve(N, minusW); } catch (const std::exception& e) { std::cerr << "法方程求解失败: " << e.what() << std::endl; return; } // 输出联系数向量 std::cout << "\n联系数向量 k:" << std::endl; for (size_t i = 0; i < r; ++i) { std::cout << "k[" << i << "] = " << std::fixed << std::setprecision(6) << k[i] << std::endl; } // 计算改正数 v = P^{-1} * A^T * k std::vector<double> v(n, 0.0); for (size_t i = 0; i < n; ++i) { for (size_t j = 0; j < r; ++j) { v[i] += A(j, i) * k[j] / P_diag[i]; } observations[i].correction = v[i]; observations[i].adjustedValue = observations[i].dh + v[i]; // 输出改正数信息 std::cout << "观测值 " << i + 1 << " (" << observations[i].from << "->" << observations[i].to << "): 改正数 = " << std::fixed << std::setprecision(2) << v[i] /1000.0 << " mm" << ", 平差值 = " << observations[i].adjustedValue << " m" << std::endl; } // 计算单位权方差 double sumPVV = 0.0; for (size_t i = 0; i < n; ++i) { sumPVV += v[i] * v[i] * P_diag[i]; } unitWeightVariance = sumPVV / r; double mu0 = std::sqrt(unitWeightVariance); std::cout << "单位权中误差: ±" << std::fixed << std::setprecision(2) << mu0 * 1000.0 << " mm" << std::endl; // 计算高程平差值 calculateAdjustedElevations(); // 精度评定 evaluatePrecision(); // 输出结果 printResults(); } // 输出计算结果 void printResults() const { std::cout << "\n===== 水准网平差计算结果 =====" << std::endl; // 1. 待定点高程平差值 std::cout << "\n1. 待定点高程平差值:" << std::endl; std::cout << std::left << std::setw(10) << "点号" << std::setw(15) << "初始高程(m)" << std::setw(15) << "平差后高程(m)" << std::setw(15) << "中误差(mm)" << std::endl; std::cout << "--------------------------------------------------------" << std::endl; for (size_t i = 0; i < points.size(); ++i) { const auto& p = points[i]; std::cout << std::left << std::setw(10) << p.id << std::setw(15) << std::fixed << std::setprecision(6) << (p.isFixed ? p.elevation : 0.0) << std::setw(15) << std::fixed << std::setprecision(6) << p.elevation; if (!p.isFixed) { std::cout << std::setw(15) << std::fixed << std::setprecision(2) << pointPrecisions[i]; } std::cout << std::endl; } // 观测值改正数(以毫米为单位) for (size_t i = 0; i < observations.size(); ++i) { const auto& obs = observations[i]; std::cout << std::left << std::setw(10) << (i + 1) << std::setw(10) << obs.from << std::setw(10) << obs.to << std::setw(15) << std::fixed << std::setprecision(6) << obs.dh << std::setw(15) << std::fixed << std::setprecision(2) << obs.correction /1000.0 // 毫米 << std::setw(15) << std::fixed << std::setprecision(6) << obs.adjustedValue << std::endl; } // 高程变化 std::cout << "\n3. 高程变化:" << std::endl; std::cout << std::left << std::setw(10) << "点号" << std::setw(15) << "初始高程(m)" << std::setw(15) << "平差后高程(m)" << std::setw(15) << "变化量(mm)" << std::endl; std::cout << "--------------------------------------------------------" << std::endl; for (size_t i = 0; i < points.size(); ++i) { const auto& p = points[i]; double change = (p.elevation - initialElevations[i]) / 1000.0; // 毫米 std::cout << std::left << std::setw(10) << p.id << std::setw(15) << std::fixed << std::setprecision(6) << initialElevations[i] << std::setw(15) << std::fixed << std::setprecision(6) << p.elevation << std::setw(15) << std::fixed << std::setprecision(2) << change << std::endl; } // 3. 精度评定 std::cout << "\n3. 精度评定:" << std::endl; std::cout << "单位权中误差: ±" << std::fixed << std::setprecision(2) << std::sqrt(unitWeightVariance) / 1000 << " mm" << std::endl; } }; int main() { try { LevelingNetwork network; if (!network.readDataFromFile("leveling.txt")) { std::cerr << "错误:无法读取数据文件。程序将退出。" << std::endl; return 1; } network.adjust(); return 0; } catch (const std::exception& e) { std::cerr << "错误: " << e.what() << std::endl; return 1; } }这是我写的程序,假如你是一个资深的测量平差以及c++的老师,面对我的水准网条件平差的程序,请帮我修改一下,使得该程序不仅能实现水准网平差过程通用程序,包括误差方程、法方程的组成与解算。得出平差后的高差平差值及各待定点的高程平差值;评定各待定点高程平差值的精度、观测差平差值的精度;;根据水准点间的相对关系,假定坐标,利用程序画出水准网示意图;还能够识别A-B,B-C,A-C,还能使得列出的所有条件方程足数(观测总数减去必要观测个数等于条件方程个数,如果有足够的已知观测数据,必要观测个数等于未知数个数;如果没有足够已知数据,必要观测个数等于总点数-1)且并不相关,不是奇异矩阵。(要求不借助外部数据库,自己编写矩阵以及其他的函数),是在visual studio2022上实现,且只是c++11或14中。且如果是不需要的请帮我简化一下。重点检查一下改正数,单位权中误差,改正后高差,高程中误差的计算以及寻找闭合和符合导线是否有问题,请帮我修正

在vs2015 c++ .h中加入这段代码会报重定义 namespace cv_dnn { namespace { template <typename T> static inline bool SortScorePairDescend(const std::pair<float, T>& pair1, const std::pair<float, T>& pair2) { return pair1.first > pair2.first; } } // namespace inline void GetMaxScoreIndex(const std::vector<float>& scores, const float threshold, const int top_k, std::vector<std::pair<float, int> >& score_index_vec) { for (size_t i = 0; i < scores.size(); ++i) { if (scores[i] > threshold) { score_index_vec.push_back(std::make_pair(scores[i], i)); } } std::stable_sort(score_index_vec.begin(), score_index_vec.end(), SortScorePairDescend<int>); if (top_k > 0 && top_k < (int)score_index_vec.size()) { score_index_vec.resize(top_k); } } template <typename BoxType> inline void NMSFast_(const std::vector<BoxType>& bboxes, const std::vector<float>& scores, const float score_threshold, const float nms_threshold, const float eta, const int top_k, std::vector<int>& indices, float(*computeOverlap)(const BoxType&, const BoxType&)) { CV_Assert(bboxes.size() == scores.size()); std::vector<std::pair<float, int> > score_index_vec; GetMaxScoreIndex(scores, score_threshold, top_k, score_index_vec); // Do nms. float adaptive_threshold = nms_threshold; indices.clear(); for (size_t i = 0; i < score_index_vec.size(); ++i) { const int idx = score_index_vec[i].second; bool keep = true; for (int k = 0; k < (int)indices.size() && keep; ++k) { const int kept_idx = indices[k]; float overlap = computeOverlap(bboxes[idx], bboxes[kept_idx]); keep = overlap <= adaptive_threshold; } if (keep) indices.push_back(idx); if (keep && eta < 1 && adaptive_threshold > 0.5) { adaptive_threshold *= eta; } } } // copied from opencv 3.4, not exist in 3.0 template<typename Tp> static inline double jaccardDistance_(const Rect_<Tp>& a, const Rect<_Tp>& b) { Tp Aa = a.area(); Tp Ab = b.area(); if ((Aa + Ab) <= std::numeric_limits<Tp>::epsilon()) { // jaccard_index = 1 -> distance = 0 return 0.0; } double Aab = (a & b).area(); // distance = 1 - jaccard_index return 1.0 - Aab / (Aa + Ab - Aab); } template <typename T> static inline float rectOverlap(const T& a, const T& b) { return 1.f - static_cast<float>(jaccardDistance(a, b)); } void NMSBoxes(const std::vector<Rect>& bboxes, const std::vector<float>& scores, const float score_threshold, const float nms_threshold, std::vector<int>& indices, const float eta = 1, const int top_k = 0) { NMSFast(bboxes, scores, score_threshold, nms_threshold, eta, top_k, indices, rectOverlap); } }

现在的代码如下:#include "obstacle_avoidance.hpp" // 辅助函数,将geometry_msgs::msg::Polygon转换为Shape数组 std::vector<ObstacleAvoidance::Shape> ObstacleAvoidance::convertPolygonToShapes(const geometry_msgs::msg::Polygon &polygon) { std::vector<ObstacleAvoidance::Shape> shapes; for (const auto &point : polygon.points) { ObstacleAvoidance::Shape s; s.x = point.x; s.y = point.y; s.z = point.z; shapes.emplace_back(s); } return shapes; } // 辅助函数:将Shape向量转换为Polygon消息 geometry_msgs::msg::Polygon ObstacleAvoidance::convertShapesToPolygon(const std::vector<Shape> &shapes) { geometry_msgs::msg::Polygon polygon; for (const auto &shape : shapes) { geometry_msgs::msg::Point32 point; point.x = shape.x; point.y = shape.y; point.z = shape.z; polygon.points.emplace_back(point); } return polygon; } // 辅助函数 位姿变换 // void ObstacleAvoidance::PoseTransform(std::vector<ObstacleAvoidance::Shape> &shapes, const KX::Pose &pose) // { // for (auto &shape : shapes) // { // KX::Pos point(shape.x, shape.y, shape.z); // // point = pose * point; // point = pose.pos() + point; // shape.x = point.x(); // shape.y = point.y(); // shape.z = point.z(); // } // } // 辅助函数 位姿变换 // std::vector<ObstacleAvoidance::Shape> ObstacleAvoidance::transformShapes(const std::vector<ObstacleAvoidance::Shape> &shapes, const KX::Pose &pose) // { // std::vector<ObstacleAvoidance::Shape> transformed; // for (const auto &s : shapes) // { // KX::Pos p(s.x, s.y, s.z); // p = p + pose.pos(); // transformed.push_back({static_cast<float>(p.x()), static_cast<float>(p.y()), static_cast<float>(p.z())}); // } // return transformed; // } // 位姿变换函数 std::vector<ObstacleAvoidance::Shape> ObstacleAvoidance::transformShapes( const std::vector<ObstacleAvoidance::Shape> &shapes, const KX::Pose &pose) { std::vector<ObstacleAvoidance::Shape> transformed; const double yaw = pose.ori().yaw(); const double cos_yaw = cos(yaw); const double sin_yaw = sin(yaw); for (const auto &s : shapes) { // 旋转后平移 double x_rot = s.x * cos_yaw - s.y * sin_yaw; double y_rot = s.x * sin_yaw + s.y * cos_yaw; transformed.push_back({static_cast<float>(x_rot + pose.pos().x()), static_cast<float>(y_rot + pose.pos().y()), static_cast<float>(s.z + pose.pos().z())}); } return transformed; } // 辅助函数:创建扩展包围盒 geometry_msgs::msg::Polygon ObstacleAvoidance::calExtensionPolygon( const geometry_msgs::msg::Polygon &base_polygon, double expand_front, double expand_back, double expand_left, double expand_right) { // 计算基础包围盒 double min_x = std::numeric_limits<double>::max(); double max_x = std::numeric_limits<double>::lowest(); double min_y = std::numeric_limits<double>::max(); double max_y = std::numeric_limits<double>::lowest(); for (const auto &point : base_polygon.points) { min_x = std::min(min_x, static_cast<double>(point.x)); max_x = std::max(max_x, static_cast<double>(point.x)); min_y = std::min(min_y, static_cast<double>(point.y)); max_y = std::max(max_y, static_cast<double>(point.y)); } // 应用扩展 min_x -= expand_back; max_x += expand_front; min_y -= expand_right; max_y += expand_left; // 创建矩形包围盒 geometry_msgs::msg::Polygon expanded_polygon; geometry_msgs::msg::Point32 p; expanded_polygon.points.reserve(4); p.x = min_x; p.y = min_y; expanded_polygon.points.emplace_back(p); // 右下 p.x = min_x; p.y = max_y; expanded_polygon.points.emplace_back(p); // 左下 p.x = max_x; p.y = max_y; expanded_polygon.points.emplace_back(p); // 左上 p.x = max_x; p.y = min_y; expanded_polygon.points.emplace_back(p); // 右上 return expanded_polygon; } // 安全系数函数(离停止区域越近,安全系数越低) double ObstacleAvoidance::calSafetyFactor( const KX::Pos &obstacle_pos_in_world, const KX::Pose &car_pose, const geometry_msgs::msg::Polygon &base_car_polygon, double stop_offset_front, double stop_offset_back, double stop_offset_left, double stop_offset_right, double dec_offset_front, double dec_offset_back, double dec_offset_left, double dec_offset_right) { // 障碍物位置转换到车体坐标系 // KX::Pos obstacle_in_car = obstacle_pos_in_world - car_pose; KX::Pos obstacle_in_car = obstacle_pos_in_world - car_pose.pos(); const double yaw = car_pose.ori().yaw(); const double cos_yaw = cos(-yaw); // 逆向旋转 const double sin_yaw = sin(-yaw); // 应用旋转 double x_rot = obstacle_in_car.x() * cos_yaw - obstacle_in_car.y() * sin_yaw; double y_rot = obstacle_in_car.x() * sin_yaw + obstacle_in_car.y() * cos_yaw; obstacle_in_car.setX(x_rot); obstacle_in_car.setY(y_rot); // 计算基础包围盒 double min_x = std::numeric_limits<double>::max(); double max_x = std::numeric_limits<double>::lowest(); double min_y = std::numeric_limits<double>::max(); double max_y = std::numeric_limits<double>::lowest(); for (const auto &point : base_car_polygon.points) { min_x = std::min(min_x, static_cast<double>(point.x)); max_x = std::max(max_x, static_cast<double>(point.x)); min_y = std::min(min_y, static_cast<double>(point.y)); max_y = std::max(max_y, static_cast<double>(point.y)); } // 计算停止区域边界 double stop_min_x = min_x - stop_offset_back; double stop_max_x = max_x + stop_offset_front; double stop_min_y = min_y - stop_offset_right; double stop_max_y = max_y + stop_offset_left; // 计算减速区域边界 double dec_min_x = min_x - dec_offset_back; double dec_max_x = max_x + dec_offset_front; double dec_min_y = min_y - dec_offset_right; double dec_max_y = max_y + dec_offset_left; // 检查障碍物是否在停止包围盒内 if (obstacle_in_car.x() >= stop_min_x && obstacle_in_car.x() <= stop_max_x && obstacle_in_car.y() >= stop_min_y && obstacle_in_car.y() <= stop_max_y) { return 0.0; // 安全系数为0 } // 检查障碍物是否在减速包围盒外 if (obstacle_in_car.x() < dec_min_x || obstacle_in_car.x() > dec_max_x || obstacle_in_car.y() < dec_min_y || obstacle_in_car.y() > dec_max_y) { return 1.0; // 安全系数为1 } // 障碍物在减速包围盒内,计算到停止包围盒的距离 double dx = 0.0, dy = 0.0; if (obstacle_in_car.x() < stop_min_x) { dx = stop_min_x - obstacle_in_car.x(); } else if (obstacle_in_car.x() > stop_max_x) { dx = obstacle_in_car.x() - stop_max_x; } if (obstacle_in_car.y() < stop_min_y) { dy = stop_min_y - obstacle_in_car.y(); } else if (obstacle_in_car.y() > stop_max_y) { dy = obstacle_in_car.y() - stop_max_y; } double dist_to_stop = std::sqrt(dx * dx + dy * dy); // 障碍物在减速包围盒内,计算到减速包围盒的距离 double dist_to_dec = 0.0; if (obstacle_in_car.x() < dec_min_x) { dist_to_dec = dec_min_x - obstacle_in_car.x(); } else if (obstacle_in_car.x() > dec_max_x) { dist_to_dec = obstacle_in_car.x() - dec_max_x; } else if (obstacle_in_car.y() < dec_min_y) { dist_to_dec = dec_min_y - obstacle_in_car.y(); } else if (obstacle_in_car.y() > dec_max_y) { dist_to_dec = obstacle_in_car.y() - dec_max_y; } else { // 计算到最近减速区域边界的距离 double dx_min = std::min( std::abs(obstacle_in_car.x() - dec_min_x), std::abs(obstacle_in_car.x() - dec_max_x)); double dy_min = std::min( std::abs(obstacle_in_car.y() - dec_min_y), std::abs(obstacle_in_car.y() - dec_max_y)); dist_to_dec = std::min(dx_min, dy_min); } // 计算(相对)减速区域宽度 double dec_width = dist_to_dec + dist_to_stop; // 安全系数计算 double ratio = (dec_width > 0) ? (dist_to_stop / dec_width) : 0; // 凹函数:s = 1 - (x/D)^2 double safety_factor = 1.0 - ratio * ratio; return std::clamp(safety_factor, 0.0, 1.0); // 限制在[0,1]范围 } // 碰撞检测函数 void ObstacleAvoidance::GJKCheckCollision( std::vector &all_points, int current_index, const kx_topic::msg::PerceptionState &perception_state, double current_speed, double ¤t_detect_distance_out, std::vector<std::string> ¤t_active_io_obstacle_out, Planning::CollisionInfo &response_collision_out) { // 获取当前路径段配置的活动IO障碍物列表 if (current_index >= 0 && current_index < static_cast<int>(all_points.size())) { current_active_io_obstacle_out = all_points[current_index].belong_route.obstacle_active_io_obstacles; } else { current_active_io_obstacle_out.clear(); } // 将活动的IO障碍物添加到当前活动列表中(如果它们不在列表中) for (size_t i = 0; i < planning_->param_data_force_active_io_obstacles_.size(); i++) { if (std::find(current_active_io_obstacle_out.begin(), current_active_io_obstacle_out.end(), planning_->param_data_force_active_io_obstacles_[i]) == current_active_io_obstacle_out.end()) { // 没有就添加 current_active_io_obstacle_out.emplace_back(planning_->param_data_force_active_io_obstacles_[i]); } } // 过滤掉不在强制活动列表中的障碍物 std::vector<std::string>::iterator it_end = std::remove_if(current_active_io_obstacle_out.begin(), current_active_io_obstacle_out.end(), [this](std::string io_obstacle) -> bool { return std::find(planning_->param_data_force_active_io_obstacles_.begin(), planning_->param_data_force_active_io_obstacles_.end(), io_obstacle) != planning_->param_data_force_active_io_obstacles_.end(); }); current_active_io_obstacle_out.assign(current_active_io_obstacle_out.begin(), it_end); // 碰撞相应初始化 response_collision_out.type = CollisionInfo::Type::NoCollision; response_collision_out.limit_speed = INFINITY; response_collision_out.car_distance = INFINITY; response_collision_out.effect_io_obstacles.clear(); response_collision_out.car_pose_in_map.setPose(KX::Pos(0, 0, 0), KX::Ori(0, 0, 0)); response_collision_out.obstacle_point_in_map.setXYZ(0, 0, 0); response_collision_out.obstacle_point_in_current_car.setXYZ(0, 0, 0); response_collision_out.obstacle_point_in_future_car.setXYZ(0, 0, 0); response_collision_out.car_polygon_in_future_car.points.clear(); response_collision_out.car_polygon_in_map.points.clear(); response_collision_out.car_stop_polygon_in_map.points.clear(); response_collision_out.car_limit_polygon_in_map.points.clear(); // 路径有效性检查 if (!all_points.size() || current_index < 0 || current_index >= (int)all_points.size()) return; // 运动方向判断(点积) // bool is_car_front_move = all_points[current_index].dir.dot(planning_->topic_result_pose_fusion_dir2d_) > 0; // 获取包围盒参数 const double dec_offset_front = planning_->get_parameter(planning_->PARAM_NAME_GJK_DEC_OFFSET_FRONT).as_double(); const double dec_offset_back = planning_->get_parameter(planning_->PARAM_NAME_GJK_DEC_OFFSET_BACK).as_double(); const double dec_offset_left = planning_->get_parameter(planning_->PARAM_NAME_GJK_DEC_OFFSET_LEFT).as_double(); const double dec_offset_right = planning_->get_parameter(planning_->PARAM_NAME_GJK_DEC_OFFSET_RIGHT).as_double(); // const double dec_offset_top = planning_->get_parameter(planning_->PARAM_NAME_GJK_DEC_OFFSET_TOP).as_double(); // const double dec_offset_bottom = planning_->get_parameter(planning_->PARAM_NAME_GJK_DEC_OFFSET_BOTTOM).as_double(); const double stop_offset_front = planning_->get_parameter(planning_->PARAM_NAME_GJK_STOP_OFFSET_FRONT).as_double(); const double stop_offset_back = planning_->get_parameter(planning_->PARAM_NAME_GJK_STOP_OFFSET_BACK).as_double(); const double stop_offset_left = planning_->get_parameter(planning_->PARAM_NAME_GJK_STOP_OFFSET_LEFT).as_double(); const double stop_offset_right = planning_->get_parameter(planning_->PARAM_NAME_GJK_STOP_OFFSET_RIGHT).as_double(); // const double stop_offset_top = planning_->get_parameter(planning_->PARAM_NAME_GJK_STOP_OFFSET_TOP).as_double(); // const double stop_offset_bottom = planning_->get_parameter(planning_->PARAM_NAME_GJK_STOP_OFFSET_BOTTOM).as_double(); /* ****雷达碰撞**** */ if (planning_->get_parameter(planning_->PARAM_NAME_TRACK_BEZIER3_COLLISION_IS_ENABLE_ROUTE_FORECAST).as_bool()) { // 请空路径点的碰撞状态 for (auto &point : all_points) { point.collision_limit_speed = INFINITY; point.collision_driving_brake_request = 0; } /* ****预测碰撞**** */ size_t forward_look_ahead_points_ = planning_->get_parameter(planning_->PARAM_NAME_GJK_FORWARD_LOOK_AHEAD_POINTS).as_int(); // 前向探测点数 double speed_threshold_ = planning_->get_parameter(planning_->PARAM_NAME_GJK_SPEED_THRESHOLD).as_double(); // 速度阈值,超过该阈值就减速 double percentage_of_deceleration_ = planning_->get_parameter(planning_->PARAM_NAME_GJK_PERCENTAGE_OF_DECELERATION).as_double(); // 减速百分比 double safe_stop_distance_ = planning_->get_parameter(planning_->PARAM_NAME_GJK_SAFE_STOP_DISTANCE).as_double(); // 安全停车距离 std::vector<size_t> check_indices; if (current_index == 0) { // 起始点之后x点 size_t end_index = std::min(all_points.size(), current_index + forward_look_ahead_points_); for (size_t i = current_index; i <= end_index; i++) { check_indices.emplace_back(i); } } else { // 非起始点 size_t target_index = current_index + forward_look_ahead_points_ - 1; if (target_index < all_points.size()) { check_indices.emplace_back(target_index); } // 终点之前x点 if (target_index >= all_points.size()) { for (size_t i = current_index; i < all_points.size(); i++) { check_indices.emplace_back(i); } } } bool prediction_collition_detected = false; double min_prediction_limit_speed = INFINITY; size_t collision_index = SIZE_MAX; for (size_t index : check_indices) { // 获取该路径点 auto &point = all_points[index]; // 车辆外形 geometry_msgs::msg::Polygon base_car_polygon = point.belong_route.is_polygon_without_fork ? perception_state.car_polygon_2d_without_fork : perception_state.car_polygon_2d; // 未来车辆位姿 // KX::Pose future_car_pose(KX::Pose(point.pos.x(), point.pos.y(), 0.0, 0.0, 0.0, // is_car_front_move ? point.dir.theta() : point.dir.theta() + M_PI)); KX::Pose future_car_pose(KX::Pose(point.pos.x(), point.pos.y(), 0.0, 0.0, 0.0, point.dir.theta())); // 创建停止包围盒 auto stop_polygon = calExtensionPolygon(base_car_polygon, // all_points[current_index].belong_route.obstacle_stop_offset_front, // all_points[current_index].belong_route.obstacle_stop_offset_back, // all_points[current_index].belong_route.obstacle_stop_offset_left, // all_points[current_index].belong_route.obstacle_stop_offset_right); stop_offset_front, stop_offset_back, stop_offset_left, stop_offset_right); // 包围盒坐标系变换 std::vector<Shape> stop_polygon_shapes = convertPolygonToShapes(stop_polygon); auto transformed_stop_polygon = transformShapes(stop_polygon_shapes, future_car_pose); // 检测与所有障碍物的碰撞 bool stop_collision = false; for (const auto &obstacle : perception_state.obstacles) { std::vector<Shape> obstacle_shapes = convertPolygonToShapes(obstacle.polygon); auto transformed_obstacle_polygon = transformShapes(obstacle_shapes, future_car_pose); if (gjk(transformed_stop_polygon.data(), transformed_obstacle_polygon.data(), transformed_stop_polygon.size(), transformed_obstacle_polygon.size())) { stop_collision = true; break; } } if (stop_collision) // 预测到碰撞,立即减速 { prediction_collition_detected = true; collision_index = index; // 降速 if ((all_points[collision_index].collision_limit_speed || all_points[collision_index - 2 / (size_t)planning_->track_bezier3_waypoints_interval_m_].collision_limit_speed) > speed_threshold_) // 碰撞点速度规划的速度 > 阈值 { double safe_speed = current_speed * percentage_of_deceleration_; min_prediction_limit_speed = std::min(min_prediction_limit_speed, safe_speed); } break; } } if (prediction_collition_detected && current_speed > speed_threshold_) // 更新路径点限速 { // 计算安全停车位置 double collision_distance = all_points[collision_index].distance; double stop_distance = collision_distance - safe_stop_distance_; // 查找停车索引 size_t stop_index = collision_index; for (int i = collision_index; i >= static_cast<int>(current_index); i--) { if (all_points[i].distance <= stop_distance) { stop_index = i; break; } } for (size_t k = current_index; k < stop_index; k++) { if (min_prediction_limit_speed < all_points[k].collision_limit_speed) { all_points[k].collision_limit_speed = min_prediction_limit_speed; } } response_collision_out.limit_speed = min_prediction_limit_speed; } /* ****预测碰撞结束**** */ double min_collision_distance = INFINITY; KX::Pose collision_pose; KX::Pos collision_point; double global_limit_speed = INFINITY; // double global_speed_change_distance = current_detect_distance_out; size_t start_index = (current_index == 0) ? 0 : current_index - 1; for (size_t i = start_index; i < all_points.size(); i++) { const double distance_diff = all_points[i].distance - all_points[current_index].distance; // 超出检测距离 , break if (distance_diff > current_detect_distance_out) break; // 车辆外形 geometry_msgs::msg::Polygon base_car_polygon = all_points[i].belong_route.is_polygon_without_fork ? perception_state.car_polygon_2d_without_fork : perception_state.car_polygon_2d; // 未来车辆位姿 // KX::Pose future_car_pose(KX::Pose(all_points[i].pos.x(), all_points[i].pos.y(), 0.0, 0.0, 0.0, // is_car_front_move ? all_points[i].dir.theta() : all_points[i].dir.theta() + M_PI)); KX::Pose future_car_pose(KX::Pose(all_points[i].pos.x(), all_points[i].pos.y(), 0.0, 0.0, 0.0, all_points[i].dir.theta())); // 创建减速、停止包围盒 auto dec_polygon = calExtensionPolygon(base_car_polygon, dec_offset_front, dec_offset_back, // all_points[current_index].belong_route.obstacle_limit_offset_left, // all_points[current_index].belong_route.obstacle_limit_offset_right); dec_offset_left, dec_offset_right); auto stop_polygon = calExtensionPolygon(base_car_polygon, // all_points[current_index].belong_route.obstacle_stop_offset_front, // all_points[current_index].belong_route.obstacle_stop_offset_back, // all_points[current_index].belong_route.obstacle_stop_offset_left, // all_points[current_index].belong_route.obstacle_stop_offset_right); stop_offset_front, stop_offset_back, stop_offset_left, stop_offset_right); // 包围盒变换到世界坐标系 std::vector<Shape> dec_polygon_shapes = convertPolygonToShapes(dec_polygon); auto transformed_dec_polygon = transformShapes(dec_polygon_shapes, future_car_pose); std::vector<Shape> stop_polygon_shapes = convertPolygonToShapes(stop_polygon); auto transformed_stop_polygon = transformShapes(stop_polygon_shapes, future_car_pose); // 检测与所有障碍物的碰撞 bool stop_collision = false; bool dec_collision = false; KX::Pos current_collision_point; // double current_collision_distance = INFINITY; double point_limit_speed = INFINITY; // double point_speed_change_distance = current_detect_distance_out; for (const auto &obstacle : perception_state.obstacles) { // 转换障碍物外形-Shape std::vector<Shape> obstacle_shapes = convertPolygonToShapes(obstacle.polygon); auto transformed_obstacle = transformShapes(obstacle_shapes, future_car_pose); // 检测停止区域碰撞 if (gjk(transformed_stop_polygon.data(), transformed_obstacle.data(), transformed_stop_polygon.size(), transformed_obstacle.size())) { stop_collision = true; current_collision_point = KX::Pos(obstacle.center_x, obstacle.center_y, obstacle.center_z); point_limit_speed = 0; // 立即停止 // point_speed_change_distance = 0; break; // 停止碰撞优先级最高 } // 检测减速区域碰撞 if (gjk(transformed_dec_polygon.data(), transformed_obstacle.data(), transformed_dec_polygon.size(), transformed_obstacle.size())) { dec_collision = true; current_collision_point = KX::Pos(obstacle.center_x, obstacle.center_y, obstacle.center_z); // 计算安全系数(离停止区域越近,安全系数越低)0-1 double safety_factor = calSafetyFactor( current_collision_point, future_car_pose, base_car_polygon, // all_points[current_index].belong_route.obstacle_stop_offset_front, // all_points[current_index].belong_route.obstacle_stop_offset_back, // all_points[current_index].belong_route.obstacle_stop_offset_left, // all_points[current_index].belong_route.obstacle_stop_offset_right, stop_offset_front, stop_offset_back, stop_offset_left, stop_offset_right, dec_offset_front, dec_offset_back, // all_points[current_index].belong_route.obstacle_limit_offset_left, // all_points[current_index].belong_route.obstacle_limit_offset_right); dec_offset_left, dec_offset_right); double current_limit_speed = current_speed * safety_factor; // 限速处理 if (current_limit_speed < point_limit_speed) { point_limit_speed = current_limit_speed; } } } // 如果有碰撞,更新碰撞信息 if (stop_collision || dec_collision) { // 更新全局碰撞信息 if (point_limit_speed < global_limit_speed) { global_limit_speed = point_limit_speed; collision_pose = future_car_pose; collision_point = current_collision_point; min_collision_distance = distance_diff; response_collision_out.car_stop_polygon_in_map = convertShapesToPolygon(transformed_stop_polygon); response_collision_out.car_limit_polygon_in_map = convertShapesToPolygon(transformed_dec_polygon); std::vector<Shape> base_car_shapes = convertPolygonToShapes(base_car_polygon); auto transfromed_base_car_polygon = transformShapes(base_car_shapes, future_car_pose); response_collision_out.car_polygon_in_map = convertShapesToPolygon(transfromed_base_car_polygon); // 更新碰撞相应信息 response_collision_out.type = CollisionInfo::Type::LidarCollision; response_collision_out.limit_speed = global_limit_speed; response_collision_out.car_distance = min_collision_distance; response_collision_out.car_pose_in_map = collision_pose; response_collision_out.obstacle_point_in_map = collision_point; response_collision_out.obstacle_point_in_current_car = collision_point - planning_->topic_result_pose_fusion_; response_collision_out.obstacle_point_in_future_car = collision_point - collision_pose; response_collision_out.car_polygon_in_future_car = base_car_polygon; } } } /* ****近距离碰撞保护**** */ if (planning_->get_parameter(planning_->PARAM_NAME_TRACK_BEZIER3_COLLISION_IS_USE_NEAR_PROTECT).as_bool()) { // 获取车体外形 const auto &base_car_polygon = all_points[current_index].belong_route.is_polygon_without_fork ? perception_state.car_polygon_2d_without_fork : perception_state.car_polygon_2d; // 创建减速、停止包围盒 重复计算? auto dec_polygon = calExtensionPolygon(base_car_polygon, dec_offset_front, dec_offset_back, // all_points[current_index].belong_route.obstacle_limit_offset_left, // all_points[current_index].belong_route.obstacle_limit_offset_right); dec_offset_left, dec_offset_right); auto stop_polygon = calExtensionPolygon(base_car_polygon, // all_points[current_index].belong_route.obstacle_stop_offset_front, // all_points[current_index].belong_route.obstacle_stop_offset_back, // all_points[current_index].belong_route.obstacle_stop_offset_left, // all_points[current_index].belong_route.obstacle_stop_offset_right); stop_offset_front, stop_offset_back, stop_offset_left, stop_offset_right); // 包围盒变换到世界坐标系 std::vector<Shape> dec_polygon_shapes = convertPolygonToShapes(dec_polygon); auto transformed_dec_polygon = transformShapes(dec_polygon_shapes, planning_->topic_result_pose_fusion_); std::vector<Shape> stop_polygon_shapes = convertPolygonToShapes(stop_polygon); auto transformed_stop_polygon = transformShapes(stop_polygon_shapes, planning_->topic_result_pose_fusion_); // 检测与所有障碍物的碰撞 bool stop_collision = false; bool dec_collision = false; KX::Pos collision_point; double near_limit_speed = INFINITY; // double near_speed_change_distance = 0; for (const auto &obstacle : perception_state.obstacles) { // 转换障碍物形状 std::vector<Shape> obstacle_shapes = convertPolygonToShapes(obstacle.polygon); auto transformed_obstacle = transformShapes(obstacle_shapes, planning_->topic_result_pose_fusion_); // 检测停止区域碰撞 if (gjk(transformed_stop_polygon.data(), transformed_obstacle.data(), transformed_stop_polygon.size(), transformed_obstacle.size())) { stop_collision = true; collision_point = KX::Pos(obstacle.center_x, obstacle.center_y, obstacle.center_z); break; // 停止碰撞优先级最高 } // 检测减速碰撞 if (gjk(transformed_dec_polygon.data(), transformed_obstacle.data(), transformed_dec_polygon.size(), transformed_obstacle.size())) { dec_collision = true; collision_point = KX::Pos(obstacle.center_x, obstacle.center_y, obstacle.center_z); // 计算安全系数 double safety_factor = calSafetyFactor( collision_point, planning_->topic_result_pose_fusion_, base_car_polygon, // all_points[current_index].belong_route.obstacle_stop_offset_front, // all_points[current_index].belong_route.obstacle_stop_offset_back, // all_points[current_index].belong_route.obstacle_stop_offset_left, // all_points[current_index].belong_route.obstacle_stop_offset_right, stop_offset_front, stop_offset_back, stop_offset_left, stop_offset_right, dec_offset_front, dec_offset_back, // all_points[current_index].belong_route.obstacle_limit_offset_left, // all_points[current_index].belong_route.obstacle_limit_offset_right); dec_offset_left, dec_offset_right); // double current_limit_speed = safety_factor * current_speed; // 限速处理 if (current_limit_speed < near_limit_speed) { near_limit_speed = current_limit_speed; } } } // 处理碰撞响应 if (stop_collision || dec_collision) { const double brake_value = planning_->get_parameter(planning_->PARAM_NAME_TRACK_BEZIER3_COLLISION_IO_COLLISION_DRIVING_BRAKE).as_double(); // 设置全路径停车 for (auto &point : all_points) { if (stop_collision) { point.collision_limit_speed = 0; } else if (dec_collision) { // if (near_limit_speed < point.collision_limit_speed) // { // point.collision_limit_speed = near_limit_speed; // } point.collision_limit_speed = std::min(point.collision_limit_speed, near_limit_speed); } point.collision_driving_brake_request = brake_value; } // 更新碰撞相应信息 if (stop_collision || (dec_collision && near_limit_speed < response_collision_out.limit_speed)) { response_collision_out.car_stop_polygon_in_map = convertShapesToPolygon(transformed_stop_polygon); response_collision_out.car_limit_polygon_in_map = convertShapesToPolygon(transformed_dec_polygon); std::vector<Shape> base_car_shapes = convertPolygonToShapes(base_car_polygon); auto transfromed_base_car_polygon = transformShapes(base_car_shapes, planning_->topic_result_pose_fusion_); response_collision_out.car_polygon_in_map = convertShapesToPolygon(transfromed_base_car_polygon); response_collision_out.type = CollisionInfo::Type::LidarCollision; response_collision_out.limit_speed = stop_collision ? 0 : near_limit_speed; response_collision_out.car_distance = 0; response_collision_out.car_pose_in_map = planning_->topic_result_pose_fusion_; response_collision_out.obstacle_point_in_map = collision_point; response_collision_out.obstacle_point_in_current_car = collision_point - planning_->topic_result_pose_fusion_; response_collision_out.obstacle_point_in_future_car = collision_point - planning_->topic_result_pose_fusion_; response_collision_out.car_polygon_in_future_car = base_car_polygon; } } } /* ****雷达碰撞结束**** */ /* ****IO碰撞**** */ bool io_collision_detected = false; for (const auto &io_obstacle : current_active_io_obstacle_out) { if (std::find(perception_state.occur_io_obstacles.begin(), perception_state.occur_io_obstacles.end(), io_obstacle) != perception_state.occur_io_obstacles.end()) { response_collision_out.effect_io_obstacles.emplace_back(io_obstacle); io_collision_detected = true; } } if (io_collision_detected) { const double brake_value = planning_->get_parameter(planning_->PARAM_NAME_TRACK_BEZIER3_COLLISION_IO_COLLISION_DRIVING_BRAKE).as_double(); const double io_limit_speed = 0; // IO避障要求完全停止 // 设置全径停车 for (auto &point : all_points) { if (io_limit_speed < point.collision_limit_speed) { point.collision_limit_speed = io_limit_speed; } point.collision_driving_brake_request = brake_value; } // 更新碰撞状态 response_collision_out.type = CollisionInfo::Type::IoCollision; } /* ****IO碰撞结束**** */ } } /* ****************************3d_gjk算法实现步骤**************************** */ /* 绕x轴旋转一个点 传入旋转角度,单位度*/ ObstacleAvoidance::Shape ObstacleAvoidance::Shape::rot_x(float rot_x) { float rad = rot_x * M_PI / 180.0f; float cos_rad = cos(rad); float sin_rad = sin(rad); Shape return_shape; return_shape.x = x; return_shape.y = y * cos_rad - z * sin_rad; return_shape.z = y * sin_rad + z * cos_rad; return return_shape; } /* 绕y轴旋转一个点 传入旋转角度,单位度*/ ObstacleAvoidance::Shape ObstacleAvoidance::Shape::rot_y(float rot_y) { float rad = rot_y * M_PI / 180.0f; float cos_rad = cos(rad); float sin_rad = sin(rad); Shape return_shape; return_shape.x = x * cos_rad + z * sin_rad; return_shape.y = y; return_shape.z = z * cos_rad - x * sin_rad; return return_shape; } /* 绕z轴旋转一个点 传入旋转角度,单位度*/ ObstacleAvoidance::Shape ObstacleAvoidance::Shape::rot_z(float rot_z) { float rad = rot_z * M_PI / 180.0f; float cos_rad = cos(rad); float sin_rad = sin(rad); Shape return_shape; return_shape.x = x * cos_rad - y * sin_rad; return_shape.y = x * sin_rad + y * cos_rad; return_shape.z = z; return return_shape; } /* 对形状(向量)取反 */ ObstacleAvoidance::Shape ObstacleAvoidance::Shape::negate() { Shape result; result.x = -x; result.y = -y; result.z = -z; return result; } /* 两个形状(向量)的点积 */ float ObstacleAvoidance::Shape::dot(Shape d) { float dotproduct; dotproduct = d.x * x + d.y * y + d.z * z; return dotproduct; } /* 两个形状(向量)的叉积 */ ObstacleAvoidance::Shape ObstacleAvoidance::Shape::cross(Shape d) { Shape crossproduct; crossproduct.x = y * d.z - z * d.y; crossproduct.y = z * d.x - x * d.z; crossproduct.z = x * d.y - y * d.x; return crossproduct; } /* 点积函数 */ float dot_prod(float x1, float y1, float z1, float x2, float y2, float z2) { return (x1 * x2 + y1 * y2 + z1 * z2); } /* 叉积函数 a x (b x c) = b (a·c) - c (a·b)*/ ObstacleAvoidance::Shape triple_cross_prod(ObstacleAvoidance::Shape a, ObstacleAvoidance::Shape b, ObstacleAvoidance::Shape c) { ObstacleAvoidance::Shape triple_return; triple_return.x = b.x * a.dot(c) - c.x * a.dot(b); triple_return.y = b.y * a.dot(c) - c.y * a.dot(b); triple_return.z = b.z * a.dot(c) - c.z * a.dot(b); return triple_return; } /* 计算质心函数 dim_a为A的顶点数*/ ObstacleAvoidance::Shape mass_middle_point(ObstacleAvoidance::Shape *A, int dim_a) { ObstacleAvoidance::Shape mm; mm.x = 0; mm.y = 0; mm.z = 0; for (int i = 0; i < dim_a; i++) { mm.x += A[i].x; mm.y += A[i].y; mm.z += A[i].z; } mm.x /= dim_a; mm.y /= dim_a; mm.z /= dim_a; return mm; } /* Support 函数 传入:形状A, 形状B, 形状A和B的元素数量, 搜索的反方向 输出:单纯型点 */ ObstacleAvoidance::Shape support_function(ObstacleAvoidance::Shape *A, ObstacleAvoidance::Shape *B, int dim_a, int dim_b, ObstacleAvoidance::Shape sd) { float max_val1 = -INFINITY; int max_index1 = -1; float dotp = 0.0; // 在形状A上沿sd方向找最远点 for (int i = 0; i < dim_a; i++) { dotp = dot_prod(A[i].x, A[i].y, A[i].z, sd.x, sd.y, sd.z); if (dotp > max_val1) { max_val1 = dotp; max_index1 = i; } } // 在形状B上沿-sd方向找最远点 float max_val2 = -INFINITY; int max_index2 = -1; for (int i = 0; i < dim_b; i++) { dotp = dot_prod(B[i].x, B[i].y, B[i].z, -sd.x, -sd.y, -sd.z); if (dotp > max_val2) { max_val2 = dotp; max_index2 = i; } } ObstacleAvoidance::Shape return_shape; return_shape.x = A[max_index1].x - B[max_index2].x; return_shape.y = A[max_index1].y - B[max_index2].y; return_shape.z = A[max_index1].z - B[max_index2].z; return return_shape; } /* 向单纯形添加点 */ void ObstacleAvoidance::Simplex::add(Shape simplex_add) { int i = 3; // 从后往前找到第一个空位 while (i >= 0) { if (x[i] != -INFINITY) { break; } i--; } i++; // 在找到的位置添加新点 x[i] = simplex_add.x; y[i] = simplex_add.y; z[i] = simplex_add.z; } /* 初始化单纯型列表 */ void ObstacleAvoidance::Simplex::set_zero(void) { for (int i = 0; i < 4; i++) { // 将所有值设为-INFINITY避免无效数据 x[i] = -INFINITY; y[i] = -INFINITY; z[i] = -INFINITY; } } /* 获取最后添加的点 */ ObstacleAvoidance::Shape ObstacleAvoidance::Simplex::getLast(void) { Shape return_last; int i = 3; while (i >= 0) { if (x[i] != -INFINITY) { break; } i--; } return_last.x = x[i]; return_last.y = y[i]; return_last.z = z[i]; return return_last; } /* 获取指定索引(id = 0-3)的点的x/y/z值 */ ObstacleAvoidance::Shape ObstacleAvoidance::Simplex::get(int id) { Shape return_shape; if (id < 0 || id > 3) { std::cout << "无法获取索引为 " << id << " 的单纯形点" << std::endl; } return_shape.x = x[id]; return_shape.y = y[id]; return_shape.z = z[id]; return return_shape; } /* 计算单纯形中的有效点数 */ int ObstacleAvoidance::Simplex::size(void) { int count = 0; for (int i = 0; i < 4; i++) { if (x[i] != -INFINITY) { count++; } } return count; } /* 从单纯形中删除指定点 */ void ObstacleAvoidance::Simplex::del(int id) // id = 1、2、3、4 { // 安全检查:计算当前有效点数 int simplex_elements = 0; for (int i = 0; i < 4; i++) { if (x[i] != -INFINITY) { simplex_elements++; } } if (simplex_elements < 4 || id == 4) // 无法删除最后添加的点 { std::cout << "错误 -> 单纯形元素数: " << simplex_elements << " | 要删除的id: " << id << std::endl; } else { // 缓存形状数据 float cx[3], cy[3], cz[3]; int c = 0; id--; // 调整索引从0开始 // 备份除删除点外的所有点 for (int i = 0; i < 4; i++) { if (i != id) { cx[c] = x[i]; cy[c] = y[i]; cz[c] = z[i]; c++; } x[i] = -INFINITY; y[i] = -INFINITY; z[i] = -INFINITY; } // 恢复备份的点 for (int i = 0; i < 3; i++) { x[i] = cx[i]; y[i] = cy[i]; z[i] = cz[i]; } } } /* 检查原点是否在单纯形内 */ bool containsOrigin(ObstacleAvoidance::Simplex &simplex, ObstacleAvoidance::Shape &d) { const float EPSILON = 1e-5f; ObstacleAvoidance::Shape a = simplex.getLast(); ObstacleAvoidance::Shape a0 = a.negate(); // AO向量 switch (simplex.size()) { case 2: { // 线段情况 ObstacleAvoidance::Shape b = simplex.get(0); ObstacleAvoidance::Shape ab = {b.x - a.x, b.y - a.y, b.z - a.z}; // 计算垂直方向 if (ab.dot(a0) > EPSILON) { d = triple_cross_prod(ab, a0, ab); } else { d = a0; } return false; } case 3: { // 三角形情况 ObstacleAvoidance::Shape b = simplex.get(1); ObstacleAvoidance::Shape c = simplex.get(0); ObstacleAvoidance::Shape ab = {b.x - a.x, b.y - a.y, b.z - a.z}; ObstacleAvoidance::Shape ac = {c.x - a.x, c.y - a.y, c.z - a.z}; ObstacleAvoidance::Shape abc = ab.cross(ac); // 三角形法线 // 检查边缘区域 ObstacleAvoidance::Shape abc_c_ac = abc.cross(ac); if (abc_c_ac.dot(a0) > EPSILON) { if (ac.dot(a0) > EPSILON) { d = triple_cross_prod(ac, a0, ac); } else { if (ab.dot(a0) > EPSILON) { d = triple_cross_prod(ab, a0, ab); } else { d = a0; } } } else { ObstacleAvoidance::Shape ab_c_abc = ab.cross(abc); if (ab_c_abc.dot(a0) > EPSILON) { if (ab.dot(a0) > EPSILON) { d = triple_cross_prod(ab, a0, ab); } else { d = a0; } } else { // 原点在三角形上方或下方 if (abc.dot(a0) > EPSILON) { d = abc; } else { d = abc.negate(); } } } return false; } case 4: { // 四面体情况 ObstacleAvoidance::Shape p_d = simplex.get(0); // 点d ObstacleAvoidance::Shape p_c = simplex.get(1); // 点c ObstacleAvoidance::Shape p_b = simplex.get(2); // 点b // 计算边向量 ObstacleAvoidance::Shape ac = {p_c.x - a.x, p_c.y - a.y, p_c.z - a.z}; ObstacleAvoidance::Shape ab = {p_b.x - a.x, p_b.y - a.y, p_b.z - a.z}; ObstacleAvoidance::Shape ad = {p_d.x - a.x, p_d.y - a.y, p_d.z - a.z}; // 计算各面的法向量 ObstacleAvoidance::Shape abc = ac.cross(ab); float v_abc = abc.dot(a0); ObstacleAvoidance::Shape abd = ab.cross(ad); float v_abd = abd.dot(a0); ObstacleAvoidance::Shape acd = ad.cross(ac); float v_acd = acd.dot(a0); // 使用相对容差值 float max_abs = fmax(fabs(v_abc), fmax(fabs(v_abd), fabs(v_acd))); float rel_epsilon = EPSILON * fmax(1.0f, max_abs); // 检查原点是否在四面体内 if (v_abc > -rel_epsilon && v_abd > -rel_epsilon && v_acd > -rel_epsilon) { return true; } // 确定要移除的点 if (v_abc < v_abd && v_abc < v_acd) { simplex.del(1); // 移除点c d = (v_abc > rel_epsilon) ? abc : abc.negate(); } else if (v_abd < v_acd) { simplex.del(2); // 移除点b d = (v_abd > rel_epsilon) ? abd : abd.negate(); } else { simplex.del(3); // 移除点d d = (v_acd > rel_epsilon) ? acd : acd.negate(); } return false; } default: // 不应该到达这里 d = a0; return false; } } // gjk主函数 bool ObstacleAvoidance::gjk(Shape *A, Shape *B, int dim_a, int dim_b) { const int MAX_ITERATIONS = 100; const float EPSILON = 1e-5f; Simplex simplex; // 计算质心 Shape mma = mass_middle_point(A, dim_a); Shape mmb = mass_middle_point(B, dim_b); // 初始方向 (B质心指向A质心) Shape d; d.x = mmb.x - mma.x; d.y = mmb.y - mma.y; d.z = mmb.z - mma.z; // 避免零向量 if (fabs(d.x) < EPSILON && fabs(d.y) < EPSILON && fabs(d.z) < EPSILON) { d.x = 1.0f; } // 规范化方向向量 float len = sqrt(d.x * d.x + d.y * d.y + d.z * d.z); if (len > EPSILON) { d.x /= len; d.y /= len; d.z /= len; } // 获取第一个支撑点 Shape s = support_function(A, B, dim_a, dim_b, d); simplex.add(s); // 初始搜索方向指向原点 d = s.negate(); int iterations = 0; while (iterations < MAX_ITERATIONS) { iterations++; // 获取新支撑点 s = support_function(A, B, dim_a, dim_b, d); // 检查新点是否在搜索方向的后方 float dot_product = s.dot(d); if (dot_product < -EPSILON) { return false; // 明确在后方,无碰撞 } // 检查点是否非常接近原点 float dist_sq = s.x * s.x + s.y * s.y + s.z * s.z; if (dist_sq < EPSILON) { return true; // 非常接近原点,视为碰撞 } // 添加新点到单纯形 simplex.add(s); // 检查原点是否在单纯形内 if (containsOrigin(simplex, d)) { return true; } // 检查搜索方向是否接近零 float dir_len_sq = d.x * d.x + d.y * d.y + d.z * d.z; if (dir_len_sq < EPSILON) { // 方向向量接近零,可能表示原点在单纯形边界上 return true; } } // 达到最大迭代次数仍未收敛 return false; } 。测试情况是:在rviz中,障碍物相对于原点显示,但不是相对于车体现

Exit code: 1 Errors while compiling: g:\codelearning\C_Breach_of_Faith.cpp: �ں�����void solve_breach_of_faith(int)����: g:\codelearning\C_Breach_of_Faith.cpp:31:107: ���󣺡�accumulate���ڴ�����������δ���� 0LL, [](long long acc, long long val){return acc+pow(-1,val)*val;})); ^ g:\codelearning\C_Breach_of_Faith.cpp:33:95: ���󣺡�LLONG_MAX���ڴ�����������δ���� temp_sequence.insert(next(begin(temp_sequence), static_cast(pos)), LLONG_MAX); ^~~~~~~~~ g:\codelearning\C_Breach_of_Faith.cpp:38:41: ����������Ϊ��double������ֵ��ʼ������Ϊ��double&���ķdz���������Ч auto& elem = idx >= pos ? temp_sequence[idx-((idx>=pos)?1:0)] : ((idx==pos)?expected_sum-temp_sequence[pos]:temp_sequence[idx]); ~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ g:\codelearning\C_Breach_of_Faith.cpp:48:50: ���󣺡�numeric_limits���ڴ�����������δ���� abs(roundl(abs(expected_sum))) < numeric_limits<double>::epsilon()){ ^~~~~~~~~~~~~~ g:\codelearning\C_Breach_of_Faith.cpp:48:65: ����expected primary-expression before ��double�� abs(roundl(abs(expected_sum))) < numeric_limits<double>::epsilon()){ ^~~~~~ g:\codelearning\C_Breach_of_Faith.cpp:48:65: ����expected ��)�� before ��double�� g:\codelearning\C_Breach_of_Faith.cpp:50:29: ���󣺶Գ�Ա��join������������ڡ�" "���У������߾��з������͡�const char [2]�� cout << " ".join(to_string(x) for(auto &x : temp_sequence)); ^~~~ g:\codelearning\C_Breach_of_Faith.cpp:50:44: ���󣺡�x���ڴ�����������δ���� cout << " ".join(to_string(x) for(auto &x : temp_sequence)); ^

最新推荐

recommend-type

springboot063知识管理系统.rar

springboot063知识管理系统.rar
recommend-type

Moon: 提升团队工作效率的网络界面

从给定的文件信息中,我们可以提取并详细阐释以下知识点: ### 标题知识点 #### Moon 网络界面 1. **定义团队状态**: Moon 应用程序提供了一个界面,用户可以据此定义自己的状态,如在线、忙碌、离开或离线。这一功能有助于团队成员了解彼此的可用性,从而减少不必要的打扰,提高工作效率。 2. **时间可用性管理**: Moon 旨在管理用户的时间可用性。通过提供一个平台来显示团队成员的状态,可以减少对工作流程的干扰,使团队能够更专注于手头的任务。 ### 描述知识点 #### 安装和使用Moon应用程序 1. **安装过程**: Moon应用程序通过使用Docker进行安装和运行,这是一种流行的容器化平台,允许开发者打包应用及其依赖于一个可移植的容器中,简化了部署过程。 - 使用git clone命令从GitHub克隆Moon项目的仓库。 - 进入克隆的项目目录。 - 使用docker build命令构建Moon应用程序的镜像。 - 最后,使用docker run命令运行应用程序。 2. **设置和环境变量**: 在运行Moon应用程序时,需要设置一系列环境变量来指定API的URI、端口和入口点。这些变量帮助应用程序正确地与后端API进行通信。 ### 标签知识点 #### 关键技术栈和应用领域 1. **React**: Moon应用程序很可能使用了React框架来构建其用户界面。React是一个由Facebook开发的前端JavaScript库,用于构建用户界面,尤其是单页应用程序(SPA)。 2. **生产力提升工具**: 从标签“productivity-booster”中我们可以推断,Moon被设计为一种提升个人或团队生产力的工具。它通过减少不必要的通信干扰来帮助用户专注于当前的工作任务。 3. **JavaScript**: 这个标签表明Moon应用程序的前端或后端可能广泛使用了JavaScript编程语言。JavaScript是一种广泛应用于网页开发中的脚本语言,能够实现动态交互效果。 ### 文件名称列表知识点 #### 文件和目录结构 1. **moon-master**: 文件名称“moon-master”暗示了Moon项目的主要目录。通常,“master”表示这是一个主分支或主版本的代码库,它包含了应用程序的核心功能和最新的开发进展。 ### 综合知识点 #### Moon 应用程序的价值和目标 - **提高专注度**: Moon应用程序允许用户设置特定的专注时间,这有助于提高工作效率和质量。通过将注意力集中在特定任务上,可以有效地降低多任务处理时的认知负荷和可能的干扰。 - **优化团队协作**: 明确的团队状态标识有助于减少不必要的沟通,从而使得团队成员能够在各自专注的时间内高效工作。这种管理方式还可以在团队中培养一种专注于当前任务的文化。 - **简洁性和易用性**: Moon的界面设计被描述为“漂亮”,这表明除了功能性外,用户界面的美观和简洁性也是该应用程序的重点,这有助于提高用户体验。 综上所述,Moon应用程序是一个旨在通过网络界面帮助用户管理个人和团队状态的工具,利用Docker进行简洁的部署,强化工作中的专注度,并通过简化团队状态的沟通,提升整体生产力。
recommend-type

远程控制ESP32-CAM机器人汽车及相关库的使用

# 远程控制ESP32 - CAM机器人汽车及相关库的使用 ## 1. 远程控制ESP32 - CAM机器人汽车 ### 1.1 硬件连接 ESP32 - CAM机器人汽车的硬件连接涉及多个组件,具体连接方式如下表所示: | 组件 | 连接到 | 再连接到 | | --- | --- | --- | | TB6612FNG VM | 18650电池正极 | LM2596 IN正极 | | TB6612FNG VCC | ESP32 - CAM VCC (3.3V) | - | | TB6612FNG GND | 18650电池负极 | LM2596 IN负极 | | TB6612FNG A1
recommend-type

CFE层流结构

### CFE层流结构在流量计中的定义和作用 在流量计中,CFE通常指 **Core Flow Executive** 或 **Control Flow Executive**,其“层流结构”(Laminar Flow Structure)是流量计内部用于实现高精度流体测量的核心部件之一。该结构的设计基于流体力学中的层流原理,通过特定几何形状的通道,使流体在通过时形成稳定的层流状态,从而便于测量流体的体积或质量流量。 层流结构通常由多个平行微通道或蜂窝状结构组成,其主要作用是消除流体流动中的湍流效应,确保流体以均匀、稳定的速度分布通过测量区域。这种设计显著提高了流量计的线性度和测量精度,尤
recommend-type

网络货币汇率计算器:实时汇率API应用

货币汇率计算器是一个实用的网络应用程序,它能够帮助用户进行不同货币之间的汇率计算。在这个应用中,用户可以输入一定数量的源货币金额,选择相应的货币对,然后计算出目标货币的等值金额。该应用程序主要涉及到前端技术的实现,包括HTML、CSS和JavaScript,这些技术在网页设计和开发中起着至关重要的作用。下面我们将详细介绍这些技术,以及如何使用这些技术开发货币汇率计算器。 ### HTML (HyperText Markup Language) HTML是构建网页内容的标记语言,是网页的基础。它通过一系列的标签(elements)来定义网页的结构和内容。在货币汇率计算器中,HTML用于创建用户界面,比如输入框、按钮和结果显示区域。HTML标签用于定义各种元素,例如: - `<form>`:用于创建一个表单,用户可以在此输入数据,比如货币金额和货币对。 - `<input>`:用于创建输入字段,用户可以在其中输入要转换的金额。 - `<button>`:用于创建按钮,用户点击按钮后触发汇率计算功能。 - `<span>` 或 `<div>`:用于创建显示计算结果的区域。 ### CSS (Cascading Style Sheets) CSS是一种样式表语言,用于设置网页的视觉格式,如布局、颜色、字体等。在货币汇率计算器中,CSS用来美化界面,提供良好的用户体验。CSS可能被用来: - 设置表单和按钮的样式,比如颜色、字体大小、边距和对齐。 - 定义结果展示区域的背景、文字颜色和字体样式。 - 响应式设计,确保应用在不同大小的屏幕上都可正确显示。 ### JavaScript JavaScript是一种在浏览器中运行的编程语言,它使网页可以交互,执行各种操作。在货币汇率计算器中,JavaScript负责处理用户输入、调用汇率API以及展示计算结果。JavaScript可能需要完成以下功能: - 获取用户输入的金额和选择的货币对。 - 调用一个汇率API来获取实时的货币汇率数据。 - 将获取到的汇率数据进行处理,并计算出目标货币的金额。 - 更新网页上的结果显示区域,展示最终的计算结果。 ### 使用汇率API 应用程序使用汇率API来显示数据,API(Application Programming Interface,应用程序编程接口)是一个使软件应用之间能够进行交互的接口。在货币汇率计算器中,需要注册并使用某个提供实时汇率信息的API服务。通过发送请求到API,并接收API返回的JSON或XML格式数据,应用程序可以获取到当前的汇率信息,并进行计算。 ### 开发货币汇率计算器的步骤 1. **项目准备**:创建项目文件夹,设置基础的HTML结构。 2. **界面设计**:使用HTML构建用户界面,用CSS进行样式设计。 3. **功能实现**:编写JavaScript代码,处理用户输入和调用汇率API。 4. **测试与调试**:确保应用在不同的浏览器和设备上运行无误。 5. **部署上线**:将应用程序部署到服务器上,供用户访问。 6. **维护更新**:根据用户反馈和市场汇率波动,定期更新应用。 ### 贡献与许可 该文档还提到了如何为该项目贡献代码。首先需要将项目克隆到本地计算机,然后创建一个新的分支进行修改或增加功能,之后将分支推送到自己的GitHub仓库,并向原项目提交一个拉取请求(Pull Request)。此外,文档提到了项目的许可信息,但具体的内容未在摘要中给出。 总结以上内容,货币汇率计算器是基于前端技术实现的一个应用程序,通过HTML、CSS和JavaScript技术构建用户界面并实现功能,它依赖于外部的汇率API来获取实时数据。开发者可以遵循文档中给出的步骤对项目进行贡献,并遵守项目的许可协议。
recommend-type

蓝牙低功耗(BLE)信标与通信技术详解

### 蓝牙低功耗(BLE)信标与通信技术详解 #### 1. BLE信标数据设置 在BLE应用中,信标数据的设置是关键步骤。以下是一段设置信标数据的代码示例: ```cpp beaconData[11] = 0xAD; beaconData[12] = 0x0C; // UUID Instance BID[0 to 5] beaconData[13] = 0xFA; // 0cfa43d07079 beaconData[14] = 0x43; beaconData[15] = 0xD0; beaconData[16] = 0x70; beaconData[17] = 0x79;
recommend-type

C++的只能锁

在 C++11 及后续标准中,`std::unique_lock` 和 `std::lock_guard` 是两种常用的智能锁机制,用于管理互斥量(`std::mutex`)的加锁和解锁操作。它们均属于 RAII(Resource Acquisition Is Initialization)风格的资源管理方式,确保在异常情况下也能正确释放互斥量资源。 ### lock_guard 的使用方法 `std::lock_guard` 是一种轻量级的智能锁,适用于简单的加锁场景。它在构造时自动加锁,在析构时自动解锁,不支持手动解锁或延迟加锁操作。 示例代码: ```cpp #include <
recommend-type

实施广义保护动量交易策略的matlab示例

### 知识点一:广义保护动量交易策略 广义保护动量交易策略是一种资产管理方法,它结合了动量策略和风险控制机制。动量策略是基于资产价格动量效应,即过去一段时间表现较好的资产,未来也可能持续表现良好。在广义保护动量策略中,投资者构建一个投资组合,该投资组合不仅考虑了资产的动量表现,还包含了对冲或保护性的资产,以减少潜在的市场波动风险。 该策略可以被视为保护性资产分配(Protective Asset Allocation, PAA)的一种变体。保护性资产分配主要目的是在追求较高回报的同时,通过调整资产组合中的风险和保护性资产,来降低资产组合可能面临的大幅回撤风险。 ### 知识点二:Keller和Keuning(2016)的理论框架 Keller和Keuning在2016年提出了一种方法,该方法基于动量效应,构建了一个能够提供风险保护的投资组合。他们的方法依据是,在过去某个时间段内表现较好的资产(动量资产)可能会在未来继续表现良好,同时,通过加入保护性资产(例如债券或黄金等避险资产),可以进一步降低投资组合整体的波动性和可能的损失。 ### 知识点三:MATLAB开发环境 MATLAB(矩阵实验室)是一个高性能的数值计算和可视化的开发环境。它广泛应用于工程、科学、金融等领域,用于数据分析、算法开发以及复杂计算。MATLAB提供了丰富的函数库和工具箱,使得用户可以快速开发和实现复杂的算法。 在金融领域,MATLAB常被用于量化分析、风险管理和资产管理等应用。它提供了一套完善的金融工具箱,包括金融衍生品定价、风险管理、回测历史数据、优化投资组合等功能。这些工具使得金融分析师和投资者可以更有效地进行投资决策和风险控制。 ### 知识点四:投资组合构建 在使用广义保护动量策略构建投资组合时,需要遵循以下步骤: 1. **动量指标选择**:选择合理的动量指标来衡量资产的历史表现,常见的指标包括价格变动的相对强度或绝对收益。 2. **风险和保护资产选择**:确定保护性资产来减轻风险,例如政府债券、黄金或其他低风险资产。 3. **组合权重分配**:根据动量表现和风险偏好,动态调整投资组合中各类资产的权重。这可能涉及到定期再平衡和优化算法。 4. **策略回测和评估**:在实际应用之前,通过历史数据对策略进行回测,以评估策略的有效性和风险管理能力。 ### 知识点五:参考资料的重要性 在研究和开发广义保护动量策略时,理解Keller和Keuning(2016)的研究成果是必不可少的。该参考资料为策略提供了理论基础和实践指导。在实施和开发的过程中,应当详细阅读并理解其方法和结论,确保策略实施与原理论的一致性,同时根据实际情况进行适当调整。 ### 知识点六:文件的文件名称列表 对于提供的【压缩包子文件的文件名称列表】中的“Generalized Protective Momentum.zip”,这表明我们将接触到一个压缩的文件包,其中可能包含了关于广义保护动量交易策略在MATLAB上的具体实现代码、示例、说明文档、数据集以及其他相关支持文件。该文件包可以作为实施该策略的一个实际操作指南,其中的代码和文档将指导用户如何使用MATLAB进行策略的开发和测试。
recommend-type

ESP32开发:字体、时间库及自定义库创建指南

# ESP32开发:字体、时间库及自定义库创建指南 ## 1. 字体相关操作 ### 1.1 生成并使用额外字体 要生成额外字体,需将生成的位图数据复制到草图中的一个标签(如 `newfont.h`),并使用 `#include "newfont.h"` 指令加载该标签。在执行 `print`、`drawString` 或 `drawNumber` 指令之前,使用 `tft.setFreeFont(&Rock_Salt_Regular_24)` 指令定义字体。以下是额外字体的位图示例: ```cpp const uint8_t Rock_Salt_Regular_24Bitmaps[] PR
recommend-type

ubuntu优化vi

优化 Ubuntu 中的 `vi` 编辑器使用体验可以通过多种方式实现,包括配置 `.vimrc` 文件、使用插件管理器、启用语法高亮和自动补全等功能。以下是一些具体的优化技巧: 1. **配置 `.vimrc` 文件** 通过编辑 `~/.vimrc` 文件,可以自定义 `vi` 的行为。以下是一些常用的配置选项: ```vim " 启用语法高亮 syntax on " 显示行号 set number " 启用自动缩进 set autoindent " 设置缩进宽度为4个字符 set shiftwidth=4 "