{"id":10476,"date":"2025-09-07T21:49:05","date_gmt":"2025-09-07T13:49:05","guid":{"rendered":"https:\/\/yanjingang.com\/blog\/?p=10476"},"modified":"2025-11-13T16:52:46","modified_gmt":"2025-11-13T08:52:46","slug":"lio-sam%e8%bf%87%e6%bb%a4gps%e5%a4%9a%e8%b7%af%e5%be%84%e6%95%88%e5%ba%94%e6%95%b0%e6%8d%ae","status":"publish","type":"post","link":"https:\/\/yanjingang.com\/blog\/?p=10476","title":{"rendered":"LIO-SAM\u2014\u53bb\u9664GPS\u591a\u8def\u5f84\u6548\u5e94\u5e72\u6270"},"content":{"rendered":"<p>\u6700\u8fd1\u5728\u4e0d\u540c\u73af\u5883\u4e0b\u8fdb\u884clio-sam\u5efa\u56fe\u65f6\uff0c\u9047\u5230\u4e00\u4e2a\u95ee\u9898\uff1a\u5728\u901a\u8fc7\u72ed\u7a84\u9053\u8def\u65f6\uff0c\u4e24\u4fa7\u9ad8\u8038\u7684\u5efa\u7b51\u53cd\u5c04\u536b\u661f\u4fe1\u53f7\uff0c\u5bfc\u81f4\u51fa\u73b0RTK\u89e3\u7b97status=1\/2 RTK\u589e\u5f3a\u5b9a\u4f4d\u65f6\uff081 SBAS_FIX\u661f\u57fa\u589e\u5f3a\u7cfb\u7edf\u4fee\u590d\uff0c2 GBAS_FIX\u5730\u57fa\u589e\u5f3a\u7cfb\u7edf\u4fee\u590d\uff09\uff0c\u4ecd\u7136\u51fa\u73b0\u5927\u5e45\u5ea6\u7684\u4f4d\u7f6e\u6f02\u79fb\uff0c\u4ece\u800c\u5bfc\u81f4GPS\u56e0\u5b50\u4ecb\u5165\u5236\u56fe\u7684\u4f4d\u7f6e\u51fa\u73b0\u9519\u4f4d\u3002<\/p>\n<h1>\u4e00\u3001\u6982\u8ff0<\/h1>\n<p>\u8fd9\u79cd\u73b0\u8c61\u79f0\u4e3a<strong>\u591a\u8def\u5f84\u6548\u5e94<\/strong>\uff08Multi-Path Effect\uff09\uff0c\u6709\u65f6\u4e5f\u7b80\u79f0\u591a\u5f84\u6548\u5e94\u3002\u5b83\u51fa\u73b0\u7684\u539f\u56e0\u662f\u5728GPS\u63a5\u6536\u673a\u63a5\u6536\u536b\u661f\u4fe1\u53f7\u7684\u8fc7\u7a0b\u4e2d\uff0c\u9664\u4e86\u63a5\u6536\u5230\u901a\u8fc7\u76f4\u7ebf\u8def\u5f84\u5230\u8fbe\u63a5\u6536\u673a\u7684\u4fe1\u53f7\u4ee5\u5916\uff0c\u8fd8\u53ef\u80fd\u6536\u5230\u7ecf\u63a5\u6536\u673a<strong>\u5468\u56f4\u7269\u4f53\u7684\u53cd\u5c04<\/strong>\u518d\u4f20\u64ad\u8fc7\u6765\u7684\u536b\u661f\u4fe1\u53f7\uff0c\u8fd9\u4e9b\u53cd\u5c04\u7684\u4fe1\u53f7\u6539\u53d8\u4e86\u4f20\u64ad\u65b9\u5411\u3001\u632f\u5e45\u3001\u6781\u5316\u4ee5\u53ca\u76f8\u4f4d\u7b49\uff0c\u4e0e\u76f4\u7ebf\u4fe1\u53f7\u4ea7\u751f\u53e0\u52a0\uff0c\u4ece\u800c\u4f7f\u89c2\u6d4b\u503c\u504f\u79bb\u5176\u771f\u503c\u800c\u4ea7\u751f\u8bef\u5dee\u3002\u591a\u5f84\u6548\u5e94\u4f1a\u5728\u63a5\u6536\u5929\u7ebf\u5904\u4e0e\u53cd\u5c04\u4fe1\u53f7\u5e72\u6270\u5bfc\u81f4\u539f\u59cb\u4fe1\u53f7\u5931\u771f\u3002\u8fd9\u53ef\u80fd\u5bfc\u81f4\u8d85\u8fc7<strong>10\u7c73<\/strong>\u7684<strong>\u4f4d\u7f6e\u8bef\u5dee<\/strong>\u3002<\/p>\n<p><a href=\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/gnss-loc-scaled.jpg\"><img loading=\"lazy\" class=\"alignnone wp-image-10493 size-full\" src=\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/gnss-loc-scaled.jpg\" alt=\"\" width=\"2560\" height=\"1062\" srcset=\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/gnss-loc-scaled.jpg 2560w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/gnss-loc-300x124.jpg 300w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/gnss-loc-1024x425.jpg 1024w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/gnss-loc-768x319.jpg 768w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/gnss-loc-1536x637.jpg 1536w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/gnss-loc-2048x850.jpg 2048w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/gnss-loc-624x259.jpg 624w\" sizes=\"(max-width: 2560px) 100vw, 2560px\" \/><\/a><\/p>\n<h1>\u4e8c\u3001\u89e3\u51b3\u65b9\u6848<\/h1>\n<p>\u9047\u5230\u8fd9\u4e2a\u95ee\u9898\u540e\uff0c\u53cd\u590d\u64ad\u62a5\u770b\u4e86\u4e0b\u8df3\u52a8\u5e45\u5ea6\uff0c\u76f8\u5f53\u7684\u5927\uff0c100ms\u5185\u4f4d\u7f6e\u53ef\u80fd\u5c31\u4f1a\u6f02\u79fb\u8d85\u8fc710\u7c73\uff0c\u65e2\u7136\u6f02\u79fb\u8fd9\u4e48\u6fc0\u70c8\uff0c\u5b8c\u5168\u53ef\u4ee5\u901a\u8fc7\u4e00\u4e2a\u7b80\u5355\u7684\u4f4d\u7f6e\u8df3\u52a8\u8fc7\u6ee4\u6765\u6ee4\u6389\u8fd9\u7c7b\u6570\u636e\uff0c\u4e8e\u662f\u4ed4\u7ec6\u7814\u8bfb\u4e86\u4e0bLIO-SAM\u7684\u6e90\u7801\uff0c\u6d89\u53ca\u7684\u51e0\u4e2a\u5173\u952e\u4f4d\u7f6e\u5982\u4e0b\uff1a<\/p>\n<h4>1. GPS\u4ecb\u5165\u9891\u7387\uff08<span style=\"font-size: 1rem;\">gpsFrequence\uff09<\/span><\/h4>\n<p>config\/params.yaml\u4e2d\uff0c\u5b58\u5728\u51e0\u4e2a\u4e0eGPS\u76f8\u5173\u7684\u914d\u7f6e\u3002gpsFrequence\u662f\u5176\u4e2d\u7684\u4e00\u4e2a\u53c2\u6570\uff0c\u9ed8\u8ba4\u4e3a10\uff081.0 \/ gpsFrequence\uff0c\u76f8\u5f53\u4e8e10hz\uff0c\u95f4\u9694100ms\uff09\uff0c\u5728mapOptmizationGps.cpp\u7684syncGPS\u51fd\u6570\u8c03\u7528\u65f6\u4f20\u5165\uff1a<\/p>\n<pre class=\"pure-highlightjs\"><code class=\"\">...\r\nvoid updateInitialGuess() {  # \u4f7f\u7528GPS\u521d\u59cb\u5316pose\u5b9a\u4f4d\r\nif (syncGPS(gpsQueue, alignedGPS, <span style=\"color: #ff0000;\">timeLaser<\/span>InfoCur, <span style=\"color: #ff0000;\">1.0 \/ gpsFrequence<\/span>)) {\r\n...\r\nvoid addGPSFactor() {  # \u5efa\u56fe\u8fc7\u7a0b\u4e2d\u6dfb\u52a0GPS\u56e0\u5b50\r\nif (syncGPS(gpsQueue, thisGPS, <span style=\"color: #ff0000;\">timeLaser<\/span>InfoCur, <span style=\"color: #ff0000;\">1.0 \/ gpsFrequence<\/span>)) {\r\n...\r\n<\/code><\/pre>\n<p>\u5f53\u524d\u5e27\u6fc0\u5149\u96f7\u8fbe\u6570\u636e\u65f6\u95f4+-100ms\uff0c\u8fc7\u65e9\u7684\u6570\u636e\u4e22\u5f03\uff0c\u8fc7\u665a\u7684\u6570\u636e\u6682\u4e0d\u4f7f\u7528\uff0c\u53ea\u4f7f\u7528\u6fc0\u5149\u96f7\u8fbe\u65f6\u95f4\u6233\u524d\u540e100ms\u7684\u6570\u636e\uff1a<\/p>\n<pre class=\"pure-highlightjs\"><code class=\"\">bool syncGPS(std::deque&lt;nav_msgs::Odometry&gt; &amp;gpsBuf,\r\n             nav_msgs::Odometry &amp;aligedGps, double timestamp,\r\n             double <span style=\"color: #ff0000;\">eps_cam<\/span>) {\r\n    bool hasGPS = false;\r\n    while (!gpsQueue.empty()) {\r\n        mtxGpsInfo.lock();\r\n        if (gpsQueue.front().header.stamp.toSec() &lt; <span style=\"color: #ff0000;\">timestamp - eps_cam<\/span>) {\r\n            \/\/ message too old\uff0c<span style=\"color: #ff0000;\">\u4e22\u5f03<\/span>\r\n            gpsQueue.pop_front();\r\n            mtxGpsInfo.unlock();\r\n        } else if (gpsQueue.front().header.stamp.toSec() &gt; <span style=\"color: #ff0000;\">timestamp + eps_cam<\/span>) {\r\n            \/\/ message too new\uff0c<span style=\"color: #ff0000;\">\u6682\u4e0d\u4f7f\u7528<\/span>\r\n            mtxGpsInfo.unlock();\r\n            break;\r\n        } else {\r\n            hasGPS = true;\r\n            <span style=\"color: #ff0000;\">aligedGps<\/span> = <span style=\"color: #008000;\">gpsQueue<\/span>.front();\r\n            gpsQueue.pop_front();\r\n            mtxGpsInfo.unlock();\r\n        }\r\n    }\r\n    return hasGPS;\r\n}<\/code><\/pre>\n<p>gpsQueue\u662f\u901a\u8fc7sub gps_odom topic\u83b7\u53d6\uff1a<\/p>\n<pre class=\"pure-highlightjs\"><code class=\"\">...\r\nsubGPS = nh.subscribe&lt;nav_msgs::Odometry&gt;( \"<span style=\"color: #ff0000;\">gps_odom<\/span>\", 200, &amp;mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());\r\n...\r\nstd::deque&lt;nav_msgs::Odometry&gt; gpsQueue;\r\nvoid gpsHandler(const nav_msgs::Odometry::ConstPtr &amp;gpsMsg) {\r\n    if (useGPS) {\r\n        mtxGpsInfo.lock();\r\n        <span style=\"color: #ff0000;\">gpsQueue.push_back<span style=\"color: #000000;\">(<\/span><\/span>*gpsMsg);\r\n        mtxGpsInfo.unlock();\r\n    }\r\n}<\/code><\/pre>\n<p>\u800cgps_odom\u662f\u7531simpleGpsOdom.cpp\u8ba2\u9605\/gps\/fix\u5e76\u8f6c\u6362\u540e\u53d1\u5e03\u7684\uff0c\u6240\u4ee5\u8fd9\u91cc\u53ef\u4ee5\u5148<span style=\"color: #ff0000;\">\u589e\u52a0\u4e00\u4e2agps\u589e\u5f3a\u4fe1\u53f7\u8d28\u91cf\u8fc7\u6ee4<\/span>\uff08\u53ea\u4f7f\u7528 <span style=\"color: #ff0000;\">status=1<\/span>:SBAS_FIX\uff0c<span style=\"color: #ff0000;\">\u661f\u57fa\u589e\u5f3a\u7cfb\u7edf\u4fee\u590d<\/span> \u7684RTK\u6570\u636e\uff09\uff1a<\/p>\n<pre class=\"pure-highlightjs\"><code class=\"\">...\r\n# gpsTopic \u5728params.yaml\u4e2d\u914d\u7f6e\r\ngpsSub = nh.subscribe(<span style=\"color: #ff0000;\">gpsTopic<\/span>, 1000, &amp;GNSSOdom::GNSSCB, this, ros::TransportHints().tcpNoDelay());\r\nleft_odom_pub = nh.advertise&lt;nav_msgs::Odometry&gt;(\"\/<span style=\"color: #ff0000;\">gps_odom<\/span>\", 100, false);\r\n...\r\nvoid GNSSCB(const sensor_msgs::NavSatFixConstPtr &amp;msg) {\r\n        ...\r\n        \/** if you have some satellite info or rtk status info, put it here*\/\r\n        int status = -1;\r\n        int satell_num = -1;\r\n        double x, y, z;\r\n        \/\/ LLA-&gt;ENU, better accuacy than gpsTools especially for z value\r\n        geo_converter.Forward(lla[0], lla[1], lla[2], x, y, z);\r\n        Eigen::Vector3d enu(x, y, z);\r\n        if (abs(enu.x()) &gt; 10000 || abs(enu.x()) &gt; 10000 || abs(enu.x()) &gt; 10000) {\r\n            \/** check your lla coordinate *\/\r\n            ROS_INFO(\"Error ogigin : %f, %f, %f\", enu(0), enu(1), enu(2));\r\n            return;\r\n        }\r\n\r\n        <span style=\"color: #ff0000;\"># \u8fc7\u6ee4\u4f4e\u8d28\u91cf\u6570\u636e  status\uff1a\u5f53\u72b6\u6001 &gt;= 0 STATUS_FIX\u65f6\uff0c\u6b64\u5b9a\u4f4d\u624d\u6709\u6548\uff1b-1 NO_FIX\uff0c0 FIX\uff08\u6807\u51c6GPS\u5b9a\u4f4d\uff09\uff0c1 SBAS_FIX\uff08\u661f\u57fa\u589e\u5f3a\u7cfb\u7edf\u4fee\u590d\uff09\uff0c2 GBAS_FIX\uff08\u5730\u57fa\u589e\u5f3a\u7cfb\u7edf\u4fee\u590d\uff09<\/span>\r\n        <span style=\"color: #ff0000;\">if (msg-&gt;status.status != 1) {  # \u53ea\u4f7f\u7528 1:SBAS_FIX\u661f\u57fa\u589e\u5f3a\u7cfb\u7edf\u4fee\u590d \u7684RTK\u6570\u636e\r\n            ROS_WARN(\"GPS poor signal! gps status: %d\", msg-&gt;status.status);<\/span>\r\n<span style=\"color: #ff0000;\">            return;<\/span>\r\n<span style=\"color: #ff0000;\">        }<\/span>\r\n\r\n        bool orientationReady = false;\r\n        double yaw = 0.0;\r\n        double distance =\r\n                sqrt(pow(enu(1) - prev_pose_left(1), 2) + pow(enu(0) - prev_pose_left(0), 2));\r\n        if (<span style=\"color: #ff0000;\">distance &gt; 0.1<\/span>) {\r\n            \/\/ \u8fd4\u56de\u503c\u662f\u6b64\u70b9\u4e0e\u8fdc\u70b9\u8fde\u7ebf\u4e0ex\u8f74\u6b63\u65b9\u5411\u7684\u5939\u89d2\r\n            yaw = atan2(enu(1) - prev_pose_left(1), enu(0) - prev_pose_left(0));\r\n            yaw_quat_left = tf::createQuaternionMsgFromYaw(yaw);\r\n            prev_pose_left = enu;\r\n            orientationReady = true;\r\n        }\r\n\r\n        \/** pub gps odometry*\/\r\n        nav_msgs::Odometry odom_msg;\r\n        odom_msg.header.stamp = msg-&gt;header.stamp;\r\n        odom_msg.header.frame_id = odometryFrame;\r\n        odom_msg.child_frame_id = \"gps\";\r\n\r\n        odom_msg.pose.pose.position.x = enu(0);\r\n        odom_msg.pose.pose.position.y = enu(1);\r\n        odom_msg.pose.pose.position.z = enu(2);\r\n        odom_msg.pose.covariance[0] = msg-&gt;position_covariance[0];\r\n        odom_msg.pose.covariance[7] = msg-&gt;position_covariance[4];\r\n        odom_msg.pose.covariance[14] = msg-&gt;position_covariance[8];\r\n        odom_msg.pose.covariance[1] = lla[0];\r\n        odom_msg.pose.covariance[2] = lla[1];\r\n        odom_msg.pose.covariance[3] = lla[2];\r\n        odom_msg.pose.covariance[4] = status;\r\n        odom_msg.pose.covariance[5] = satell_num;\r\n        odom_msg.pose.covariance[6] = orientationReady;\r\n        odom_msg.pose.pose.orientation = yaw_quat_left;\r\n        <span style=\"color: #ff0000;\">left_odom_pub.publish<\/span>(odom_msg);\r\n...<\/code><\/pre>\n<h4>2. GPS\u534f\u65b9\u5dee\u566a\u58f0\u9608\u503c\uff08<span style=\"font-size: 1rem;\">gpsCovThreshold\uff09<\/span><\/h4>\n<p>\u8fd9\u4e2a\u53c2\u6570\u4e5f\u5728config\/params.yaml\u4e2d\uff0c\u9ed8\u8ba4\u4e3a8\u3002\u5b83\u4e3b\u8981\u901a\u8fc7\u534f\u65b9\u5dee\u566a\u58f0\u8fdb\u884c\u8fc7\u6ee4\uff0c\u8fd9\u91cc\u53ef\u4ee5\u628a\u503c\u6253\u5370\u51fa\u6765\uff0c\u89c2\u5bdf\u4e0b\u5728\u591a\u8def\u5f84\u6f02\u79fb\u548c\u6b63\u5e38\u65f6\u7684\u566a\u58f0\uff0c\u7136\u540e\u914d\u7f6e\u4e00\u4e2a\u5408\u9002\u7684\u503c\u8fdb\u884c\u8fc7\u6ee4\u3002<\/p>\n<pre class=\"pure-highlightjs\"><code class=\"\">...\r\nvoid addGPSFactor() {\r\n...\r\n    if (syncGPS(gpsQueue, thisGPS, timeLaserInfoCur, 1.0 \/ gpsFrequence)) {\r\n        \/\/ GPS too noisy, skip\r\n        float noise_x = thisGPS.pose.covariance[0];\r\n        float noise_y = thisGPS.pose.covariance[7];\r\n        float noise_z = thisGPS.pose.covariance[14];\r\n\r\n        \/\/ make sure the gps data is stable encough\r\n        <span style=\"color: #ff0000;\">ROS_INFO(\"curr gps noise: %f, %f , %f\", noise_x, noise_y, noise_z);<\/span>\r\n        if (abs(noise_x) &gt; <span style=\"color: #ff0000;\">gpsCovThreshold<\/span> || abs(noise_y) &gt; <span style=\"color: #ff0000;\">gpsCovThreshold<\/span>) {\r\n            ROS_WARN(\"GPS noise too large: %d,%d &gt; %d\", abs(noise_x), abs(noise_y), gpsCovThreshold);\r\n            return;\r\n        }\r\n...<\/code><\/pre>\n<h4>3. \u8ddd\u79bb\u8df3\u52a8\u8fc7\u6ee4\uff08<span style=\"font-size: 1rem;\">GPSDISTANCE<\/span>\uff09<\/h4>\n<p>\u8fd9\u4e2a\u53c2\u6570\u4e5f\u5728config\/params.yaml\u4e2d\uff0c\u5bf9\u5e94\u53c2\u6570\u540d\u4e3agpsDistance\uff0c\u9ed8\u8ba4\u4e3a0.5\uff0c\u5355\u4f4d\u4e3a\u7c73\u3002\u5f53\u6700\u65b0\u6709\u6548gps\u4f4d\u7f6e\u4e0e\u4e0a\u4e00\u5e27\u6709\u6548gps\u4f4d\u7f6e\u4e4b\u95f4\u7684\u8ddd\u79bb\u5c0f\u4e8e0.5\u7c73\u65f6\uff0c\u4f1a\u4e22\u5f03\u6570\u636e\u3002\u8fd9\u91cc\u8bbe\u8ba1\u672c\u610f\u5e94\u8be5\u662f\u4e3a\u4e86\u964d\u4f4e\u9891\u7e41\u7684lidar-rtk\u4f4d\u7f6e\u5bf9\u9f50\uff0c\u53ef\u4ee5\u501f\u7528\u6539\u9020\u4e0b\uff0c\u589e\u52a0gps\u8df3\u52a8\u8fc7\u5927\u6570\u636e\u7684\u8fc7\u6ee4\u3002\u8fd9\u91cc\u7684\u8fc7\u6ee4\u9700\u8981\u8003\u8651\u65f6\u95f4\u95f4\u9694\u548c\u8f66\u901f\u56e0\u7d20\uff0c\u53ef\u4ee5\u4ee5\u201crtk\u8df3\u52a8\u8ddd\u79bb &gt; \u6700\u5927\u8f66\u901f*\u65f6\u95f4\u53ef\u80fd\u884c\u9a76\u7684\u6700\u8fdc\u8ddd\u79bb\u201d\u4e3a\u8fc7\u6ee4\u6761\u4ef6\uff0c\u8f66\u8f86\u4e0d\u53ef\u80fd\u5728\u90a3\u4e48\u77ed\u65f6\u95f4\u8dd1\u90a3\u4e48\u8fdc\uff0c\u5c31\u610f\u5473\u7740gps\u6570\u636e\u5df2\u7ecf\u5931\u771f\u4e86\u3002<\/p>\n<pre class=\"pure-highlightjs\"><code class=\"\">...\r\nvoid addGPSFactor() {\r\n      static float <span style=\"color: #ff0000;\">lastGPSTime<\/span> = 0;\r\n      if (syncGPS(gpsQueue, thisGPS, timeLaserInfoCur, 1.0 \/ gpsFrequence)) {\r\n            ...\r\n            \/\/ Add GPS every a few meters\r\n            PointType curGPSPoint;\r\n            curGPSPoint.x = gps_x;\r\n            curGPSPoint.y = gps_y;\r\n            curGPSPoint.z = gps_z;\r\n            <span style=\"color: #ff0000;\">float point_dist = pointDistance(curGPSPoint, lastGPSPoint);\r\n            float time_diff = thisGPS.header.stamp.toSec() - lastGPSTime; \/\/ yan 25.8.28 \u6700\u65b0\u6709\u6548gps\u4f4d\u7f6e\u4e0e\u4e0a\u4e00\u5e27\u6709\u6548gps\u4f4d\u7f6e\u4e4b\u95f4\u7684\u65f6\u95f4\u5dee\uff08\u79d2\uff09 \r\n            float vehicleMaxSecSpeed = 1.5; \/\/ yan 25.8.28 \u8f66\u8f86\u6700\u5927\u884c\u9a76\u901f\u5ea6\uff08\u7c73\/\u6bcf\u79d2\uff09\r\n<\/span>            time_diff = thisGPS.header.stamp.toSec() - <span style=\"color: #ff0000;\">lastGPSTime;<\/span>\r\n            if (point_dist &lt; <span style=\"color: #ff0000;\">GPSDISTANCE<\/span>) {\r\n                ROS_WARN(\"Skip addGPSFactor, Point Distance &lt; gpsDistance: %f &lt; %f\", point_dist, GPSDISTANCE);\r\n                return;\r\n            } else if (point_dist &gt; time_diff * vehicleMaxSecSpeed) { \/\/ yan 25.8.28 \u589e\u52a0gps\u8df3\u52a8\u8fc7\u5927\u7684\u8fc7\u6ee4\uff0c\u9700\u8981\u8003\u8651\u65f6\u95f4\u95f4\u9694\u548c\u8f66\u901f\u56e0\u7d20\r\n                ROS_WARN(\"Skip addGPSFactor, Point Distance &gt; time_diff * vehicleMaxSecSpeed: %f &gt; %f * %f\", point_dist, time_diff, vehicleMaxSecSpeed);\r\n                return;\r\n            } else {\r\n                lastGPSPoint = curGPSPoint;\r\n                <span style=\"color: #ff0000;\">lastGPSTime = thisGPS.header.stamp.toSec();<\/span>\r\n            }\r\n...<\/code><\/pre>\n<h1>\u4e09\u3001\u603b\u7ed3<\/h1>\n<p>\u4e0a\u8f66\u9a8c\u8bc1\uff0c\u5728\u7a7f\u884c\u5305\u542b\u9ad8\u8038\u5efa\u7b51\uff08\u591a\u8def\u5f84\u5e72\u6270\uff09\u3001\u72ed\u7a84\u9053\u8def\u3001\u5bbd\u9614\u5e7f\u573a\uff08\u7279\u5f81\u5c11\uff09\u3001\u6811\u6728\u679d\u53f6\u5bc6\u96c6\uff08GPS\u4fe1\u53f7\u5dee\uff09\u3001\u4e0a\u65b9\u5efa\u7b51\u906e\u6321\uff08\u5b8c\u5168\u65e0GPS\u4fe1\u53f7\uff09\u73af\u5883\u4e2d\u884c\u9a763km\uff0c\u65e0\u6f02\u79fb\u73b0\u8c61\uff0c\u5728\u56de\u73af\u540e\uff0c\u6784\u5efa\u51fa\u7684\u5730\u56fe\u4e0egoogle\u536b\u661f\u5730\u56fe\u53ef\u4ee5\u5b8c\u5168\u91cd\u5408\u5339\u914d\uff0c\u8bf4\u660e\u6b64\u89e3\u51b3\u65b9\u6cd5\u6709\u6548\u3002<\/p>\n<p><a href=\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/map-m.jpg\"><img loading=\"lazy\" class=\"alignnone wp-image-10544 size-full\" src=\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/map-m.jpg\" alt=\"\" width=\"2024\" height=\"617\" srcset=\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/map-m.jpg 2024w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/map-m-300x91.jpg 300w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/map-m-1024x312.jpg 1024w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/map-m-768x234.jpg 768w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/map-m-1536x468.jpg 1536w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/map-m-624x190.jpg 624w\" sizes=\"(max-width: 2024px) 100vw, 2024px\" \/><\/a><\/p>\n<p>\u5b8c\u6574\u5730\u56fe\uff1a<\/p>\n<p><a href=\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/xiangyansi-map.jpg\"><img loading=\"lazy\" class=\"alignnone size-large wp-image-10550\" src=\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/xiangyansi-map-1024x534.jpg\" alt=\"\" width=\"625\" height=\"326\" srcset=\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/xiangyansi-map-1024x534.jpg 1024w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/xiangyansi-map-300x157.jpg 300w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/xiangyansi-map-768x401.jpg 768w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/xiangyansi-map-624x326.jpg 624w, https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2025\/08\/xiangyansi-map.jpg 1200w\" sizes=\"(max-width: 625px) 100vw, 625px\" \/><\/a><\/p>\n<p>\u5176\u4ed6\u5efa\u56fe\u9700\u8981\u6ce8\u610f\u7684\u70b9\uff1a<\/p>\n<ul>\n<li>\u5148\u5c0f\u56de\u73af\uff0c\u540e\u5927\u95ed\u73af\uff1a\u5c3d\u91cf\u907f\u514d\u76f4\u63a5\u5c1d\u8bd5\u8f83\u5927\u7684\u73af\u8def\u95ed\u5408\uff0c\u82e5\u7d2f\u8ba1\u8bef\u5dee\u8fc7\u5927\u5c06\u76f4\u63a5\u5bfc\u81f4\u95ed\u73af\u5931\u8d25\uff1b<\/li>\n<li>\u5148\u95ed\u73af\uff0c\u518d\u8865\u7ec6\u8282\uff1a\u5148\u786e\u4fdd\u56de\u73af\u5b8c\u6210\uff0c\u9501\u5b9a\u8f6e\u5ed3\uff0c\u907f\u514d\u5728\u56de\u73af\u524d\u7ed5\u7ec6\u8282\u5bfc\u81f4\u7d2f\u79ef\u8bef\u5dee\u8fc7\u5927\u65e0\u6cd5\u56de\u73af\uff1b\u5f53\u539f\u8ba1\u5212\u7684\u95ed\u73af\u70b9\u65e0\u6cd5\u95ed\u5408\uff0c\u5e94\u7ee7\u7eed\u884c\u8d70\u91cd\u5408\u8def\u5f84\uff0c\u5c1d\u8bd5\u95ed\u73af\uff0c\u907f\u514d\u5728\u95ed\u73af\u524d\u6539\u53d8\u8def\u5f84\uff0c\u5c1d\u8bd5\u65b0\u7684\u533a\u57df\uff0c\u8fd9\u6709\u53ef\u80fd\u5bfc\u81f4\u6c38\u4e45\u65e0\u6cd5\u95ed\u5408\u3002<\/li>\n<li>\u9009\u62e9\u7279\u5f81\u4e30\u5bcc\u7684\u70b9\u4f5c\u4e3a\u95ed\u73af\u70b9\uff1a\u7a7a\u65f7\u573a\u5730\u3001\u957f\u76f4\u8d70\u5eca\u7b49\u7279\u5f81\u4e0d\u8db3\u573a\u666f\uff0c\u5c3d\u91cf\u8865\u5145\u4e00\u4e9b\u7279\u5f81\uff0c\u6bd4\u5982\u589e\u52a0\u6728\u7bb1\u3001\u56fa\u5b9a\u7269\u7b49\uff0c\u589e\u52a0\u7279\u5f81\u3002<\/li>\n<\/ul>\n<p>&nbsp;<\/p>\n<p>yan 25.8.31 23:15<\/p>\n","protected":false},"excerpt":{"rendered":"<p>\u6700\u8fd1\u5728\u4e0d\u540c\u73af\u5883\u4e0b\u8fdb\u884clio-sam\u5efa\u56fe\u65f6\uff0c\u9047\u5230\u4e00\u4e2a\u95ee\u9898\uff1a\u5728\u901a\u8fc7\u72ed\u7a84\u9053\u8def\u65f6\uff0c\u4e24\u4fa7\u9ad8\u8038\u7684\u5efa\u7b51\u53cd\u5c04\u536b\u661f\u4fe1\u53f7\uff0c\u5bfc\u81f4\u51fa\u73b0 [&hellip;]<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"closed","sticky":false,"template":"","format":"standard","meta":[],"categories":[1409,1403],"tags":[1199,1451,975],"_links":{"self":[{"href":"https:\/\/yanjingang.com\/blog\/index.php?rest_route=\/wp\/v2\/posts\/10476"}],"collection":[{"href":"https:\/\/yanjingang.com\/blog\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/yanjingang.com\/blog\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/yanjingang.com\/blog\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/yanjingang.com\/blog\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=10476"}],"version-history":[{"count":4,"href":"https:\/\/yanjingang.com\/blog\/index.php?rest_route=\/wp\/v2\/posts\/10476\/revisions"}],"predecessor-version":[{"id":10784,"href":"https:\/\/yanjingang.com\/blog\/index.php?rest_route=\/wp\/v2\/posts\/10476\/revisions\/10784"}],"wp:attachment":[{"href":"https:\/\/yanjingang.com\/blog\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=10476"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/yanjingang.com\/blog\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=10476"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/yanjingang.com\/blog\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=10476"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}