{"id":87191,"date":"2026-07-30T00:09:03","date_gmt":"2026-07-29T16:09:03","guid":{"rendered":"https:\/\/www.wsisp.com\/helps\/87191.html"},"modified":"2026-07-30T00:09:03","modified_gmt":"2026-07-29T16:09:03","slug":"%e3%80%90navigation2%e8%bf%9b%e9%98%b6%e3%80%91%e5%8d%81%e4%b8%89%ef%bc%89%ef%bc%9a%e6%89%8b%e5%86%99%e6%a6%82%e7%8e%87%e5%9c%b0%e5%9b%be%e4%bb%8elog-odds%e6%8e%a8%e5%af%bc%e5%88%b0mi%e4%ba%92","status":"publish","type":"post","link":"https:\/\/www.wsisp.com\/helps\/87191.html","title":{"rendered":"\u3010Navigation2\u8fdb\u9636\u3011(\u5341\u4e09\uff09\uff1a\u624b\u5199\u6982\u7387\u5730\u56fe!\u4ecelog-odds\u63a8\u5bfc\u5230MI\u4e92\u4fe1\u606f\u63a2\u7d22\u4e0eNav2\u63d2\u4ef6\u5b9e\u73b0"},"content":{"rendered":"<h3>\u524d\u8a00<\/h3>\n<ul>\n<li>\u4e0a\u4e00\u671f\u6211\u4eec\u628a RIGGoalSelector \u5f15\u5165\u4e86\u63a2\u7d22\u7cfb\u7edf\u2014\u2014\u901a\u8fc7 RRT \u504f\u5411\u9ad8\u4fe1\u606f\u524d\u6cbf\u6765\u8bc4\u4f30\u8def\u5f84\u4ee3\u4ef7&#xff0c;\u6bd4\u5355\u7eaf\u7684 Nearest \u548c Utility \u5df2\u7ecf\u6709\u4e86\u660e\u663e\u63d0\u5347<\/li>\n<li>\u5f80\u671f\u5185\u5bb9&#xff1a;\n<ul>\n<li>\u7b2c\u4e00\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u4e00) \u6846\u67b6\u603b\u89c8\u548c\u6982\u5ff5\u89e3\u91ca<\/li>\n<li>\u7b2c\u4e8c\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u4e8c) &#xff1a;ROS2gazebo\u963f\u514b\u66fc\u5c0f\u8f66\u6a21\u578b\u642d\u5efa-gazebo_ackermann_drive\u7b49\u63d2\u4ef6\u7684\u914d\u7f6e\u548c\u8bf4\u660e<\/li>\n<li>\u7b2c\u4e09\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u4e09) &#xff1a;Cartographer\u5efa\u56fe\u7b97\u6cd5\u914d\u7f6e&#xff1a;\u4ece\u4eff\u771f\u5230\u5b9e\u8f66&#xff0c;\u4ece\u539f\u7406\u5230\u5b9e\u73b0<\/li>\n<li>\u7b2c\u56db\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u56db) &#xff1a;ORB-SLAM3\u7684ROS2 humble\u7f16\u8bd1\u548c\u914d\u7f6e<\/li>\n<li>\u7b2c\u4e94\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u4e94) &#xff1a;\u57fa\u4e8egazebo\u4eff\u771f\u7684\u590d\u6742\u5730\u5f62\u7684ORB-SLAM3\u914d\u7f6e<\/li>\n<li>\u7b2c\u516d\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u516d) &#xff1a;Navigation2\u57fa\u7840\u914d\u7f6e\u4e0e\u53c2\u6570\u89e3\u6790<\/li>\n<li>\u7b2c\u4e03\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u4e03) &#xff1a;Hybrid-A*\u5168\u5c40\u89c4\u5212\u5668\u7684\u539f\u7406\u63a8\u5bfc\u4e0eNav2\u63d2\u4ef6\u5b9e\u73b0<\/li>\n<li>\u7b2c\u516b\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u516b&#xff09;&#xff1a;RRT\u4e0eRRT*\u91c7\u6837\u89c4\u5212\u5668\u7684\u539f\u7406\u63a8\u5bfc\u4e0eNav2\u63d2\u4ef6\u5b9e\u73b0<\/li>\n<li>\u7b2c\u4e5d\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u4e5d&#xff09;&#xff1a;LQR\u6700\u4f18\u63a7\u5236\u5668\u7684\u539f\u7406\u63a8\u5bfc\u4e0eNav2\u63d2\u4ef6\u5b9e\u73b0<\/li>\n<li>\u7b2c\u5341\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u5341&#xff09;&#xff1a;MPC\u6a21\u578b\u9884\u6d4b\u63a7\u5236\u5668\u7684\u539f\u7406\u63a8\u5bfc\u4e0eNav2\u63d2\u4ef6\u5b9e\u73b0<\/li>\n<li>\u7b2c\u5341\u4e00\u671f&#xff1a;\u3010Navigation2\u8fdb\u9636\u3011(\u5341\u4e00&#xff09;&#xff1a;\u81ea\u4e3b\u63a2\u7d22\u7684\u8d77\u70b9\u2014\u2014Nearest \u4e0e Utility Frontier \u7b97\u6cd5\u63a8\u5bfc\u4e0e Nav2 \u63d2\u4ef6\u5b9e\u73b0<\/li>\n<li>\u7b2c\u5341\u4e8c\u671f&#xff1a;\u3010Navigation2\u8fdb\u9636\u3011(\u5341\u4e8c&#xff09;&#xff1a;\u81ea\u4e3b\u63a2\u7d22\u6027\u80fd\u4f18\u5316\u4e0e RIG \u7b97\u6cd5\u63a8\u5bfc\u4e0e Nav2 \u63d2\u4ef6\u5b9e\u73b0<\/li>\n<\/ul>\n<\/li>\n<li>\u4f46\u5f80\u671f\u4e09\u79cd\u63a2\u7d22\u7b56\u7565\u6709\u4e00\u4e2a\u5171\u540c\u7f3a\u9677&#xff1a;&#034;\u4fe1\u606f\u589e\u76ca&#034;\u7684\u8ba1\u7b97\u672c\u8d28\u4e0a\u53ea\u662f\u5728\u6570 UNKNOWN \u90bb\u5c45\u683c\u5b50\u2014\u2014\u5728 OccupancyGrid \u7684\u4e09\u503c&#xff08;-1 \/ 0 \/ 100&#xff09;\u5730\u56fe\u4e0a&#xff0c;\u8fd9\u5df2\u7ecf\u662f\u80fd\u505a\u5230\u7684\u6700\u597d\u4e86<\/li>\n<li>\u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;\u6211\u4eec\u7684\u5730\u56fe\u53ea\u77e5\u9053&#034;\u8fd9\u91cc\u6709\u6ca1\u6709\u4e1c\u897f&#034;&#xff0c;\u4e0d\u77e5\u9053&#034;\u8fd9\u91cc\u6709\u591a\u4e0d\u786e\u5b9a&#034;&#xff0c;\u6240\u4ee5&#034;\u4fe1\u606f\u589e\u76ca&#034;\u53ea\u80fd\u8fd1\u4f3c\u5730\u8ba4\u4e3a\u2014\u2014\u524d\u6cbf\u6328\u7740\u7684 UNKNOWN \u683c\u5b50\u8d8a\u591a&#xff0c;\u53bb\u90a3\u91cc\u63a2\u7d22&#034;\u5e94\u8be5&#034;\u8d8a\u503c<\/li>\n<li>\u672c\u671f\u6211\u4eec\u5c31\u6765\u89e3\u51b3\u8fd9\u4e2a\u95ee\u9898&#xff1a;\n<ul>\n<li>\u5148\u624b\u5199\u4e00\u4e2a\u6982\u7387\u5730\u56fe\u53d1\u5e03\u8282\u70b9 prob_map&#xff0c;\u7528 log-odds \u9012\u63a8 &#043; \u9006\u4f20\u611f\u5668\u6a21\u578b \u7ef4\u62a4\u6bcf cell \u7684\u8fde\u7eed\u5360\u636e\u6982\u7387 P \u2208 [0, 1]<\/li>\n<li>\u518d\u5b9e\u73b0 MI&#xff08;Mutual Information&#xff0c;\u4e92\u4fe1\u606f&#xff09;\u76ee\u6807\u9009\u62e9\u5668&#xff0c;\u4ece\u5019\u9009\u524d\u6cbf\u8d28\u5fc3\u53d1\u5c04\u865a\u62df\u5149\u675f&#xff0c;\u7d2f\u52a0\u53ef\u89c1 cell \u7684\u9999\u519c\u71b5\u4f5c\u4e3a\u771f\u6b63\u7684\u4fe1\u606f\u589e\u76ca<\/li>\n<li>\u6700\u7ec8\u5728\u4eff\u771f\u4e2d\u5b8c\u6210 MI \u63a2\u7d22\u7684\u9a8c\u8bc1 <img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160844-6a6a258cef735.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<hr \/>\n<h4>\u6587\u7ae0\u76ee\u5f55<\/h4>\n<ul>\n<li>\n<ul>\n<li>\u524d\u8a00<\/li>\n<li>1 \u95ee\u9898&#xff1a;\u4e09\u503c\u5730\u56fe\u4e3a\u4ec0\u4e48\u4e0d\u591f<\/li>\n<li>\n<ul>\n<li>\n<ul>\n<li>1-1 \u5f80\u671f\u63a2\u7d22\u7b56\u7565\u7684\u4fe1\u606f\u589e\u76ca\u56de\u987e<\/li>\n<li>1-2 \u7f3a\u5931\u7684\u5173\u952e\u7ef4\u5ea6<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<\/li>\n<li>2 \u6982\u7387\u5360\u636e\u7f51\u683c<\/li>\n<li>\n<ul>\n<li>\n<ul>\n<li>2-1 \u8d1d\u53f6\u65af\u9012\u63a8\u4e0e log-odds \u66f4\u65b0<\/li>\n<li>\n<ul>\n<li>2-1-1 log-odds \u5230\u5e95\u662f\u4ec0\u4e48<\/li>\n<li>2-1-2 \u8d1d\u53f6\u65af\u66f4\u65b0\u5728 log-odds \u7a7a\u95f4 &#061; \u52a0\u6cd5<\/li>\n<\/ul>\n<\/li>\n<li>2-2 \u9006\u4f20\u611f\u5668\u6a21\u578b&#xff08;Inverse Sensor Model&#xff09;<\/li>\n<li>2-3 Bresenham \u5149\u7ebf\u8ffd\u8e2a<\/li>\n<li>2-4 \u4ee3\u7801\u5b9e\u73b0&#xff1a;ProbabilityMapper \u5b8c\u6574\u7c7b<\/li>\n<li>2-5 \u53cc Topic \u53d1\u5e03&#xff1a;\u4e09\u503c\u5730\u56fe &#043; \u6982\u7387\u5730\u56fe<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<\/li>\n<li>3 \u57fa\u4e8e\u4e92\u4fe1\u606f\u7684\u524d\u6cbf\u9009\u62e9<\/li>\n<li>\n<ul>\n<li>\n<ul>\n<li>3-1 \u9999\u519c\u71b5<\/li>\n<li>3-2 \u4e92\u4fe1\u606f &#061; \u9884\u671f\u71b5\u51cf<\/li>\n<li>3-3 \u865a\u62df\u5149\u675f\u6a21\u62df MI<\/li>\n<li>3-4 \u4ee3\u7801\u5b9e\u73b0&#xff1a;MIGoalSelector<\/li>\n<li>3-5 \u6548\u7528\u51fd\u6570\u4e0e\u8def\u5f84\u4ee3\u4ef7<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<\/li>\n<li>4 \u7cfb\u7edf\u96c6\u6210<\/li>\n<li>\n<ul>\n<li>\n<ul>\n<li>4-1 \u9879\u76ee\u7ed3\u6784<\/li>\n<li>4-2 \u63a2\u7d22\u8282\u70b9\u7684\u4fee\u6539<\/li>\n<li>4-3 \u542f\u52a8\u6d41\u7a0b<\/li>\n<li>4-4 RViz \u53ef\u89c6\u5316<\/li>\n<li>4-5 \u5b8c\u6574\u6e90\u7801\u6e05\u5355<\/li>\n<li>\n<ul>\n<li>probability_mapper.hpp<\/li>\n<li>probability_mapper.cpp<\/li>\n<li>mapper_main.cpp<\/li>\n<li>prob_mapper.launch.py<\/li>\n<li>slam_prob_mapper.launch.py<\/li>\n<li>mi_goal_selector.hpp<\/li>\n<li>mi_goal_selector.cpp<\/li>\n<li>exploration_node.hpp&#xff08;\u5b8c\u6574\u6587\u4ef6&#xff0c;\u542b MI \u6269\u5c55&#xff09;<\/li>\n<li>exploration_node.cpp&#xff08;\u5b8c\u6574\u6587\u4ef6&#xff0c;\u542b MI \u6269\u5c55&#xff09;<\/li>\n<li>5_prob_map.sh<\/li>\n<li>7_mi_explore.sh<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<\/li>\n<li>5 \u5b8c\u6574\u53c2\u6570\u4e0e\u5bf9\u6bd4<\/li>\n<li>\n<ul>\n<li>\n<ul>\n<li>5-1 prob_mapper \u53c2\u6570\u8868<\/li>\n<li>5-2 exploration_node MI \u76f8\u5173\u53c2\u6570<\/li>\n<li>5-3 \u56db\u79cd\u63a2\u7d22\u7b56\u7565\u5bf9\u6bd4<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<\/li>\n<li>6 Python \u53ef\u89c6\u5316\u4ee3\u7801<\/li>\n<li>\n<ul>\n<li>\n<ul>\n<li>6-1 \u4e09\u503c vs \u6982\u7387 vs \u71b5 \u5bf9\u6bd4\u56fe&#xff08;1-2 \u8282&#xff09;<\/li>\n<li>6-2 log-odds \u51fd\u6570\u66f2\u7ebf&#xff08;2-1-1 \u8282&#xff09;<\/li>\n<li>6-3 MI \u865a\u62df\u5149\u675f\u6a21\u62df&#xff08;3-2 \/ 3-3 \u8282&#xff09;<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<\/li>\n<li>\u603b\u7ed3<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<hr \/>\n<h3>1 \u95ee\u9898&#xff1a;\u4e09\u503c\u5730\u56fe\u4e3a\u4ec0\u4e48\u4e0d\u591f<\/h3>\n<h5>1-1 \u5f80\u671f\u63a2\u7d22\u7b56\u7565\u7684\u4fe1\u606f\u589e\u76ca\u56de\u987e<\/h5>\n<ul>\n<li>\u5f80\u671f\u4e09\u79cd\u63a2\u7d22\u7b56\u7565\u7684 computeInfoGain() \u5b9e\u73b0\u672c\u8d28\u4e0a\u90fd\u662f\u540c\u4e00\u4e2a\u51fd\u6570\u2014\u2014\u6570\u67d0\u4e2a\u524d\u6cbf\u805a\u7c7b\u5468\u56f4\u6709\u591a\u5c11\u4e2a UNKNOWN \u90bb\u5c45&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ UtilityGoalSelector \u548c RIGGoalSelector \u7684 computeInfoGain()<\/span><br \/>\n<span class=\"token keyword\">double<\/span> <span class=\"token function\">computeInfoGain<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> FrontierCluster <span class=\"token operator\">&amp;<\/span> cluster<span class=\"token punctuation\">,<\/span><br \/>\n                       <span class=\"token keyword\">const<\/span> nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid <span class=\"token operator\">&amp;<\/span> map<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> unknown_count <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> <span class=\"token keyword\">auto<\/span> <span class=\"token operator\">&amp;<\/span> cell <span class=\"token operator\">:<\/span> cluster<span class=\"token punctuation\">.<\/span>cells<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> d <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> d <span class=\"token operator\">&lt;<\/span> <span class=\"token number\">4<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>d<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      <span class=\"token keyword\">int<\/span> nx <span class=\"token operator\">&#061;<\/span> cell<span class=\"token punctuation\">.<\/span>first <span class=\"token operator\">&#043;<\/span> dx<span class=\"token punctuation\">[<\/span>d<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">int<\/span> ny <span class=\"token operator\">&#061;<\/span> cell<span class=\"token punctuation\">.<\/span>second <span class=\"token operator\">&#043;<\/span> dy<span class=\"token punctuation\">[<\/span>d<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>map<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">[<\/span>ny <span class=\"token operator\">*<\/span> w <span class=\"token operator\">&#043;<\/span> nx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;&#061;<\/span> UNKNOWN<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#043;&#043;<\/span>unknown_count<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>unknown_count<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>\u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;\u524d\u6cbf\u7684 4 \u90bb\u57df\u91cc\u6709\u591a\u5c11\u4e2a -1&#xff0c;\u8fd9\u4e2a\u524d\u6cbf\u7684&#034;\u4fe1\u606f\u91cf&#034;\u5c31\u7ea6\u7b49\u4e8e\u8fd9\u4e2a\u6570<\/li>\n<li>\u8fd9\u4e2a\u65b9\u6cd5\u6709\u4e24\u4e2a\u81f4\u547d\u95ee\u9898&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u95ee\u9898\u63cf\u8ff0<\/tr>\n<tbody>\n<tr>\n<td>\u6bcf\u4e2a UNKNOWN \u683c\u5b50\u7684\u8d21\u732e\u5b8c\u5168\u4e00\u6837<\/td>\n<td>\u4e00\u4e2a\u521a\u5f15\u5165\u4f46\u5df2\u7ecf\u6709 3 \u675f\u6fc0\u5149\u626b\u8fc7\u7684 UNKNOWN \u683c\u5b50\u548c\u4e00\u4e2a\u4ece\u6765\u6ca1\u88ab\u89c2\u5bdf\u8fc7\u7684\u6df1\u5c42\u672a\u77e5\u683c\u5b50&#xff0c;\u4ef7\u503c\u76f8\u540c<\/td>\n<\/tr>\n<tr>\n<td>\u65e0\u6cd5\u6a21\u62df\u906e\u6321<\/td>\n<td>\u4e00\u4e2a\u88ab\u969c\u788d\u7269\u6321\u5728\u540e\u9762\u7684 UNKNOWN \u533a\u57df&#xff0c;\u673a\u5668\u4eba\u5b9e\u9645\u4e0a\u770b\u4e0d\u5230&#xff0c;\u4f46\u7b97\u6cd5\u4ecd\u7136\u7ed9\u5b83&#034;\u5168\u989d&#034;\u4fe1\u606f\u589e\u76ca<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<h5>1-2 \u7f3a\u5931\u7684\u5173\u952e\u7ef4\u5ea6<\/h5>\n<ul>\n<li>\u6838\u5fc3\u7f3a\u5931\u7684\u662f \u4e0d\u786e\u5b9a\u6027\u91cf\u5316\u3002OccupancyGrid \u7684 -1 \/ 0 \/ 100 \u662f&#034;\u5b9a\u6027&#034;\u7684\u5730\u56fe&#xff0c;\u5b83\u544a\u8bc9\u673a\u5668\u4eba\u5728\u54ea\u91cc\u6709\u4ec0\u4e48\u4e1c\u897f&#xff0c;\u4f46\u4e0d\u544a\u8bc9\u673a\u5668\u4eba&#034;\u6211\u6709\u591a\u786e\u5b9a&#034;<\/li>\n<li>\u5982\u679c\u6211\u4eec\u80fd\u7ef4\u62a4\u6bcf cell \u7684 \u8fde\u7eed\u5360\u636e\u6982\u7387 P \u2208 [0, 1]&#xff0c;\u6211\u4eec\u5c31\u80fd\u5b9a\u4e49 \u9999\u519c\u71b5&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         H<\/p>\n<p>         (<\/p>\n<p>         c<\/p>\n<p>         e<\/p>\n<p>         l<\/p>\n<p>         l<\/p>\n<p>         )<\/p>\n<p>         &#061;<\/p>\n<p>         \u2212<\/p>\n<p>         P<\/p>\n<p>           log<\/p>\n<p>           \u2061<\/p>\n<p>          2<\/p>\n<p>         P<\/p>\n<p>         \u2212<\/p>\n<p>         (<\/p>\n<p>         1<\/p>\n<p>         \u2212<\/p>\n<p>         P<\/p>\n<p>         )<\/p>\n<p>           log<\/p>\n<p>           \u2061<\/p>\n<p>          2<\/p>\n<p>         (<\/p>\n<p>         1<\/p>\n<p>         \u2212<\/p>\n<p>         P<\/p>\n<p>         )<\/p>\n<p>        H(cell) &#061; -P\\\\log_2 P &#8211; (1-P)\\\\log_2(1-P)<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0813em\">H<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">ce<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.9386em;vertical-align: -0.2441em\"><\/span><span class=\"mord\">\u2212<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mop\"><span class=\"mop\">lo<span style=\"margin-right: 0.0139em\">g<\/span><\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.207em\"><span class=\"\" style=\"top: -2.4559em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\">2<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2441em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mopen\">(<\/span><span class=\"mord\">1<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mop\"><span class=\"mop\">lo<span style=\"margin-right: 0.0139em\">g<\/span><\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.207em\"><span class=\"\" style=\"top: -2.4559em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\">2<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2441em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mopen\">(<\/span><span class=\"mord\">1<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\u4e3e\u4e2a\u4f8b\u5b50\u4f60\u5c31\u61c2\u4e86&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>P(m)\u542b\u4e49\u71b5 H\u89e3\u91ca<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">0.00<\/td>\n<td align=\"left\">\u786e\u5b9a\u7a7a\u95f2<\/td>\n<td align=\"left\">0.00<\/td>\n<td align=\"left\">\u5b8c\u5168\u786e\u5b9a&#xff0c;\u96f6\u4e0d\u786e\u5b9a\u6027<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">0.25<\/td>\n<td align=\"left\">\u8f83\u53ef\u80fd\u7a7a\u95f2<\/td>\n<td align=\"left\">0.81<\/td>\n<td align=\"left\">\u6709\u4e00\u5b9a\u4e0d\u786e\u5b9a\u6027<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">0.50<\/td>\n<td align=\"left\">\u5b8c\u5168\u672a\u77e5<\/td>\n<td align=\"left\">1.00<\/td>\n<td align=\"left\">\u6700\u5927\u4e0d\u786e\u5b9a\u6027<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">0.75<\/td>\n<td align=\"left\">\u8f83\u53ef\u80fd\u5360\u636e<\/td>\n<td align=\"left\">0.81<\/td>\n<td align=\"left\">\u6709\u4e00\u5b9a\u4e0d\u786e\u5b9a\u6027<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">1.00<\/td>\n<td align=\"left\">\u786e\u5b9a\u5360\u636e<\/td>\n<td align=\"left\">0.00<\/td>\n<td align=\"left\">\u5b8c\u5168\u786e\u5b9a&#xff0c;\u96f6\u4e0d\u786e\u5b9a\u6027<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li>\u6709\u4e86\u71b5&#xff0c;\u201c\u4fe1\u606f\u589e\u76ca&#034;\u5c31\u4e0d\u518d\u662f&#034;\u6570\u683c\u5b50\u201d&#xff0c;\u800c\u662f \u201c\u6d88\u9664\u591a\u5c11\u4e0d\u786e\u5b9a\u6027\u201d\u3002\u8fd9\u6b63\u662f MI \u63a2\u7d22\u7684\u6570\u5b66\u57fa\u7840<\/li>\n<li>\u4e09\u503c\u5730\u56fe\u548c\u6982\u7387\u5730\u56fe\u7684\u76f4\u89c2\u5bf9\u6bd4\u5982\u4e0b\u56fe\u6240\u793a\u2014\u2014\u540c\u4e00\u4e2a\u573a\u666f&#xff0c;\u4e09\u79cd\u89c6\u89d2&#xff1a;<\/li>\n<\/ul>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160845-6a6a258db090e.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<ul>\n<li>\u4ece\u8fd9\u5f20\u5bf9\u6bd4\u56fe\u53ef\u4ee5\u6e05\u695a\u770b\u5230&#xff1a;\n<ul>\n<li>\u5de6\u56fe&#xff08;\u4e09\u503c\u5730\u56fe&#xff09;&#xff1a;\u53ea\u6709\u4e09\u79cd\u989c\u8272\u2014\u2014\u767d\u8272&#xff08;FREE&#xff09;\u3001\u9ed1\u8272&#xff08;OCCUPIED&#xff09;\u3001\u7070\u8272&#xff08;UNKNOWN&#xff09;\u3002\u524d\u6cbf\u8fb9\u754c\u662f\u786c\u8fb9\u7f18&#xff0c;\u4e00\u683c\u4e4b\u5185\u4ece&#034;\u5df2\u77e5&#034;\u8df3\u5230&#034;\u672a\u77e5&#034;&#xff0c;\u6ca1\u6709\u4efb\u4f55\u8fc7\u6e21<\/li>\n<li>\u4e2d\u56fe&#xff08;\u6982\u7387\u5730\u56fe&#xff09;&#xff1a;\u7070\u5ea6\u8fde\u7eed\u6e10\u53d8\u3002\u5df2\u626b\u63cf\u533a\u57df\u5448\u6d45\u7070\u8272&#xff08;\u6982\u7387\u8d8b\u8fd1 0&#xff09;&#xff0c;\u969c\u788d\u7269\u5448\u6df1\u9ed1\u8272&#xff08;\u6982\u7387\u8d8b\u8fd1 1&#xff09;&#xff0c;\u524d\u6cbf\u9644\u8fd1\u4fdd\u7559 5~10 \u683c\u5bbd\u7684\u7070\u8272\u8fc7\u6e21\u5e26<\/li>\n<li>\u53f3\u56fe&#xff08;\u71b5\u5730\u56fe&#xff09;&#xff1a;\u4eae\u8272 &#061; \u9ad8\u71b5 &#061; \u4e0d\u786e\u5b9a\u6027\u5927\u3002\u5df2\u63a2\u7d22\u533a\u57df\u548c\u969c\u788d\u7269\u5185\u90e8\u90fd\u662f\u6697\u7684&#xff08;\u5df2\u7ecf\u786e\u5b9a\u4e86&#xff09;&#xff0c;\u53ea\u6709\u524d\u6cbf\u8fb9\u7f18\u5448\u660e\u4eae\u73af\u5e26\u2014\u2014\u8fd9\u5c31\u662f MI \u7b97\u6cd5\u8981\u53bb&#034;\u6d88\u9664&#034;\u7684\u5730\u65b9<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<hr \/>\n<h3>2 \u6982\u7387\u5360\u636e\u7f51\u683c<\/h3>\n<h5>2-1 \u8d1d\u53f6\u65af\u9012\u63a8\u4e0e log-odds \u66f4\u65b0<\/h5>\n<h6>2-1-1 log-odds \u5230\u5e95\u662f\u4ec0\u4e48<\/h6>\n<ul>\n<li>log-odds \u7684\u4e2d\u6587\u53eb \u201c\u5bf9\u6570\u51e0\u7387\u201d&#xff0c;\u5b83\u628a [0, 1] \u7684\u6982\u7387\u6620\u5230 (-\u221e, &#043;\u221e) \u7684\u5b9e\u6570\u8f74&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>p \u503clog-odds l\u542b\u4e49<\/tr>\n<tbody>\n<tr>\n<td align=\"center\">0.01<\/td>\n<td align=\"center\">-4.60<\/td>\n<td>\u51e0\u4e4e\u786e\u5b9a\u7a7a\u95f2<\/td>\n<\/tr>\n<tr>\n<td align=\"center\">0.25<\/td>\n<td align=\"center\">-1.10<\/td>\n<td>\u8f83\u53ef\u80fd\u7a7a\u95f2<\/td>\n<\/tr>\n<tr>\n<td align=\"center\">0.50<\/td>\n<td align=\"center\">0<\/td>\n<td>\u5b8c\u5168\u672a\u77e5<\/td>\n<\/tr>\n<tr>\n<td align=\"center\">0.75<\/td>\n<td align=\"center\">&#043;1.10<\/td>\n<td>\u8f83\u53ef\u80fd\u5360\u636e<\/td>\n<\/tr>\n<tr>\n<td align=\"center\">0.99<\/td>\n<td align=\"center\">&#043;4.60<\/td>\n<td>\u51e0\u4e4e\u786e\u5b9a\u5360\u636e<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li><span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\">\n<p>         p<\/p>\n<p>         &#061;<\/p>\n<p>         0.5<\/p>\n<p>        p&#061;0.5<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.625em;vertical-align: -0.1944em\"><\/span><span class=\"mord mathnormal\">p<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.6444em\"><\/span><span class=\"mord\">0.5<\/span><\/span><\/span><\/span><\/span> \u5bf9\u5e94 <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         l<\/p>\n<p>         &#061;<\/p>\n<p>         0<\/p>\n<p>        l&#061;0<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.6944em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.6444em\"><\/span><span class=\"mord\">0<\/span><\/span><\/span><\/span><\/span>&#xff0c;<span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         p<\/p>\n<p>        p<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.625em;vertical-align: -0.1944em\"><\/span><span class=\"mord mathnormal\">p<\/span><\/span><\/span><\/span><\/span> \u8d8b\u8fd1 0 \u5219 <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         l<\/p>\n<p>        l<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.6944em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><\/span><\/span><\/span><\/span> \u5954\u8d1f\u65e0\u7a77&#xff0c;<span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         p<\/p>\n<p>        p<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.625em;vertical-align: -0.1944em\"><\/span><span class=\"mord mathnormal\">p<\/span><\/span><\/span><\/span><\/span> \u8d8b\u8fd1 1 \u5219 <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         l<\/p>\n<p>        l<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.6944em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><\/span><\/span><\/span><\/span> \u5954\u6b63\u65e0\u7a77<\/li>\n<li>\u4ee3\u7801\u5b9e\u73b0\u5c31\u662f\u8fd9\u4e24\u884c\u4e92\u76f8\u8f6c\u6362&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token keyword\">double<\/span> <span class=\"token function\">logOddsToProb<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> l<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span> <span class=\"token keyword\">return<\/span> <span class=\"token number\">1.0<\/span> <span class=\"token operator\">\/<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#043;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">exp<\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">&#8211;<\/span>l<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token keyword\">double<\/span> <span class=\"token function\">probToLogOdds<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> p<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span> <span class=\"token keyword\">return<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">log<\/span><span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">\/<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#8211;<\/span> p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>\u4e24\u6761\u66f2\u7ebf\u7684\u5f62\u72b6\u5982\u4e0b\u2014\u2014\u5de6\u56fe\u662f log-odds \u2192 \u6982\u7387&#xff08;sigmoid \/ logistic&#xff09;&#xff0c;\u53f3\u56fe\u662f\u6982\u7387 \u2192 log-odds&#xff08;logit&#xff09;&#xff1a;<\/li>\n<\/ul>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160847-6a6a258f8739e.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<ul>\n<li>\u51e0\u4e2a\u5173\u952e\u70b9\u770b\u4e00\u773c\u5c31\u660e\u767d&#xff1a;\n<ul>\n<li>l&#061;0 \u59cb\u7ec8\u5bf9\u5e94 p&#061;0.5&#xff08;\u5b8c\u5168\u672a\u77e5&#xff0c;log-odds \u4e3a 0&#xff09;<\/li>\n<li>l \u5728 \u00b14 \u9644\u8fd1\u94b3\u4f4d&#xff0c;\u5bf9\u5e94\u7684\u6982\u7387\u5728 0.018 ~ 0.982 \u4e4b\u95f4&#xff0c;\u9632\u6b62\u6781\u7aef\u503c<\/li>\n<li>\u6982\u7387\u5728 0.3~0.7 \u4e4b\u95f4\u7684\u7070\u8272\u8fc7\u6e21\u5e26&#xff0c;\u5bf9\u5e94 l \u5728 [-0.85, &#043;0.85] \u4e4b\u95f4\u2014\u2014\u8fd9\u6b63\u662f\u4fdd\u5b88\u4f20\u611f\u5668\u6a21\u578b\u5e0c\u671b\u4fdd\u7559\u7684\u533a\u57df<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<h6>2-1-2 \u8d1d\u53f6\u65af\u66f4\u65b0\u5728 log-odds \u7a7a\u95f4 &#061; \u52a0\u6cd5<\/h6>\n<ul>\n<li>\u5047\u8bbe\u6bcf\u4e2a cell \u7684\u5360\u636e\u4e8b\u4ef6 <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\">\n<p>         m<\/p>\n<p>        m<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.4306em\"><\/span><span class=\"mord mathnormal\">m<\/span><\/span><\/span><\/span><\/span> \u5728\u7ed9\u5b9a\u6240\u6709\u89c2\u6d4b <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>          z<\/p>\n<p>           1<\/p>\n<p>           :<\/p>\n<p>           t<\/p>\n<p>        z_{1:t}<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.5806em;vertical-align: -0.15em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3011em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.044em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mtight\">1<\/span><span class=\"mrel mtight\">:<\/span><span class=\"mord mathnormal mtight\">t<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span> \u4e0b\u7684\u540e\u9a8c\u6982\u7387\u4e3a <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         P<\/p>\n<p>         (<\/p>\n<p>         m<\/p>\n<p>         \u2223<\/p>\n<p>          z<\/p>\n<p>           1<\/p>\n<p>           :<\/p>\n<p>           t<\/p>\n<p>         )<\/p>\n<p>        P(m \\\\mid z_{1:t})<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3011em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.044em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mtight\">1<\/span><span class=\"mrel mtight\">:<\/span><span class=\"mord mathnormal mtight\">t<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><\/span><\/li>\n<li>\u76f4\u63a5\u4f7f\u7528\u8d1d\u53f6\u65af\u516c\u5f0f&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         P<\/p>\n<p>         (<\/p>\n<p>         m<\/p>\n<p>         \u2223<\/p>\n<p>          z<\/p>\n<p>           1<\/p>\n<p>           :<\/p>\n<p>           t<\/p>\n<p>         )<\/p>\n<p>         &#061;<\/p>\n<p>           P<\/p>\n<p>           (<\/p>\n<p>            z<\/p>\n<p>            t<\/p>\n<p>           \u2223<\/p>\n<p>           m<\/p>\n<p>           )<\/p>\n<p>           \u22c5<\/p>\n<p>           P<\/p>\n<p>           (<\/p>\n<p>           m<\/p>\n<p>           \u2223<\/p>\n<p>            z<\/p>\n<p>             1<\/p>\n<p>             :<\/p>\n<p>             t<\/p>\n<p>             \u2212<\/p>\n<p>             1<\/p>\n<p>           )<\/p>\n<p>           P<\/p>\n<p>           (<\/p>\n<p>            z<\/p>\n<p>            t<\/p>\n<p>           \u2223<\/p>\n<p>            z<\/p>\n<p>             1<\/p>\n<p>             :<\/p>\n<p>             t<\/p>\n<p>             \u2212<\/p>\n<p>             1<\/p>\n<p>           )<\/p>\n<p>        P(m \\\\mid z_{1:t}) &#061; \\\\frac{P(z_t \\\\mid m) \\\\cdot P(m \\\\mid z_{1:t-1})}{P(z_t \\\\mid z_{1:t-1})}<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3011em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.044em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mtight\">1<\/span><span class=\"mrel mtight\">:<\/span><span class=\"mord mathnormal mtight\">t<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 2.363em;vertical-align: -0.936em\"><\/span><span class=\"mord\"><span class=\"mopen nulldelimiter\"><\/span><span class=\"mfrac\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.427em\"><span class=\"\" style=\"top: -2.314em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2806em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.044em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">t<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3011em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.044em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mtight\">1<\/span><span class=\"mrel mtight\">:<\/span><span class=\"mord mathnormal mtight\">t<\/span><span class=\"mbin mtight\">\u2212<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2083em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mclose\">)<\/span><\/span><\/span><span class=\"\" style=\"top: -3.23em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"frac-line\" style=\"border-bottom-width: 0.04em\"><\/span><\/span><span class=\"\" style=\"top: -3.677em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2806em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.044em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">t<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u22c5<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3011em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.044em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mtight\">1<\/span><span class=\"mrel mtight\">:<\/span><span class=\"mord mathnormal mtight\">t<\/span><span class=\"mbin mtight\">\u2212<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2083em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.936em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mclose nulldelimiter\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\u8fd9\u4e2a\u5f62\u5f0f\u6bcf\u6b21\u90fd\u8981\u7b97\u5f52\u4e00\u5316\u9879 P(z_t | z_{1:t-1})&#xff0c;\u4e0d\u597d\u7b97<\/li>\n<li>\u4f46\u5bf9\u4e24\u8fb9\u53d6 log-odds \u540e&#xff0c;\u8d1d\u53f6\u65af\u9012\u63a8 \u9000\u5316\u6210\u52a0\u6cd5&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>          l<\/p>\n<p>          t<\/p>\n<p>         &#061;<\/p>\n<p>          l<\/p>\n<p>           t<\/p>\n<p>           \u2212<\/p>\n<p>           1<\/p>\n<p>         &#043;<\/p>\n<p>         ln<\/p>\n<p>         \u2061<\/p>\n<p>           P<\/p>\n<p>           (<\/p>\n<p>            z<\/p>\n<p>            t<\/p>\n<p>           \u2223<\/p>\n<p>           m<\/p>\n<p>           &#061;<\/p>\n<p>           1<\/p>\n<p>           )<\/p>\n<p>           P<\/p>\n<p>           (<\/p>\n<p>            z<\/p>\n<p>            t<\/p>\n<p>           \u2223<\/p>\n<p>           m<\/p>\n<p>           &#061;<\/p>\n<p>           0<\/p>\n<p>           )<\/p>\n<p>        l_t &#061; l_{t-1} &#043; \\\\ln\\\\frac{P(z_t \\\\mid m&#061;1)}{P(z_t \\\\mid m&#061;0)}<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.8444em;vertical-align: -0.15em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2806em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0197em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">t<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.9028em;vertical-align: -0.2083em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3011em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0197em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\">t<\/span><span class=\"mbin mtight\">\u2212<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2083em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 2.363em;vertical-align: -0.936em\"><\/span><span class=\"mop\">ln<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord\"><span class=\"mopen nulldelimiter\"><\/span><span class=\"mfrac\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.427em\"><span class=\"\" style=\"top: -2.314em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2806em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.044em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">t<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mord\">0<\/span><span class=\"mclose\">)<\/span><\/span><\/span><span class=\"\" style=\"top: -3.23em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"frac-line\" style=\"border-bottom-width: 0.04em\"><\/span><\/span><span class=\"\" style=\"top: -3.677em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2806em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.044em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">t<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mord\">1<\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.936em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mclose nulldelimiter\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\u672c\u8d28\u4e0a\u8ddf&#034;\u53d6\u5bf9\u6570\u628a <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\">\n<p>         a<\/p>\n<p>         \u00d7<\/p>\n<p>         b<\/p>\n<p>        a \\\\times b<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.6667em;vertical-align: -0.0833em\"><\/span><span class=\"mord mathnormal\">a<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u00d7<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.6944em\"><\/span><span class=\"mord mathnormal\">b<\/span><\/span><\/span><\/span><\/span> \u53d8\u6210 <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         log<\/p>\n<p>         \u2061<\/p>\n<p>         a<\/p>\n<p>         &#043;<\/p>\n<p>         log<\/p>\n<p>         \u2061<\/p>\n<p>         b<\/p>\n<p>        \\\\log a &#043; \\\\log b<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.8889em;vertical-align: -0.1944em\"><\/span><span class=\"mop\">lo<span style=\"margin-right: 0.0139em\">g<\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord mathnormal\">a<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.8889em;vertical-align: -0.1944em\"><\/span><span class=\"mop\">lo<span style=\"margin-right: 0.0139em\">g<\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord mathnormal\">b<\/span><\/span><\/span><\/span><\/span>&#034;\u662f\u540c\u4e00\u4e2a\u9053\u7406<\/li>\n<li>\u843d\u5b9e\u5230\u4ee3\u7801&#xff0c;\u6bcf\u6761\u6fc0\u5149\u675f\u7a7f\u8fc7\u6216\u547d\u4e2d\u7684 cell \u53ea\u9700\u8981\u4e00\u884c\u52a0\u6cd5&#xff1a;<\/li>\n<\/ul>\n<p>log_odds_<span class=\"token punctuation\">[<\/span>idx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span>log_odds_<span class=\"token punctuation\">[<\/span>idx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#043;<\/span> l_free_<span class=\"token punctuation\">,<\/span> log_odds_min_<span class=\"token punctuation\">,<\/span> log_odds_max_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<ul>\n<li>\u505a\u4e00\u6b21\u52a0\u6cd5 &#061; \u5b8c\u6210\u4e00\u6b21\u8d1d\u53f6\u65af\u66f4\u65b0\u3002\u4e00\u6574\u5e27\u96f7\u8fbe\u51e0\u5343\u6761\u5c04\u7ebf&#xff0c;\u5168\u662f\u52a0\u6cd5\u2014\u2014\u8fd9\u4e5f\u662f OctoMap \u5e95\u5c42\u7528\u540c\u4e00\u5957\u516c\u5f0f\u7684\u539f\u56e0<\/li>\n<\/ul>\n<h5>2-2 \u9006\u4f20\u611f\u5668\u6a21\u578b&#xff08;Inverse Sensor Model&#xff09;<\/h5>\n<ul>\n<li>\u4f20\u611f\u5668\u6a21\u578b\u5206\u4e24\u79cd\u2014\u2014\u95ee\u7684\u95ee\u9898\u6070\u597d\u76f8\u53cd&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u6b63\u5411 (Forward)\u53cd\u5411 (Inverse)<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">\u8981\u56de\u7b54\u4ec0\u4e48<\/td>\n<td align=\"left\">\u7ed9\u5b9a\u5730\u56fe&#xff0c;\u4f20\u611f\u5668\u4f1a\u770b\u5230\u4ec0\u4e48<\/td>\n<td align=\"left\">\u7ed9\u5b9a\u4f20\u611f\u5668\u8bfb\u6570&#xff0c;\u5730\u56fe\u957f\u4ec0\u4e48\u6837<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\u6570\u5b66\u5f62\u5f0f<\/td>\n<td align=\"left\"><span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>           P<\/p>\n<p>           (<\/p>\n<p>           z<\/p>\n<p>           \u2223<\/p>\n<p>           m<\/p>\n<p>           )<\/p>\n<p>          P(z \\\\mid m)<\/p>\n<p>       <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><\/span><\/td>\n<td align=\"left\"><span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>           P<\/p>\n<p>           (<\/p>\n<p>           m<\/p>\n<p>           \u2223<\/p>\n<p>           z<\/p>\n<p>           )<\/p>\n<p>          P(m \\\\mid z)<\/p>\n<p>       <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><\/span><\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\u8c01\u6765\u505a<\/td>\n<td align=\"left\">\u4eff\u771f\u5668\u3001\u5149\u7ebf\u8ffd\u8e2a<\/td>\n<td align=\"left\">SLAM\u3001\u5360\u636e\u7f51\u683c\u5efa\u56fe<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\u96be\u5ea6<\/td>\n<td align=\"left\">\u7b80\u5355&#xff08;\u7eaf\u51e0\u4f55&#xff09;<\/td>\n<td align=\"left\">\u9700\u8981\u8d1d\u53f6\u65af\u63a8\u65ad<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li>\u6211\u4eec\u8fd9\u4e2a\u573a\u666f\u663e\u7136\u662f\u540e\u8005\u2014\u2014\u5df2\u77e5\u6bcf\u675f\u6fc0\u5149\u662f&#034;\u6253\u5230\u4e86\u4e1c\u897f&#xff08;hit&#xff09;\u201c\u8fd8\u662f&#034;\u7a7f\u8fc7\u4e86&#xff08;miss&#xff09;\u201d&#xff0c;\u8981\u53cd\u8fc7\u6765\u63a8\u65ad\u6bcf\u683c\u88ab\u5360\u636e\u7684\u6982\u7387<\/li>\n<li>\u56de\u5230 2-1-2 \u7684\u516c\u5f0f&#xff0c;<span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\">\n<p>          l<\/p>\n<p>          t<\/p>\n<p>         &#061;<\/p>\n<p>          l<\/p>\n<p>           t<\/p>\n<p>           \u2212<\/p>\n<p>           1<\/p>\n<p>         &#043;<\/p>\n<p>         ln<\/p>\n<p>         \u2061<\/p>\n<p>           P<\/p>\n<p>           (<\/p>\n<p>            z<\/p>\n<p>            t<\/p>\n<p>           \u2223<\/p>\n<p>           m<\/p>\n<p>           &#061;<\/p>\n<p>           1<\/p>\n<p>           )<\/p>\n<p>           P<\/p>\n<p>           (<\/p>\n<p>            z<\/p>\n<p>            t<\/p>\n<p>           \u2223<\/p>\n<p>           m<\/p>\n<p>           &#061;<\/p>\n<p>           0<\/p>\n<p>           )<\/p>\n<p>        l_t &#061; l_{t-1} &#043; \\\\ln\\\\frac{P(z_t \\\\mid m&#061;1)}{P(z_t \\\\mid m&#061;0)}<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.8444em;vertical-align: -0.15em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2806em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0197em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">t<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.9028em;vertical-align: -0.2083em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3011em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0197em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\">t<\/span><span class=\"mbin mtight\">\u2212<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2083em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1.53em;vertical-align: -0.52em\"><\/span><span class=\"mop\">ln<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord\"><span class=\"mopen nulldelimiter\"><\/span><span class=\"mfrac\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.01em\"><span class=\"\" style=\"top: -2.655em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen mtight\">(<\/span><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.044em\">z<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2963em\"><span class=\"\" style=\"top: -2.357em;margin-left: -0.044em;margin-right: 0.0714em\"><span class=\"pstrut\" style=\"height: 2.5em\"><\/span><span class=\"sizing reset-size3 size1 mtight\"><span class=\"mord mathnormal mtight\">t<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.143em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mrel mtight\">\u2223<\/span><span class=\"mord mathnormal mtight\">m<\/span><span class=\"mrel mtight\">&#061;<\/span><span class=\"mord mtight\">0<\/span><span class=\"mclose mtight\">)<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.23em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"frac-line\" style=\"border-bottom-width: 0.04em\"><\/span><\/span><span class=\"\" style=\"top: -3.485em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen mtight\">(<\/span><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.044em\">z<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2963em\"><span class=\"\" style=\"top: -2.357em;margin-left: -0.044em;margin-right: 0.0714em\"><span class=\"pstrut\" style=\"height: 2.5em\"><\/span><span class=\"sizing reset-size3 size1 mtight\"><span class=\"mord mathnormal mtight\">t<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.143em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mrel mtight\">\u2223<\/span><span class=\"mord mathnormal mtight\">m<\/span><span class=\"mrel mtight\">&#061;<\/span><span class=\"mord mtight\">1<\/span><span class=\"mclose mtight\">)<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.52em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mclose nulldelimiter\"><\/span><\/span><\/span><\/span><\/span><\/span> \u91cc\u90a3\u4e2a\u5206\u6570\u5c31\u662f\u9006\u4f20\u611f\u5668\u6a21\u578b\u7ed9\u51fa\u7684\u6bd4\u503c&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u6fc0\u5149\u8bfb\u6570\u9006\u4f20\u611f\u5668\u6a21\u578b\u6211\u4eec\u7528\u7684\u503clog-odds \u589e\u91cf<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">\u547d\u4e2d&#xff08;hit&#xff09;<\/td>\n<td align=\"left\"><span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>           P<\/p>\n<p>           (<\/p>\n<p>           m<\/p>\n<p>           &#061;<\/p>\n<p>           1<\/p>\n<p>           \u2223<\/p>\n<p>           z<\/p>\n<p>           &#061;<\/p>\n<p>           hit<\/p>\n<p>           )<\/p>\n<p>          P(m&#061;1 \\\\mid z&#061;\\\\text{hit})<\/p>\n<p>       <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord\">1<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.4306em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord text\"><span class=\"mord\">hit<\/span><\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><\/span><\/td>\n<td align=\"center\">prob_hit &#061; 0.55<\/td>\n<td align=\"center\"><span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>            l<\/p>\n<p>             o<\/p>\n<p>             c<\/p>\n<p>             c<\/p>\n<p>           &#061;<\/p>\n<p>           ln<\/p>\n<p>           \u2061<\/p>\n<p>            0.55<\/p>\n<p>            0.45<\/p>\n<p>           \u2248<\/p>\n<p>           &#043;<\/p>\n<p>           0.20<\/p>\n<p>          l_{occ} &#061; \\\\ln\\\\frac{0.55}{0.45} \\\\approx &#043;0.20<\/p>\n<p>       <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.8444em;vertical-align: -0.15em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.1514em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0197em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\">occ<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1.1901em;vertical-align: -0.345em\"><\/span><span class=\"mop\">ln<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord\"><span class=\"mopen nulldelimiter\"><\/span><span class=\"mfrac\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.8451em\"><span class=\"\" style=\"top: -2.655em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mtight\">0.45<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.23em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"frac-line\" style=\"border-bottom-width: 0.04em\"><\/span><\/span><span class=\"\" style=\"top: -3.394em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mtight\">0.55<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.345em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mclose nulldelimiter\"><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2248<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.7278em;vertical-align: -0.0833em\"><\/span><span class=\"mord\">&#043;<\/span><span class=\"mord\">0.20<\/span><\/span><\/span><\/span><\/span><\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\u7a7f\u8fc7&#xff08;miss&#xff09;<\/td>\n<td align=\"left\"><span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>           P<\/p>\n<p>           (<\/p>\n<p>           m<\/p>\n<p>           &#061;<\/p>\n<p>           1<\/p>\n<p>           \u2223<\/p>\n<p>           z<\/p>\n<p>           &#061;<\/p>\n<p>           miss<\/p>\n<p>           )<\/p>\n<p>          P(m&#061;1 \\\\mid z&#061;\\\\text{miss})<\/p>\n<p>       <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord\">1<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.4306em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord text\"><span class=\"mord\">miss<\/span><\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><\/span><\/td>\n<td align=\"center\">prob_miss &#061; 0.45<\/td>\n<td align=\"center\"><span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>            l<\/p>\n<p>             f<\/p>\n<p>             r<\/p>\n<p>             e<\/p>\n<p>             e<\/p>\n<p>           &#061;<\/p>\n<p>           ln<\/p>\n<p>           \u2061<\/p>\n<p>            0.45<\/p>\n<p>            0.55<\/p>\n<p>           \u2248<\/p>\n<p>           \u2212<\/p>\n<p>           0.20<\/p>\n<p>          l_{free} &#061; \\\\ln\\\\frac{0.45}{0.55} \\\\approx -0.20<\/p>\n<p>       <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.9805em;vertical-align: -0.2861em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0197em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.1076em\">f<\/span><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0278em\">r<\/span><span class=\"mord mathnormal mtight\">ee<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2861em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1.1901em;vertical-align: -0.345em\"><\/span><span class=\"mop\">ln<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord\"><span class=\"mopen nulldelimiter\"><\/span><span class=\"mfrac\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.8451em\"><span class=\"\" style=\"top: -2.655em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mtight\">0.55<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.23em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"frac-line\" style=\"border-bottom-width: 0.04em\"><\/span><\/span><span class=\"\" style=\"top: -3.394em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mtight\">0.45<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.345em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mclose nulldelimiter\"><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2248<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.7278em;vertical-align: -0.0833em\"><\/span><span class=\"mord\">\u2212<\/span><span class=\"mord\">0.20<\/span><\/span><\/span><\/span><\/span><\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\u521d\u59cb<\/td>\n<td align=\"left\"><span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>           P<\/p>\n<p>           (<\/p>\n<p>           m<\/p>\n<p>           &#061;<\/p>\n<p>           1<\/p>\n<p>           )<\/p>\n<p>           &#061;<\/p>\n<p>           0.5<\/p>\n<p>          P(m&#061;1) &#061; 0.5<\/p>\n<p>       <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord\">1<\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.6444em\"><\/span><span class=\"mord\">0.5<\/span><\/span><\/span><\/span><\/span><\/td>\n<td align=\"center\">0.5<\/td>\n<td align=\"center\"><span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>           l<\/p>\n<p>           &#061;<\/p>\n<p>           0<\/p>\n<p>          l &#061; 0<\/p>\n<p>       <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.6944em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.6444em\"><\/span><span class=\"mord\">0<\/span><\/span><\/span><\/span><\/span><\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li>\u8fd9\u91cc\u9009\u62e9\u4e86 \u4fdd\u5b88\u53c2\u6570 P(hit)&#061;0.55 \/ P(miss)&#061;0.45&#xff0c;\u76ee\u7684\u662f\u8ba9\u6982\u7387 \u7f13\u6162\u6536\u655b&#xff0c;\u5728\u524d\u6cbf\u9644\u8fd1\u4fdd\u7559 5~10 \u683c\u5bbd\u7684\u7070\u8272\u6e10\u53d8\u5e26<\/li>\n<li>\u5bf9\u6bd4\u4e00\u4e0b\u4e0d\u540c\u53c2\u6570\u7684\u6536\u655b\u901f\u5ea6&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u6fc0\u8fdb (0.75 \/ 0.30)\u4fdd\u5b88 (0.55 \/ 0.45)<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">1 \u6b21\u626b\u63cf\u540e P<\/td>\n<td align=\"center\">0.75 \/ 0.30<\/td>\n<td align=\"center\">0.55 \/ 0.45<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">3 \u6b21\u626b\u63cf\u540e P<\/td>\n<td align=\"center\">0.96 \/ 0.09<\/td>\n<td align=\"center\">0.65 \/ 0.35<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">10 \u6b21\u626b\u63cf\u540e P<\/td>\n<td align=\"center\">\u2248 1.0 \/ \u2248 0<\/td>\n<td align=\"center\">0.81 \/ 0.19<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\u524d\u6cbf\u6e10\u53d8\u5e26\u5bbd<\/td>\n<td align=\"center\">1~2 \u683c<\/td>\n<td align=\"center\">5~10 \u683c<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">MI \u533a\u5206\u5ea6<\/td>\n<td align=\"center\">\u5f31&#xff08;\u8ddf\u4e09\u503c\u5730\u56fe\u5dee\u4e0d\u591a&#xff09;<\/td>\n<td align=\"center\">\u5f3a&#xff08;\u71b5\u503c\u5dee\u5f02\u663e\u8457&#xff09;<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li>\u8bf4\u4eba\u8bdd&#xff1a;\u6fc0\u8fdb\u53c2\u6570\u8ba9\u6982\u7387&#034;\u79d2\u53d8\u9ed1\u767d&#034;&#xff0c;\u524d\u6cbf\u9644\u8fd1\u51e0\u4e4e\u6ca1\u6709\u7070\u5ea6\u8fc7\u6e21&#xff0c;MI \u7b97\u6cd5\u65e0\u4ece\u533a\u5206&#xff1b;\u4fdd\u5b88\u53c2\u6570\u8ba9\u6982\u7387\u6162\u6162\u6536\u655b&#xff0c;\u7070\u8272\u6e10\u53d8\u5e26\u8db3\u591f\u5bbd&#xff0c;MI \u624d\u80fd\u5728\u4e0d\u540c\u524d\u6cbf\u4e4b\u95f4\u62c9\u5f00\u5dee\u8ddd<\/li>\n<\/ul>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160849-6a6a25910f992.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<h5>2-3 Bresenham \u5149\u7ebf\u8ffd\u8e2a<\/h5>\n<ul>\n<li>\u6bcf\u6761\u6fc0\u5149\u675f\u4ece\u4f20\u611f\u5668\u539f\u70b9\u51fa\u53d1&#xff0c;\u5728\u7f51\u683c\u4e2d\u7a7f\u8fc7\u7684\u6240\u6709 cell \u90fd\u9700\u8981\u88ab\u66f4\u65b0\u2014\u2014\u547d\u4e2d\u70b9&#xff08;l_occ&#xff09;&#043; \u7a7f\u8fc7\u70b9&#xff08;l_free&#xff09;<\/li>\n<li>\u8fd9\u91cc\u4f7f\u7528\u7ecf\u5178\u7684 Bresenham 2D \u76f4\u7ebf\u7b97\u6cd5&#xff0c;\u5b83\u53ef\u4ee5\u9ad8\u6548\u5730\u679a\u4e3e\u4e24\u4e2a\u7f51\u683c\u5750\u6807\u4e4b\u95f4\u7684\u6240\u6709 cell&#xff0c;\u65f6\u95f4\u590d\u6742\u5ea6 <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\">\n<p>         O<\/p>\n<p>         (<\/p>\n<p>         max<\/p>\n<p>         \u2061<\/p>\n<p>         (<\/p>\n<p>         \u2223<\/p>\n<p>         \u0394<\/p>\n<p>         x<\/p>\n<p>         \u2223<\/p>\n<p>         ,<\/p>\n<p>         \u2223<\/p>\n<p>         \u0394<\/p>\n<p>         y<\/p>\n<p>         \u2223<\/p>\n<p>         )<\/p>\n<p>         )<\/p>\n<p>        O(\\\\max(|\\\\Delta x|, |\\\\Delta y|))<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0278em\">O<\/span><span class=\"mopen\">(<\/span><span class=\"mop\">max<\/span><span class=\"mopen\">(<\/span><span class=\"mord\">\u2223\u0394<\/span><span class=\"mord mathnormal\">x<\/span><span class=\"mord\">\u2223<\/span><span class=\"mpunct\">,<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord\">\u2223\u0394<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">y<\/span><span class=\"mord\">\u2223<\/span><span class=\"mclose\">))<\/span><\/span><\/span><\/span><\/span>&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ \u5bf9\u5e94 2-3 \u8282&#xff1a;Bresenham 2D \u76f4\u7ebf\u904d\u5386&#xff08;\u6574\u6570\u7f51\u683c&#xff0c;\u542b\u4e24\u7aef\u70b9&#xff09;<\/span><br \/>\n<span class=\"token keyword\">void<\/span> <span class=\"token function\">bresenhamLine<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> x0<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> y0<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> x1<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> y1<span class=\"token punctuation\">,<\/span><br \/>\n                   std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>std<span class=\"token double-colon punctuation\">::<\/span>pair<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;&gt;<\/span> <span class=\"token operator\">&amp;<\/span> out<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> dx <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">abs<\/span><span class=\"token punctuation\">(<\/span>x1 <span class=\"token operator\">&#8211;<\/span> x0<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> dy <span class=\"token operator\">&#061;<\/span> <span class=\"token operator\">&#8211;<\/span>std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">abs<\/span><span class=\"token punctuation\">(<\/span>y1 <span class=\"token operator\">&#8211;<\/span> y0<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span>   <span class=\"token comment\">\/\/ \u8d1f\u503c&#xff08;\u7b97\u6cd5\u7ea6\u5b9a&#xff09;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> sx <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>x0 <span class=\"token operator\">&lt;<\/span> x1<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">?<\/span> <span class=\"token number\">1<\/span> <span class=\"token operator\">:<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> sy <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>y0 <span class=\"token operator\">&lt;<\/span> y1<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">?<\/span> <span class=\"token number\">1<\/span> <span class=\"token operator\">:<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> err <span class=\"token operator\">&#061;<\/span> dx <span class=\"token operator\">&#043;<\/span> dy<span class=\"token punctuation\">;<\/span><\/p>\n<p>  out<span class=\"token punctuation\">.<\/span><span class=\"token function\">clear<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">while<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token boolean\">true<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    out<span class=\"token punctuation\">.<\/span><span class=\"token function\">emplace_back<\/span><span class=\"token punctuation\">(<\/span>x0<span class=\"token punctuation\">,<\/span> y0<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>x0 <span class=\"token operator\">&#061;&#061;<\/span> x1 <span class=\"token operator\">&amp;&amp;<\/span> y0 <span class=\"token operator\">&#061;&#061;<\/span> y1<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">break<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">int<\/span> e2 <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">2<\/span> <span class=\"token operator\">*<\/span> err<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>e2 <span class=\"token operator\">&gt;&#061;<\/span> dy<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span> err <span class=\"token operator\">&#043;&#061;<\/span> dy<span class=\"token punctuation\">;<\/span> x0 <span class=\"token operator\">&#043;&#061;<\/span> sx<span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>e2 <span class=\"token operator\">&lt;&#061;<\/span> dx<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span> err <span class=\"token operator\">&#043;&#061;<\/span> dx<span class=\"token punctuation\">;<\/span> y0 <span class=\"token operator\">&#043;&#061;<\/span> sy<span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>\n<p>\u6bcf\u6761\u6fc0\u5149\u675f&#xff08;\u5171\u7ea6 1080 \u6761&#xff09;\u90fd\u8c03\u7528\u4e00\u6b21 Bresenham&#xff0c;\u7136\u540e&#xff1a;<\/p>\n<ul>\n<li>\u904d\u5386 cell&#xff08;0 ~ len-2&#xff09;&#xff1a;log_odds &#043;&#061; l_free&#xff08;\u6807\u8bb0\u4e3a \u7a7a\u95f2&#xff09;<\/li>\n<li>\u547d\u4e2d cell&#xff08;len-1&#xff09;&#xff1a;log_odds &#043;&#061; l_occ&#xff08;\u6807\u8bb0\u4e3a \u5360\u636e&#xff09;<\/li>\n<\/ul>\n<\/li>\n<li>\n<p>\u6ce8\u610f&#xff1a;\u5982\u679c\u5149\u675f\u8bfb\u6570\u5230\u8fbe max_range&#xff08;\u65e0\u56de\u6ce2&#xff09;&#xff0c;\u53ea\u66f4\u65b0\u904d\u5386 cell&#xff0c;\u4e0d\u66f4\u65b0\u547d\u4e2d cell\u2014\u2014\u5408\u7406\u7684&#xff0c;\u4f60\u770b\u4e0d\u5230\u56de\u6ce2\u610f\u5473\u7740\u5149\u675f\u6ca1\u9047\u5230\u4e1c\u897f<\/p>\n<\/li>\n<\/ul>\n<h5>2-4 \u4ee3\u7801\u5b9e\u73b0&#xff1a;ProbabilityMapper \u5b8c\u6574\u7c7b<\/h5>\n<ul>\n<li>\u5148\u770b\u5934\u6587\u4ef6\u2014\u2014\u7c7b\u5bf9\u5916\u66b4\u9732\u7684\u63a5\u53e3&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ prob_map\/probability_mapper.hpp \u2014\u2014 \u5bf9\u5e94 2-4 \u8282<\/span><br \/>\n<span class=\"token keyword\">namespace<\/span> prob_map <span class=\"token punctuation\">{<\/span><\/p>\n<p><span class=\"token keyword\">class<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n<span class=\"token keyword\">public<\/span><span class=\"token operator\">:<\/span><br \/>\n  <span class=\"token keyword\">explicit<\/span> <span class=\"token function\">ProbabilityMapper<\/span><span class=\"token punctuation\">(<\/span>rclcpp<span class=\"token double-colon punctuation\">::<\/span>Node <span class=\"token operator\">*<\/span> node<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p><span class=\"token keyword\">private<\/span><span class=\"token operator\">:<\/span><br \/>\n  <span class=\"token comment\">\/\/ \u6fc0\u5149\u56de\u8c03&#xff1a;TF \u67e5\u5230\u4f20\u611f\u5668\u4f4d\u59ff \u2192 updateMap()<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">laserCallback<\/span><span class=\"token punctuation\">(<\/span>sensor_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>LaserScan<span class=\"token double-colon punctuation\">::<\/span>SharedPtr msg<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">publishTimerCallback<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ \u6838\u5fc3\u5efa\u56fe&#xff1a;\u9010\u675f\u5c04\u7ebf Bresenham \u5149\u7ebf\u8ffd\u8e2a&#xff0c;log-odds \u9012\u63a8\u66f4\u65b0<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">updateMap<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> sensor_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>LaserScan <span class=\"token operator\">&amp;<\/span> scan<span class=\"token punctuation\">,<\/span><br \/>\n                 <span class=\"token keyword\">const<\/span> geometry_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Pose <span class=\"token operator\">&amp;<\/span> sensor_pose<span class=\"token punctuation\">,<\/span><br \/>\n                 <span class=\"token keyword\">double<\/span> sensor_yaw<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">void<\/span> <span class=\"token function\">initGrid<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">static<\/span> <span class=\"token keyword\">void<\/span> <span class=\"token function\">bresenhamLine<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> x0<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> y0<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> x1<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> y1<span class=\"token punctuation\">,<\/span><br \/>\n                            std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>std<span class=\"token double-colon punctuation\">::<\/span>pair<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;&gt;<\/span> <span class=\"token operator\">&amp;<\/span> out<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ log-odds \u2194 \u6982\u7387 \u2194 \u71b5 \u7684\u4e92\u8f6c<\/span><br \/>\n  <span class=\"token keyword\">static<\/span> <span class=\"token keyword\">double<\/span> <span class=\"token function\">logOddsToProb<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> l<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">static<\/span> <span class=\"token keyword\">double<\/span> <span class=\"token function\">probToLogOdds<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">static<\/span> <span class=\"token keyword\">double<\/span> <span class=\"token function\">entropy<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ \u53cc Topic \u53d1\u5e03<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">publishMaps<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span>Time <span class=\"token operator\">&amp;<\/span> stamp<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">const<\/span> std<span class=\"token double-colon punctuation\">::<\/span>string <span class=\"token operator\">&amp;<\/span> frame_id<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ \u6838\u5fc3\u6570\u636e&#xff1a;log-odds \u7f51\u683c&#xff08;\u4e0d\u518d\u5b58\u4e09\u503c&#xff01;&#xff09;<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span> log_odds_<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> width_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">,<\/span> height_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ \u4f20\u611f\u5668\u6a21\u578b\u53c2\u6570<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> prob_hit_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.55<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span>    <span class=\"token comment\">\/\/ P(occ | hit)<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> prob_miss_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.45<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span>   <span class=\"token comment\">\/\/ P(occ | miss)<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> l_occ_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span>        <span class=\"token comment\">\/\/ ln(0.55\/0.45) \u2248 0.20<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> l_free_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span>       <span class=\"token comment\">\/\/ ln(0.45\/0.55) \u2248 -0.20<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> log_odds_min_<span class=\"token punctuation\">{<\/span><span class=\"token operator\">&#8211;<\/span><span class=\"token number\">4.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token comment\">\/\/ P \u2248 0.018<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> log_odds_max_<span class=\"token punctuation\">{<\/span><span class=\"token number\">4.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span>  <span class=\"token comment\">\/\/ P \u2248 0.982<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> max_range_<span class=\"token punctuation\">{<\/span><span class=\"token number\">30.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span>    <span class=\"token comment\">\/\/ \u6fc0\u5149\u6700\u5927\u4f7f\u7528\u8ddd\u79bb<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ Publishers \u2014 \u6ce8\u610f\u8fd9\u91cc\u662f\u4e09\u4e2a publisher<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span>Publisher<span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span>SharedPtr prob_map_pub_<span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span>Publisher<span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span>SharedPtr thresh_map_pub_<span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span>Publisher<span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span>SharedPtr entropy_map_pub_<span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p><span class=\"token punctuation\">}<\/span>  <span class=\"token comment\">\/\/ namespace prob_map<\/span><\/p>\n<ul>\n<li>\u6838\u5fc3\u7684 updateMap() \u903b\u8f91\u2014\u2014\u9010\u675f\u5904\u7406\u5168\u90e8\u6fc0\u5149&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ \u5bf9\u5e94 2-4 \u8282&#xff1a;updateMap \u2014\u2014 \u9010\u675f\u5c04\u7ebf\u5904\u7406<\/span><br \/>\n<span class=\"token keyword\">void<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">updateMap<\/span><span class=\"token punctuation\">(<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> sensor_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>LaserScan <span class=\"token operator\">&amp;<\/span> scan<span class=\"token punctuation\">,<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> geometry_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Pose <span class=\"token operator\">&amp;<\/span> sensor_pose<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> sensor_yaw<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> sx <span class=\"token operator\">&#061;<\/span> sensor_pose<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> sy <span class=\"token operator\">&#061;<\/span> sensor_pose<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ \u4f20\u611f\u5668\u7f51\u683c\u5750\u6807&#xff08;\u6240\u6709\u5c04\u7ebf\u7684\u8d77\u70b9&#xff09;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> sx_g<span class=\"token punctuation\">,<\/span> sy_g<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token operator\">!<\/span><span class=\"token function\">worldToGrid<\/span><span class=\"token punctuation\">(<\/span>sx<span class=\"token punctuation\">,<\/span> sy<span class=\"token punctuation\">,<\/span> sx_g<span class=\"token punctuation\">,<\/span> sy_g<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">return<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>std<span class=\"token double-colon punctuation\">::<\/span>pair<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;&gt;<\/span> line<span class=\"token punctuation\">;<\/span><br \/>\n  line<span class=\"token punctuation\">.<\/span><span class=\"token function\">reserve<\/span><span class=\"token punctuation\">(<\/span><span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>size_t<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>max_range_ <span class=\"token operator\">\/<\/span> resolution_<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">10<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span>size_t i <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> i <span class=\"token operator\">&lt;<\/span> scan<span class=\"token punctuation\">.<\/span>ranges<span class=\"token punctuation\">.<\/span><span class=\"token function\">size<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">float<\/span> range <span class=\"token operator\">&#061;<\/span> scan<span class=\"token punctuation\">.<\/span>ranges<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">isnan<\/span><span class=\"token punctuation\">(<\/span>range<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">||<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">isinf<\/span><span class=\"token punctuation\">(<\/span>range<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">continue<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>range <span class=\"token operator\">&lt;<\/span> min_range_<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">continue<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">float<\/span> world_angle <span class=\"token operator\">&#061;<\/span> sensor_yaw <span class=\"token operator\">&#043;<\/span> scan<span class=\"token punctuation\">.<\/span>angle_min <span class=\"token operator\">&#043;<\/span> i <span class=\"token operator\">*<\/span> scan<span class=\"token punctuation\">.<\/span>angle_increment<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">bool<\/span> hit_valid <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>range <span class=\"token operator\">&lt;<\/span> scan<span class=\"token punctuation\">.<\/span>range_max <span class=\"token operator\">&amp;&amp;<\/span> range <span class=\"token operator\">&lt;&#061;<\/span> max_range_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">float<\/span> effective_range <span class=\"token operator\">&#061;<\/span> hit_valid <span class=\"token operator\">?<\/span> range <span class=\"token operator\">:<\/span> max_range_<span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token comment\">\/\/ \u7ec8\u70b9\u4e16\u754c\u5750\u6807<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> ex <span class=\"token operator\">&#061;<\/span> sx <span class=\"token operator\">&#043;<\/span> effective_range <span class=\"token operator\">*<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">cos<\/span><span class=\"token punctuation\">(<\/span>world_angle<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> ey <span class=\"token operator\">&#061;<\/span> sy <span class=\"token operator\">&#043;<\/span> effective_range <span class=\"token operator\">*<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">sin<\/span><span class=\"token punctuation\">(<\/span>world_angle<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token keyword\">int<\/span> ex_g<span class=\"token punctuation\">,<\/span> ey_g<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token function\">worldToGrid<\/span><span class=\"token punctuation\">(<\/span>ex<span class=\"token punctuation\">,<\/span> ey<span class=\"token punctuation\">,<\/span> ex_g<span class=\"token punctuation\">,<\/span> ey_g<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    ex_g <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span>ex_g<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> width_ <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    ey_g <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span>ey_g<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> height_ <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token comment\">\/\/ Bresenham \u5149\u7ebf\u8ffd\u8e2a<\/span><br \/>\n    <span class=\"token function\">bresenhamLine<\/span><span class=\"token punctuation\">(<\/span>sx_g<span class=\"token punctuation\">,<\/span> sy_g<span class=\"token punctuation\">,<\/span> ex_g<span class=\"token punctuation\">,<\/span> ey_g<span class=\"token punctuation\">,<\/span> line<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token comment\">\/\/ \u7a7f\u8fc7\u70b9 \u2192 FREE \u66f4\u65b0&#xff08;\u4e0d\u542b\u7ec8\u70b9&#xff09;<\/span><br \/>\n    <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span>size_t j <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> j <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">1<\/span> <span class=\"token operator\">&lt;<\/span> line<span class=\"token punctuation\">.<\/span><span class=\"token function\">size<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>j<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      <span class=\"token keyword\">int<\/span> idx <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">gridIndex<\/span><span class=\"token punctuation\">(<\/span>line<span class=\"token punctuation\">[<\/span>j<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">.<\/span>first<span class=\"token punctuation\">,<\/span> line<span class=\"token punctuation\">[<\/span>j<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">.<\/span>second<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      log_odds_<span class=\"token punctuation\">[<\/span>idx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span>log_odds_<span class=\"token punctuation\">[<\/span>idx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#043;<\/span> l_free_<span class=\"token punctuation\">,<\/span><br \/>\n                                   log_odds_min_<span class=\"token punctuation\">,<\/span> log_odds_max_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><\/p>\n<p>    <span class=\"token comment\">\/\/ \u547d\u4e2d\u70b9 \u2192 OCCUPIED \u66f4\u65b0&#xff08;\u4ec5\u5f53\u5149\u675f\u5b9e\u9645\u51fb\u4e2d&#xff0c;\u975e max_range&#xff09;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>hit_valid <span class=\"token operator\">&amp;&amp;<\/span> <span class=\"token operator\">!<\/span>line<span class=\"token punctuation\">.<\/span><span class=\"token function\">empty<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      <span class=\"token keyword\">auto<\/span> <span class=\"token operator\">&amp;<\/span> ep <span class=\"token operator\">&#061;<\/span> line<span class=\"token punctuation\">.<\/span><span class=\"token function\">back<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">int<\/span> idx <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">gridIndex<\/span><span class=\"token punctuation\">(<\/span>ep<span class=\"token punctuation\">.<\/span>first<span class=\"token punctuation\">,<\/span> ep<span class=\"token punctuation\">.<\/span>second<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      log_odds_<span class=\"token punctuation\">[<\/span>idx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span>log_odds_<span class=\"token punctuation\">[<\/span>idx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#043;<\/span> l_occ_<span class=\"token punctuation\">,<\/span><br \/>\n                                   log_odds_min_<span class=\"token punctuation\">,<\/span> log_odds_max_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>\u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;\u5bf9\u6bcf\u4e00\u5e27 LaserScan&#xff0c;\u7528 TF \u67e5\u5230\u4f20\u611f\u5668\u5728 map \u7cfb\u4e0b\u7684\u4f4d\u59ff&#xff0c;\u7136\u540e\u6bcf\u6761\u5c04\u7ebf \u2192 Bresenham \u753b\u7ebf \u2192 \u7a7f\u8fc7 cell \u52a0 l_free&#xff08;\u7a7a\u95f2\u8bc1\u636e&#xff09;\u2192 \u547d\u4e2d cell \u52a0 l_occ&#xff08;\u5360\u636e\u8bc1\u636e&#xff09;\u2192 \u94b3\u4f4d\u5728 [-4, 4] \u9632\u6b62\u6781\u7aef\u503c<\/li>\n<\/ul>\n<h5>2-5 \u53cc Topic \u53d1\u5e03&#xff1a;\u4e09\u503c\u5730\u56fe &#043; \u6982\u7387\u5730\u56fe<\/h5>\n<ul>\n<li>ProbabilityMapper \u540c\u65f6\u53d1\u5e03 \u4e09\u4e2a Topic&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>Topic\u6570\u636e\u7c7b\u578b\u503c\u8303\u56f4\u7528\u9014<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">\/prob_map<\/td>\n<td align=\"left\">OccupancyGrid<\/td>\n<td align=\"left\">[0, 100] \u8fde\u7eed<\/td>\n<td align=\"left\">MI \u51b3\u7b56 &#043; RViz \u7070\u5ea6\u70ed\u529b\u56fe<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\/thresholded_map<\/td>\n<td align=\"left\">OccupancyGrid<\/td>\n<td align=\"left\">-1 \/ 0 \/ 100<\/td>\n<td align=\"left\">Nav2 \u8def\u5f84\u89c4\u5212&#xff08;\u5168\u5c40\u89c4\u5212\u5668&#xff09;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\/entropy_map<\/td>\n<td align=\"left\">OccupancyGrid<\/td>\n<td align=\"left\">[0, 100] \u71b5\u503c<\/td>\n<td align=\"left\">RViz \u4e0d\u786e\u5b9a\u6027\u53ef\u89c6\u5316<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li>\n<p>\u5168\u5c40\u4e09\u503c\u5730\u56fe <img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160850-6a6a25921b106.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<\/li>\n<li>\n<p>\u6982\u7387\u5730\u56fe&#xff08;\u7070\u5ea6\u8272\u677f map&#xff09;&#xff1a; * \u767d\u8272 \/ \u6d45\u7070&#xff1a;\u6982\u7387\u8d8b\u8fd1 0&#xff0c;\u786e\u5b9a\u7a7a\u95f2\u2014\u2014\u5df2\u88ab\u591a\u6b21\u6fc0\u5149\u7a7f\u8fc7&#xff0c;\u51e0\u4e4e\u6ca1\u6709\u4e0d\u786e\u5b9a\u6027 * \u6df1\u9ed1&#xff1a;\u6982\u7387\u8d8b\u8fd1 1&#xff0c;\u786e\u5b9a\u5360\u636e\u2014\u2014\u6fc0\u5149\u53cd\u590d\u547d\u4e2d&#xff0c;\u786e\u5b9a\u4e3a\u969c\u788d\u7269 * \u7070\u8272\u6e10\u53d8\u5e26&#xff1a;\u6982\u7387\u5728 0.3~0.7 \u4e4b\u95f4&#xff0c;\u4e0d\u786e\u5b9a\u2014\u2014\u524d\u6cbf\u8fb9\u7f18&#xff0c;\u53ea\u6709\u5c11\u91cf\u6fc0\u5149\u626b\u8fc7&#xff0c;\u6b63\u662f MI \u7b97\u6cd5\u8981\u53bb&#034;\u6d88\u9664&#034;\u7684\u5730\u65b9 <img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160851-6a6a25930b7d8.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<\/li>\n<li>\n<p>\u71b5\u5730\u56fe&#xff08;costmap \u8272\u677f&#xff0c;\u63a8\u8350&#xff09;&#xff1a;<\/p>\n<ul>\n<li>\u84dd\u8272 \/ \u6df1\u84dd&#xff1a;\u71b5\u8d8b\u8fd1 0&#xff0c;\u4f4e\u4e0d\u786e\u5b9a\u6027\u2014\u2014\u5df2\u63a2\u7d22\u533a\u57df\u6216\u969c\u788d\u7269\u5185\u90e8&#xff0c;\u5df2\u5145\u5206\u6536\u655b<\/li>\n<li>\u9752\u8272 \/ \u7eff\u8272&#xff1a;\u71b5 0.3~0.7&#xff0c;\u4e2d\u7b49\u4e0d\u786e\u5b9a\u6027\u2014\u2014\u524d\u6cbf\u540e\u65b9&#xff0c;\u6fc0\u5149\u626b\u8fc7\u7684\u6b21\u6570\u8fd8\u4e0d\u591f\u591a<\/li>\n<li>\u9ec4\u8272 \/ \u6a59\u7ea2&#xff1a;\u71b5\u8d8b\u8fd1 1&#xff0c;\u9ad8\u4e0d\u786e\u5b9a\u6027\u2014\u2014\u524d\u6cbf\u8fb9\u7f18\u548c\u672a\u77e5\u533a\u57df\u4ea4\u754c\u5904&#xff0c;MI \u63a2\u7d22\u7684\u6838\u5fc3\u76ee\u6807<\/li>\n<li>\u5207\u6362\u65b9\u5f0f&#xff1a;RViz \u5de6\u4fa7\u9762\u677f \u2192 \/entropy_map \u7684 Map Display \u2192 Color Scheme \u4ece map \u6539\u4e3a costmap <img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160852-6a6a2594ad436.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/li>\n<\/ul>\n<\/li>\n<li>\n<p>\u53d1\u5e03\u4ee3\u7801&#xff1a;<\/p>\n<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ \u5bf9\u5e94 2-5 \u8282&#xff1a;\u4e09\u5f20\u5730\u56fe\u7684\u53d1\u5e03<\/span><br \/>\n<span class=\"token keyword\">void<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">publishMaps<\/span><span class=\"token punctuation\">(<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span>Time <span class=\"token operator\">&amp;<\/span> stamp<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">const<\/span> std<span class=\"token double-colon punctuation\">::<\/span>string <span class=\"token operator\">&amp;<\/span> frame_id<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">int<\/span> total <span class=\"token operator\">&#061;<\/span> width_ <span class=\"token operator\">*<\/span> height_<span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ 1. \u9608\u503c\u5316\u5730\u56fe&#xff08;\u7ed9 Nav2 \u7528&#xff09;<\/span><br \/>\n  <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> i <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> i <span class=\"token operator\">&lt;<\/span> total<span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> p <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">logOddsToProb<\/span><span class=\"token punctuation\">(<\/span>log_odds_<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">&gt;<\/span> <span class=\"token number\">0.70<\/span><span class=\"token punctuation\">)<\/span>        msg<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">100<\/span><span class=\"token punctuation\">;<\/span>   <span class=\"token comment\">\/\/ \u5360\u636e<\/span><br \/>\n    <span class=\"token keyword\">else<\/span> <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">&lt;<\/span> <span class=\"token number\">0.30<\/span><span class=\"token punctuation\">)<\/span>   msg<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span>     <span class=\"token comment\">\/\/ \u7a7a\u95f2<\/span><br \/>\n    <span class=\"token keyword\">else<\/span>                  msg<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">;<\/span>    <span class=\"token comment\">\/\/ \u672a\u77e5<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ 2. \u8fde\u7eed\u6982\u7387\u5730\u56fe&#xff08;\u7ed9 MI \u7528&#xff09;<\/span><br \/>\n  <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> i <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> i <span class=\"token operator\">&lt;<\/span> total<span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> p <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">logOddsToProb<\/span><span class=\"token punctuation\">(<\/span>log_odds_<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    msg<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int8_t<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">round<\/span><span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">*<\/span> <span class=\"token number\">100.0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span>  <span class=\"token comment\">\/\/ 0~100<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ 3. \u9999\u519c\u71b5\u5730\u56fe&#xff08;\u7ed9 RViz \u770b&#xff09;<\/span><br \/>\n  <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> i <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> i <span class=\"token operator\">&lt;<\/span> total<span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> p <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">logOddsToProb<\/span><span class=\"token punctuation\">(<\/span>log_odds_<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> h <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">entropy<\/span><span class=\"token punctuation\">(<\/span>p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span>  <span class=\"token comment\">\/\/ max &#061; 1.0 at p &#061; 0.5<\/span><br \/>\n    msg<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int8_t<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">round<\/span><span class=\"token punctuation\">(<\/span>h <span class=\"token operator\">*<\/span> <span class=\"token number\">100.0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>\u8fd9\u91cc\u7528 transient_local QoS&#xff08;\u7c7b\u4f3c ROS1 \u7684 latching&#xff09;&#xff0c;\u786e\u4fdd RViz \u548c Nav2 \u5ef6\u8fdf\u8ba2\u9605\u65f6\u4e5f\u80fd\u62ff\u5230\u6700\u65b0\u4e00\u5e27<\/li>\n<\/ul>\n<hr \/>\n<h3>3 \u57fa\u4e8e\u4e92\u4fe1\u606f\u7684\u524d\u6cbf\u9009\u62e9<\/h3>\n<h5>3-1 \u9999\u519c\u71b5<\/h5>\n<ul>\n<li>\u6709\u4e86\u6982\u7387\u5730\u56fe&#xff0c;\u6bcf\u4e2a cell \u7684\u4e0d\u786e\u5b9a\u6027\u53ef\u4ee5\u7528 \u9999\u519c\u71b5&#xff08;Shannon Entropy&#xff09;\u6765\u91cf\u5316&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         H<\/p>\n<p>         (<\/p>\n<p>         c<\/p>\n<p>         e<\/p>\n<p>         l<\/p>\n<p>         l<\/p>\n<p>         )<\/p>\n<p>         &#061;<\/p>\n<p>         \u2212<\/p>\n<p>         P<\/p>\n<p>         (<\/p>\n<p>         m<\/p>\n<p>         )<\/p>\n<p>         \u22c5<\/p>\n<p>           log<\/p>\n<p>           \u2061<\/p>\n<p>          2<\/p>\n<p>         P<\/p>\n<p>         (<\/p>\n<p>         m<\/p>\n<p>         )<\/p>\n<p>         \u2212<\/p>\n<p>         (<\/p>\n<p>         1<\/p>\n<p>         \u2212<\/p>\n<p>         P<\/p>\n<p>         (<\/p>\n<p>         m<\/p>\n<p>         )<\/p>\n<p>         )<\/p>\n<p>         \u22c5<\/p>\n<p>           log<\/p>\n<p>           \u2061<\/p>\n<p>          2<\/p>\n<p>         (<\/p>\n<p>         1<\/p>\n<p>         \u2212<\/p>\n<p>         P<\/p>\n<p>         (<\/p>\n<p>         m<\/p>\n<p>         )<\/p>\n<p>         )<\/p>\n<p>        H(cell) &#061; -P(m) \\\\cdot \\\\log_2 P(m) &#8211; (1 &#8211; P(m)) \\\\cdot \\\\log_2(1 &#8211; P(m))<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0813em\">H<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">ce<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord\">\u2212<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u22c5<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mop\"><span class=\"mop\">lo<span style=\"margin-right: 0.0139em\">g<\/span><\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.207em\"><span class=\"\" style=\"top: -2.4559em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\">2<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2441em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mopen\">(<\/span><span class=\"mord\">1<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mclose\">))<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u22c5<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mop\"><span class=\"mop\">lo<span style=\"margin-right: 0.0139em\">g<\/span><\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.207em\"><span class=\"\" style=\"top: -2.4559em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\">2<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2441em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mopen\">(<\/span><span class=\"mord\">1<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">m<\/span><span class=\"mclose\">))<\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\u4ee3\u7801\u5b9e\u73b0&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ \u5bf9\u5e94 3-1 \u8282&#xff1a;\u9999\u519c\u71b5<\/span><br \/>\n<span class=\"token keyword\">static<\/span> <span class=\"token keyword\">double<\/span> <span class=\"token function\">entropy<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> p<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">&lt;&#061;<\/span> <span class=\"token number\">1e-6<\/span> <span class=\"token operator\">||<\/span> p <span class=\"token operator\">&gt;&#061;<\/span> <span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1e-6<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">return<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">*<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">log2<\/span><span class=\"token punctuation\">(<\/span>p<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#8211;<\/span> p<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">log2<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#8211;<\/span> p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<h5>3-2 \u4e92\u4fe1\u606f &#061; \u9884\u671f\u71b5\u51cf<\/h5>\n<ul>\n<li>MI \u63a2\u7d22\u7684\u6838\u5fc3\u601d\u8def&#xff1a;\u5982\u679c\u6211\u53bb\u4f4d\u7f6e x&#xff0c;\u6211\u9884\u671f\u80fd\u6d88\u9664\u591a\u5c11\u4e0d\u786e\u5b9a\u6027&#xff1f;<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         I<\/p>\n<p>         (<\/p>\n<p>         M<\/p>\n<p>         ;<\/p>\n<p>         Z<\/p>\n<p>         \u2223<\/p>\n<p>         x<\/p>\n<p>         )<\/p>\n<p>         &#061;<\/p>\n<p>         H<\/p>\n<p>         (<\/p>\n<p>         M<\/p>\n<p>         )<\/p>\n<p>         \u2212<\/p>\n<p>         H<\/p>\n<p>         (<\/p>\n<p>         M<\/p>\n<p>         \u2223<\/p>\n<p>         Z<\/p>\n<p>         ,<\/p>\n<p>         x<\/p>\n<p>         )<\/p>\n<p>        I(M; Z \\\\mid x) &#061; H(M) &#8211; H(M \\\\mid Z, x)<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0785em\">I<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.109em\">M<\/span><span class=\"mpunct\">;<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0715em\">Z<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\">x<\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0813em\">H<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.109em\">M<\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0813em\">H<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.109em\">M<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0715em\">Z<\/span><span class=\"mpunct\">,<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord mathnormal\">x<\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\u5176\u4e2d <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\">\n<p>         H<\/p>\n<p>         (<\/p>\n<p>         M<\/p>\n<p>         )<\/p>\n<p>        H(M)<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0813em\">H<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.109em\">M<\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><\/span> \u662f\u5f53\u524d\u5730\u56fe\u7684\u603b\u71b5&#xff0c;<span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         H<\/p>\n<p>         (<\/p>\n<p>         M<\/p>\n<p>         \u2223<\/p>\n<p>         Z<\/p>\n<p>         ,<\/p>\n<p>         x<\/p>\n<p>         )<\/p>\n<p>        H(M \\\\mid Z, x)<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0813em\">H<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.109em\">M<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2223<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0715em\">Z<\/span><span class=\"mpunct\">,<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord mathnormal\">x<\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><\/span> \u662f\u5047\u8bbe\u4ece x \u5904\u89c2\u6d4b\u540e\u7684&#034;\u4e8b\u540e&#034;\u71b5<\/li>\n<li>\u76f4\u63a5\u7cbe\u786e\u8ba1\u7b97\u8fd9\u4e2a\u503c\u975e\u5e38\u6602\u8d35&#xff08;\u9700\u8981\u5bf9\u6240\u6709\u53ef\u80fd\u7684\u89c2\u6d4b\u505a\u79ef\u5206&#xff09;&#xff0c;\u4f46\u6211\u4eec\u53ef\u4ee5\u7528\u4e00\u4e2a \u5b9e\u7528\u8fd1\u4f3c&#xff1a;<\/li>\n<\/ul>\n<p>\u5728\u5019\u9009\u4f4d\u59ff x \u5904\u6a21\u62df\u4e00\u5957\u865a\u62df\u6fc0\u5149\u626b\u63cf&#xff0c;\u7d2f\u52a0\u6240\u6709&#034;\u53ef\u89c1 cell&#034;\u7684\u71b5&#xff0c;\u4f5c\u4e3a MI \u7684\u8fd1\u4f3c<\/p>\n<ul>\n<li>\u8fd9\u4e2a\u8fd1\u4f3c\u7684\u76f4\u89c9\u662f\u5408\u7406\u7684&#xff1a;\u5982\u679c\u4ece x \u5904\u80fd\u770b\u5230\u5927\u91cf\u9ad8\u71b5 cell&#xff0c;\u90a3\u4e48\u53bb x \u5904&#034;\u5e94\u8be5&#034;\u80fd\u83b7\u5f97\u5927\u91cf\u4fe1\u606f<\/li>\n<\/ul>\n<h5>3-3 \u865a\u62df\u5149\u675f\u6a21\u62df MI<\/h5>\n<ul>\n<li>\u5bf9\u4e8e\u6bcf\u4e2a\u524d\u6cbf\u7684\u8d28\u5fc3 (cx, cy)&#xff0c;\u653e\u7f6e\u4e00\u53f0\u865a\u62df 360 \u4f20\u611f\u5668&#xff0c;\u53d1\u5c04 N&#061;60 \u675f\u5c04\u7ebf&#xff0c;\u6bcf\u6761\u65b9\u5411\u5747\u5300\u5206\u5e03&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>          \u03b8<\/p>\n<p>          i<\/p>\n<p>         &#061;<\/p>\n<p>           2<\/p>\n<p>           \u03c0<\/p>\n<p>           i<\/p>\n<p>          N<\/p>\n<p>         ,<\/p>\n<p>         i<\/p>\n<p>         &#061;<\/p>\n<p>         0<\/p>\n<p>         ,<\/p>\n<p>         1<\/p>\n<p>         ,<\/p>\n<p>         \u2026<\/p>\n<p>         ,<\/p>\n<p>         N<\/p>\n<p>         \u2212<\/p>\n<p>         1<\/p>\n<p>        \\\\theta_i &#061; \\\\frac{2\\\\pi i}{N}, \\\\quad i &#061; 0, 1, \\\\dots, N-1<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.8444em;vertical-align: -0.15em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0278em\">\u03b8<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3117em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0278em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">i<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 2.0225em;vertical-align: -0.686em\"><\/span><span class=\"mord\"><span class=\"mopen nulldelimiter\"><\/span><span class=\"mfrac\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.3365em\"><span class=\"\" style=\"top: -2.314em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.109em\">N<\/span><\/span><\/span><span class=\"\" style=\"top: -3.23em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"frac-line\" style=\"border-bottom-width: 0.04em\"><\/span><\/span><span class=\"\" style=\"top: -3.677em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\">2<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">\u03c0<\/span><span class=\"mord mathnormal\">i<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.686em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mclose nulldelimiter\"><\/span><\/span><span class=\"mpunct\">,<\/span><span class=\"mspace\" style=\"margin-right: 1em\"><\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord mathnormal\">i<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.8778em;vertical-align: -0.1944em\"><\/span><span class=\"mord\">0<\/span><span class=\"mpunct\">,<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord\">1<\/span><span class=\"mpunct\">,<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"minner\">\u2026<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mpunct\">,<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.109em\">N<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.6444em\"><\/span><span class=\"mord\">1<\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\u6bcf\u675f\u5c04\u7ebf\u6267\u884c\u4e00\u6b21 Bresenham \u904d\u5386&#xff1a;\n<ul>\n<li>\u4ece\u8d28\u5fc3\u51fa\u53d1&#xff0c;\u6cbf\u65b9\u5411 (cos \u03b8, sin \u03b8) \u524d\u8fdb<\/li>\n<li>\u9047\u5230 P(occ) &gt; 0.7 \u2192 \u505c\u6b62&#xff08;\u88ab\u969c\u788d\u7269\u906e\u6321&#xff09;<\/li>\n<li>\u5426\u5219 \u2192 \u7d2f\u52a0\u5f53\u524d cell \u7684\u71b5 H(cell)<\/li>\n<li>\u5230\u8fbe max_range&#xff08;8m&#xff09;\u2192 \u505c\u6b62<\/li>\n<\/ul>\n<\/li>\n<li>\u5c06\u6240\u6709\u675f\u7684\u71b5\u7d2f\u52a0\u540e\u518d\u5e73\u5747&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         MI<\/p>\n<p>         (<\/p>\n<p>         c<\/p>\n<p>         a<\/p>\n<p>         n<\/p>\n<p>         d<\/p>\n<p>         i<\/p>\n<p>         d<\/p>\n<p>         a<\/p>\n<p>         t<\/p>\n<p>         e<\/p>\n<p>         )<\/p>\n<p>         &#061;<\/p>\n<p>          1<\/p>\n<p>          N<\/p>\n<p>          \u2211<\/p>\n<p>           i<\/p>\n<p>           &#061;<\/p>\n<p>           0<\/p>\n<p>           N<\/p>\n<p>           \u2212<\/p>\n<p>           1<\/p>\n<p>          \u2211<\/p>\n<p>           c<\/p>\n<p>           e<\/p>\n<p>           l<\/p>\n<p>           l<\/p>\n<p>           \u2208<\/p>\n<p>           b<\/p>\n<p>           e<\/p>\n<p>           a<\/p>\n<p>            m<\/p>\n<p>            i<\/p>\n<p>         H<\/p>\n<p>         (<\/p>\n<p>         c<\/p>\n<p>         e<\/p>\n<p>         l<\/p>\n<p>         l<\/p>\n<p>         )<\/p>\n<p>        \\\\text{MI}(candidate) &#061; \\\\frac{1}{N} \\\\sum_{i&#061;0}^{N-1} \\\\sum_{cell \\\\in beam_i} H(cell)<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord text\"><span class=\"mord\">MI<\/span><\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">c<\/span><span class=\"mord mathnormal\">an<\/span><span class=\"mord mathnormal\">d<\/span><span class=\"mord mathnormal\">i<\/span><span class=\"mord mathnormal\">d<\/span><span class=\"mord mathnormal\">a<\/span><span class=\"mord mathnormal\">t<\/span><span class=\"mord mathnormal\">e<\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 3.2305em;vertical-align: -1.4022em\"><\/span><span class=\"mord\"><span class=\"mopen nulldelimiter\"><\/span><span class=\"mfrac\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.3214em\"><span class=\"\" style=\"top: -2.314em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.109em\">N<\/span><\/span><\/span><span class=\"\" style=\"top: -3.23em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"frac-line\" style=\"border-bottom-width: 0.04em\"><\/span><\/span><span class=\"\" style=\"top: -3.677em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\">1<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.686em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mclose nulldelimiter\"><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mop op-limits\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.8283em\"><span class=\"\" style=\"top: -1.8723em;margin-left: 0em\"><span class=\"pstrut\" style=\"height: 3.05em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\">i<\/span><span class=\"mrel mtight\">&#061;<\/span><span class=\"mord mtight\">0<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.05em\"><span class=\"pstrut\" style=\"height: 3.05em\"><\/span><span class=\"\"><span class=\"mop op-symbol large-op\">\u2211<\/span><\/span><\/span><span class=\"\" style=\"top: -4.3em;margin-left: 0em\"><span class=\"pstrut\" style=\"height: 3.05em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.109em\">N<\/span><span class=\"mbin mtight\">\u2212<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.2777em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mop op-limits\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.05em\"><span class=\"\" style=\"top: -1.8479em;margin-left: 0em\"><span class=\"pstrut\" style=\"height: 3.05em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\">ce<\/span><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"mrel mtight\">\u2208<\/span><span class=\"mord mathnormal mtight\">b<\/span><span class=\"mord mathnormal mtight\">e<\/span><span class=\"mord mathnormal mtight\">a<\/span><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\">m<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3281em\"><span class=\"\" style=\"top: -2.357em;margin-left: 0em;margin-right: 0.0714em\"><span class=\"pstrut\" style=\"height: 2.5em\"><\/span><span class=\"sizing reset-size3 size1 mtight\"><span class=\"mord mathnormal mtight\">i<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.143em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.05em\"><span class=\"pstrut\" style=\"height: 3.05em\"><\/span><span class=\"\"><span class=\"mop op-symbol large-op\">\u2211<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.4022em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0813em\">H<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">ce<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0197em\">l<\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<p>\u8bf4\u4eba\u8bdd&#xff1a;\u4ece\u5019\u9009\u4f4d\u59ff&#034;\u770b\u4e00\u5708&#034;&#xff0c;\u770b\u89c1\u7684\u6240\u6709 cell \u7684\u71b5\u52a0\u8d77\u6765 \u00f7 \u5c04\u7ebf\u6570\u3002\u5982\u679c\u67d0\u4e2a\u65b9\u5411\u5927\u7247\u90fd\u662f\u7070\u8272\u7684\u4e0d\u786e\u5b9a\u533a\u57df&#xff0c;\u90a3 MI \u5c31\u9ad8&#xff1b;\u5982\u679c\u56db\u9762\u90fd\u5df2\u7ecf\u88ab\u626b\u63cf\u8fc7&#xff08;\u9ed1\u767d\u5206\u660e&#xff09;&#xff0c;\u90a3 MI \u5c31\u4f4e\u3002<\/p>\n<ul>\n<li>\u4e0b\u9762\u8fd9\u5f20\u56fe\u628a 3-2 \u548c 3-3 \u7684\u6982\u5ff5\u653e\u5728\u4e86\u4e00\u8d77&#xff1a;<\/li>\n<\/ul>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160853-6a6a2595cbc4c.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<ul>\n<li>\u4ece\u56fe\u4e2d\u53ef\u4ee5\u770b\u5230&#xff1a;\n<ul>\n<li>\u5de6\u56fe&#xff08;\u6982\u7387 &#043; \u5c04\u7ebf&#xff09;&#xff1a;\u5019\u9009\u4f4d\u59ff\u4f4d\u4e8e\u524d\u6cbf\u4ea4\u754c\u5904&#xff0c;\u5c04\u7ebf\u7a7f\u5165\u53f3\u4e0a\u672a\u77e5\u533a\u57df&#xff08;\u7ea2\/\u6a59\u8272&#xff0c;\u9ad8\u71b5 \u2192 \u9ad8 MI&#xff09;&#xff0c;\u5c04\u7ebf\u7a7f\u5165\u5de6\u4e0b\u5df2\u63a2\u7d22\u533a\u57df&#xff08;\u84dd\u8272&#xff0c;\u4f4e\u71b5 \u2192 \u4f4e MI&#xff09;&#xff0c;\u5c04\u7ebf\u649e\u5230\u5899\u58c1\u63d0\u524d\u7ec8\u6b62<\/li>\n<li>\u4e2d\u56fe&#xff08;\u71b5 &#043; \u5c04\u7ebf&#xff09;&#xff1a;\u540c\u7ec4\u5c04\u7ebf\u8986\u5728\u71b5\u70ed\u529b\u56fe\u4e0a&#xff0c;\u4eae\u8272&#xff08;\u9ad8\u71b5&#xff09;\u533a\u57df\u6070\u597d\u662f MI \u60f3\u8981\u6d88\u9664\u7684\u5730\u65b9<\/li>\n<li>\u53f3\u56fe&#xff08;\u71b5\u51fd\u6570\u66f2\u7ebf&#xff09;&#xff1a;<span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\">\n<p>           H<\/p>\n<p>           (<\/p>\n<p>           p<\/p>\n<p>           )<\/p>\n<p>          H(p)<\/p>\n<p>       <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0813em\">H<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\">p<\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><\/span> \u5728 <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>           p<\/p>\n<p>           &#061;<\/p>\n<p>           0.5<\/p>\n<p>          p&#061;0.5<\/p>\n<p>       <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.625em;vertical-align: -0.1944em\"><\/span><span class=\"mord mathnormal\">p<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.6444em\"><\/span><span class=\"mord\">0.5<\/span><\/span><\/span><\/span><\/span> \u5904\u8fbe\u5230\u6700\u5927\u503c 1.0 bit&#xff0c;\u5728 <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>           p<\/p>\n<p>           \u2192<\/p>\n<p>           0<\/p>\n<p>          p \\\\to 0<\/p>\n<p>       <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.625em;vertical-align: -0.1944em\"><\/span><span class=\"mord mathnormal\">p<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2192<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.6444em\"><\/span><span class=\"mord\">0<\/span><\/span><\/span><\/span><\/span> \u6216 <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>           p<\/p>\n<p>           \u2192<\/p>\n<p>           1<\/p>\n<p>          p \\\\to 1<\/p>\n<p>       <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.625em;vertical-align: -0.1944em\"><\/span><span class=\"mord mathnormal\">p<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2192<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.6444em\"><\/span><span class=\"mord\">1<\/span><\/span><\/span><\/span><\/span> \u65f6\u8d8b\u8fd1 0\u2014\u2014\u8fd9\u5c31\u662f\u4e3a\u4ec0\u4e48 MI \u7b97\u6cd5\u5929\u7136\u504f\u5411&#034;\u7070\u8272&#034;\u7684\u4e0d\u786e\u5b9a\u533a\u57df<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<h5>3-4 \u4ee3\u7801\u5b9e\u73b0&#xff1a;MIGoalSelector<\/h5>\n<ul>\n<li>\u5934\u6587\u4ef6\u2014\u2014\u7ee7\u627f GoalSelector&#xff0c;\u989d\u5916\u7f13\u5b58\u6982\u7387\u5730\u56fe&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ exploration\/mi_goal_selector.hpp \u2014\u2014 \u5bf9\u5e94 3-4 \u8282<\/span><br \/>\n<span class=\"token keyword\">class<\/span> <span class=\"token class-name\">MIGoalSelector<\/span> <span class=\"token operator\">:<\/span> <span class=\"token base-clause\"><span class=\"token keyword\">public<\/span> <span class=\"token class-name\">GoalSelector<\/span><\/span> <span class=\"token punctuation\">{<\/span><br \/>\n<span class=\"token keyword\">public<\/span><span class=\"token operator\">:<\/span><br \/>\n  <span class=\"token function\">MIGoalSelector<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token keyword\">default<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">setProbMap<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid <span class=\"token operator\">&amp;<\/span> prob_map<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  FrontierCluster <span class=\"token function\">select<\/span><span class=\"token punctuation\">(<\/span><br \/>\n    <span class=\"token keyword\">const<\/span> std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>FrontierCluster<span class=\"token operator\">&gt;<\/span> <span class=\"token operator\">&amp;<\/span> frontiers<span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token keyword\">const<\/span> geometry_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Pose <span class=\"token operator\">&amp;<\/span> robot_pose<span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token keyword\">const<\/span> nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid <span class=\"token operator\">&amp;<\/span> map<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">override<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p><span class=\"token keyword\">private<\/span><span class=\"token operator\">:<\/span><br \/>\n  <span class=\"token comment\">\/\/ \u4ece (wx, wy) \u6a21\u62df 360\u00b0 \u626b\u63cf&#xff0c;\u7d2f\u52a0\u53ef\u89c1 cell \u71b5<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> <span class=\"token function\">computeMI<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> wx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> wy<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ \u5355\u675f\u5c04\u7ebf\u904d\u5386&#xff0c;\u7d2f\u52a0\u6cbf\u9014\u71b5&#xff08;\u9047\u5230\u5360\u636e cell \u505c\u6b62&#xff09;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> <span class=\"token function\">beamEntropy<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> sx_g<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> sy_g<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> dx_w<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> dy_w<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid prob_map_<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">bool<\/span> prob_map_received_<span class=\"token punctuation\">{<\/span><span class=\"token boolean\">false<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">int<\/span>    num_beams_<span class=\"token punctuation\">{<\/span><span class=\"token number\">60<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span>       <span class=\"token comment\">\/\/ \u6a21\u62df\u626b\u63cf\u7684\u5c04\u7ebf\u6570<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> mi_max_range_<span class=\"token punctuation\">{<\/span><span class=\"token number\">8.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span>   <span class=\"token comment\">\/\/ \u6bcf\u6761\u5c04\u7ebf\u6700\u5927\u8ddd\u79bb<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> occ_threshold_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.7<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span>  <span class=\"token comment\">\/\/ P &gt; \u6b64\u503c \u2192 \u89c6\u4e3a\u5360\u636e&#xff0c;\u5c04\u7ebf\u505c\u6b62<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<ul>\n<li>\u6838\u5fc3\u8ba1\u7b97\u2014\u2014 computeMI() \u548c beamEntropy()&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ \u5bf9\u5e94 3-4 \u8282&#xff1a;\u5355\u675f\u5c04\u7ebf\u7684\u71b5\u7d2f\u52a0<\/span><br \/>\n<span class=\"token keyword\">double<\/span> <span class=\"token class-name\">MIGoalSelector<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">beamEntropy<\/span><span class=\"token punctuation\">(<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> sx_g<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> sy_g<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> dx_w<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> dy_w<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> res <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>resolution<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> ox <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> oy <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">double<\/span> start_wx <span class=\"token operator\">&#061;<\/span> ox <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span>sx_g <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">0.5<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> res<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> start_wy <span class=\"token operator\">&#061;<\/span> oy <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span>sy_g <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">0.5<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> res<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> end_wx <span class=\"token operator\">&#061;<\/span> start_wx <span class=\"token operator\">&#043;<\/span> dx_w <span class=\"token operator\">*<\/span> mi_max_range_<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> end_wy <span class=\"token operator\">&#061;<\/span> start_wy <span class=\"token operator\">&#043;<\/span> dy_w <span class=\"token operator\">*<\/span> mi_max_range_<span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">int<\/span> ex_g <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>end_wx <span class=\"token operator\">&#8211;<\/span> ox<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> res<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> ey_g <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>end_wy <span class=\"token operator\">&#8211;<\/span> oy<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> res<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  ex_g <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span>ex_g<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>width<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  ey_g <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span>ey_g<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>height<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>std<span class=\"token double-colon punctuation\">::<\/span>pair<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;&gt;<\/span> cells<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token function\">bresenhamLine<\/span><span class=\"token punctuation\">(<\/span>sx_g<span class=\"token punctuation\">,<\/span> sy_g<span class=\"token punctuation\">,<\/span> ex_g<span class=\"token punctuation\">,<\/span> ey_g<span class=\"token punctuation\">,<\/span> cells<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">double<\/span> total <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> <span class=\"token keyword\">auto<\/span> <span class=\"token operator\">&amp;<\/span> <span class=\"token punctuation\">[<\/span>mx<span class=\"token punctuation\">,<\/span> my<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">:<\/span> cells<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> p <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">probAt<\/span><span class=\"token punctuation\">(<\/span>mx<span class=\"token punctuation\">,<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> <span class=\"token number\">100.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">&gt;<\/span> occ_threshold_<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">break<\/span><span class=\"token punctuation\">;<\/span>  <span class=\"token comment\">\/\/ \u906e\u6321 \u2192 \u505c\u6b62<\/span><br \/>\n    total <span class=\"token operator\">&#043;&#061;<\/span> <span class=\"token function\">entropy<\/span><span class=\"token punctuation\">(<\/span>p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span>            <span class=\"token comment\">\/\/ \u7d2f\u52a0\u71b5<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> total<span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token comment\">\/\/ \u5bf9\u5e94 3-4 \u8282&#xff1a;360\u00b0 \u6a21\u62df\u626b\u63cf \u2192 \u5e73\u5747 MI<\/span><br \/>\n<span class=\"token keyword\">double<\/span> <span class=\"token class-name\">MIGoalSelector<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">computeMI<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> wx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> wy<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> res <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>resolution<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> ox <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> oy <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">int<\/span> sx_g <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>wx <span class=\"token operator\">&#8211;<\/span> ox<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> res<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> sy_g <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>wy <span class=\"token operator\">&#8211;<\/span> oy<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> res<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token operator\">!<\/span><span class=\"token function\">inBoundsProb<\/span><span class=\"token punctuation\">(<\/span>sx_g<span class=\"token punctuation\">,<\/span> sy_g<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">return<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">double<\/span> mi <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> i <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> i <span class=\"token operator\">&lt;<\/span> num_beams_<span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> angle <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">2.0<\/span> <span class=\"token operator\">*<\/span> M_PI <span class=\"token operator\">*<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> num_beams_<span class=\"token punctuation\">;<\/span><br \/>\n    mi <span class=\"token operator\">&#043;&#061;<\/span> <span class=\"token function\">beamEntropy<\/span><span class=\"token punctuation\">(<\/span>sx_g<span class=\"token punctuation\">,<\/span> sy_g<span class=\"token punctuation\">,<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">cos<\/span><span class=\"token punctuation\">(<\/span>angle<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">sin<\/span><span class=\"token punctuation\">(<\/span>angle<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> mi <span class=\"token operator\">\/<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>num_beams_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span>  <span class=\"token comment\">\/\/ \u5f52\u4e00\u5316<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>\u5bf9\u6bd4\u5f80\u671f\u7684 computeInfoGain()&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u7ef4\u5ea6\u5f80\u671f&#xff08;Counting&#xff09;\u672c\u671f&#xff08;MI&#xff09;<\/tr>\n<tbody>\n<tr>\n<td>\u5730\u56fe\u7c7b\u578b<\/td>\n<td align=\"center\">\u4e09\u503c -1\/0\/100<\/td>\n<td align=\"center\">\u8fde\u7eed\u6982\u7387 [0, 100]<\/td>\n<\/tr>\n<tr>\n<td>\u4fe1\u606f\u5ea6\u91cf<\/td>\n<td align=\"center\">UNKNOWN \u90bb\u5c45\u8ba1\u6570<\/td>\n<td align=\"center\">\u9999\u519c\u71b5\u7d2f\u52a0<\/td>\n<\/tr>\n<tr>\n<td>\u906e\u6321\u8003\u8651<\/td>\n<td align=\"center\">\u65e0<\/td>\n<td align=\"center\">\u6709&#xff08;P &gt; 0.7 \u7684 cell \u505c\u6b62\u5c04\u7ebf&#xff09;<\/td>\n<\/tr>\n<tr>\n<td>\u65b9\u5411\u611f\u77e5<\/td>\n<td align=\"center\">\u65e0&#xff08;\u4ec5 4 \u90bb\u57df&#xff09;<\/td>\n<td align=\"center\">\u6709&#xff08;360\u00b0 60 \u65b9\u5411\u6a21\u62df&#xff09;<\/td>\n<\/tr>\n<tr>\n<td>\u7a7a\u95f4\u8303\u56f4<\/td>\n<td align=\"center\">\u524d\u6cbf cell \u7684 1 \u683c\u90bb\u57df<\/td>\n<td align=\"center\">8m \u534a\u5f84\u6247\u5f62\u533a\u57df<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<h5>3-5 \u6548\u7528\u51fd\u6570\u4e0e\u8def\u5f84\u4ee3\u4ef7<\/h5>\n<ul>\n<li>MI \u672c\u8eab\u53ea\u8861\u91cf&#034;\u5230\u8fbe\u90a3\u4e2a\u4f4d\u7f6e\u540e\u80fd\u770b\u5230\u591a\u5c11\u4e0d\u786e\u5b9a\u533a\u57df&#034;&#xff0c;\u4e0d\u8003\u8651&#034;\u5f00\u8fc7\u53bb\u8981\u82b1\u591a\u5c11\u4ee3\u4ef7&#034;<\/li>\n<li>\u6cbf\u7528 Burgard \u5f0f\u6548\u7528\u51fd\u6570&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>         U<\/p>\n<p>         (<\/p>\n<p>         f<\/p>\n<p>         )<\/p>\n<p>         &#061;<\/p>\n<p>           M<\/p>\n<p>           I<\/p>\n<p>           (<\/p>\n<p>           f<\/p>\n<p>           )<\/p>\n<p>           c<\/p>\n<p>           o<\/p>\n<p>           s<\/p>\n<p>           t<\/p>\n<p>           (<\/p>\n<p>           r<\/p>\n<p>           o<\/p>\n<p>           b<\/p>\n<p>           o<\/p>\n<p>           t<\/p>\n<p>           \u2192<\/p>\n<p>           f<\/p>\n<p>           )<\/p>\n<p>        U(f) &#061; \\\\frac{MI(f)}{cost(robot \\\\to f)}<\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.109em\">U<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1076em\">f<\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 2.363em;vertical-align: -0.936em\"><\/span><span class=\"mord\"><span class=\"mopen nulldelimiter\"><\/span><span class=\"mfrac\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.427em\"><span class=\"\" style=\"top: -2.314em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\">cos<\/span><span class=\"mord mathnormal\">t<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0278em\">r<\/span><span class=\"mord mathnormal\">o<\/span><span class=\"mord mathnormal\">b<\/span><span class=\"mord mathnormal\">o<\/span><span class=\"mord mathnormal\">t<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2192<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1076em\">f<\/span><span class=\"mclose\">)<\/span><\/span><\/span><span class=\"\" style=\"top: -3.23em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"frac-line\" style=\"border-bottom-width: 0.04em\"><\/span><\/span><span class=\"\" style=\"top: -3.677em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.109em\">M<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0785em\">I<\/span><span class=\"mopen\">(<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1076em\">f<\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.936em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mclose nulldelimiter\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\u8def\u5f84\u4ee3\u4ef7\u7528 Dijkstra \u6ce2\u524d\u4f20\u64ad \u5728\u9608\u503c\u5316\u5730\u56fe\u4e0a\u8ba1\u7b97&#xff08;8 \u8fde\u901a&#xff0c;\u5141\u8bb8\u7a7f\u8d8a FREE \u548c UNKNOWN cell&#xff09;&#xff0c;\u4e0e UtilityGoalSelector \u5b8c\u5168\u4e00\u81f4&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ \u5bf9\u5e94 3-5 \u8282&#xff1a;select() \u4e3b\u5faa\u73af<\/span><br \/>\n<span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span>size_t i <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> i <span class=\"token operator\">&lt;<\/span> frontiers<span class=\"token punctuation\">.<\/span><span class=\"token function\">size<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> mi <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">computeMI<\/span><span class=\"token punctuation\">(<\/span>frontiers<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">.<\/span>centroid<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">,<\/span> frontiers<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">.<\/span>centroid<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> nav_cost <span class=\"token operator\">&#061;<\/span> <span class=\"token comment\">\/* Dijkstra \u8ddd\u79bb *\/<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> utility <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>nav_cost <span class=\"token operator\">&gt;<\/span> <span class=\"token number\">1e-6<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">?<\/span> mi <span class=\"token operator\">\/<\/span> nav_cost <span class=\"token operator\">:<\/span> mi<span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>utility <span class=\"token operator\">&gt;<\/span> best_utility<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    best_utility <span class=\"token operator\">&#061;<\/span> utility<span class=\"token punctuation\">;<\/span><br \/>\n    best_idx <span class=\"token operator\">&#061;<\/span> i<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<hr \/>\n<h3>4 \u7cfb\u7edf\u96c6\u6210<\/h3>\n<h5>4-1 \u9879\u76ee\u7ed3\u6784<\/h5>\n<ul>\n<li>\u672c\u671f\u65b0\u589e\u4e86 prob_map \u5305&#xff0c;\u5e76\u5728 exploration \u5305\u4e2d\u6269\u5c55\u4e86 MI \u9009\u62e9\u5668\u3002\u4e24\u4e2a\u5305\u7684\u5b8c\u6574\u6587\u4ef6\u6811\u5982\u4e0b&#xff1a;<\/li>\n<\/ul>\n<p>ros2_ws\/src\/<br \/>\n\u251c\u2500\u2500 prob_map\/                        # \u65b0\u5efa&#xff1a;\u6982\u7387\u5730\u56fe\u5305<br \/>\n\u2502   \u251c\u2500\u2500 CMakeLists.txt<br \/>\n\u2502   \u251c\u2500\u2500 package.xml<br \/>\n\u2502   \u251c\u2500\u2500 include\/prob_map\/<br \/>\n\u2502   \u2502   \u2514\u2500\u2500 probability_mapper.hpp   # ProbabilityMapper \u7c7b\u58f0\u660e<br \/>\n\u2502   \u251c\u2500\u2500 src\/<br \/>\n\u2502   \u2502   \u251c\u2500\u2500 probability_mapper.cpp   # \u6838\u5fc3&#xff1a;log-odds \u66f4\u65b0 &#043; \u5149\u7ebf\u8ffd\u8e2a &#043; \u4e09 topic \u53d1\u5e03<br \/>\n\u2502   \u2502   \u2514\u2500\u2500 mapper_main.cpp          # \u5165\u53e3<br \/>\n\u2502   \u2514\u2500\u2500 launch\/<br \/>\n\u2502       \u251c\u2500\u2500 prob_mapper.launch.py          # \u5355\u72ec\u542f\u52a8 prob_mapper<br \/>\n\u2502       \u2514\u2500\u2500 slam_prob_mapper.launch.py     # SLAM &#043; prob_mapper &#043; RViz \u8054\u5408\u542f\u52a8<br \/>\n\u2502<br \/>\n\u2514\u2500\u2500 exploration\/                     # \u6269\u5c55\u73b0\u6709\u5305<br \/>\n    \u251c\u2500\u2500 CMakeLists.txt               # \u65b0\u589e mi_goal_selector.cpp \u7f16\u8bd1<br \/>\n    \u251c\u2500\u2500 package.xml<br \/>\n    \u251c\u2500\u2500 include\/exploration\/<br \/>\n    \u2502   \u251c\u2500\u2500 frontier_detector.hpp    # &#xff08;\u4e0d\u53d8&#xff09;\u524d\u6cbf\u68c0\u6d4b<br \/>\n    \u2502   \u251c\u2500\u2500 goal_selector.hpp        # &#xff08;\u4e0d\u53d8&#xff09;\u9009\u62e9\u5668\u57fa\u7c7b &#043; Nearest\/Utility\/RIG<br \/>\n    \u2502   \u251c\u2500\u2500 mi_goal_selector.hpp     # \u65b0\u589e&#xff1a;MIGoalSelector \u7c7b\u58f0\u660e<br \/>\n    \u2502   \u2514\u2500\u2500 exploration_node.hpp     # \u65b0\u589e prob_map \u8ba2\u9605 &#043; mi \u5206\u652f<br \/>\n    \u2514\u2500\u2500 src\/<br \/>\n        \u251c\u2500\u2500 frontier_detector.cpp    # &#xff08;\u4e0d\u53d8&#xff09;<br \/>\n        \u251c\u2500\u2500 goal_selector.cpp        # &#xff08;\u4e0d\u53d8&#xff09;<br \/>\n        \u251c\u2500\u2500 mi_goal_selector.cpp     # \u65b0\u589e&#xff1a;\u865a\u62df\u5149\u675f MI \u8ba1\u7b97 &#043; select()<br \/>\n        \u251c\u2500\u2500 exploration_node.cpp     # \u65b0\u589e probMapCallback &#043; mi \u9009\u62e9\u5668\u96c6\u6210<br \/>\n        \u2514\u2500\u2500 explorer_main.cpp        # &#xff08;\u4e0d\u53d8&#xff09;\u5165\u53e3<\/p>\n<ul>\n<li>\u65b0\u589e\u6587\u4ef6\u4e00\u89c8&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u5305\u6587\u4ef6\u804c\u8d23<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">prob_map<\/td>\n<td align=\"left\">probability_mapper.hpp\/.cpp<\/td>\n<td align=\"left\">\u6982\u7387\u5efa\u56fe\u6838\u5fc3\u7c7b<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">prob_map<\/td>\n<td align=\"left\">mapper_main.cpp<\/td>\n<td align=\"left\">\u72ec\u7acb\u8282\u70b9\u5165\u53e3<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">prob_map<\/td>\n<td align=\"left\">launch\/*.launch.py<\/td>\n<td align=\"left\">\u4e24\u4e2a\u542f\u52a8\u6587\u4ef6<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">exploration<\/td>\n<td align=\"left\">mi_goal_selector.hpp\/.cpp<\/td>\n<td align=\"left\">MI \u76ee\u6807\u9009\u62e9\u5668<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">exploration<\/td>\n<td align=\"left\">exploration_node.hpp\/.cpp<\/td>\n<td align=\"left\">\u6269\u5c55&#xff1a;\u8ba2\u9605 prob_map &#043; mi \u5206\u652f<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li>\u4ee5\u4e0a\u6240\u6709\u6587\u4ef6\u7684\u5b8c\u6574\u6e90\u7801\u89c1 4-5 \u5b8c\u6574\u6e90\u7801\u6e05\u5355&#xff0c;\u53ef\u76f4\u63a5\u590d\u5236\u7c98\u8d34\u5230\u5bf9\u5e94\u4f4d\u7f6e\u540e\u7f16\u8bd1\u8fd0\u884c<\/li>\n<\/ul>\n<h5>4-2 \u63a2\u7d22\u8282\u70b9\u7684\u4fee\u6539<\/h5>\n<ul>\n<li>\n<p>\u5728 ExplorationNode \u4e2d\u65b0\u589e&#xff1a;<\/p>\n<ul>\n<li>\u4e00\u4e2a \/prob_map \u8ba2\u9605\u5668&#xff0c;\u72ec\u7acb\u4e8e \/map \u8ba2\u9605\u5668<\/li>\n<li>\u4e00\u4e2a MIGoalSelector \u7684\u5b9e\u4f8b\u5316\u5206\u652f&#xff08;goal_selector_type &#061;&#061; &#034;mi&#034;&#xff09;<\/li>\n<li>\u5728 selectAndSendGoal() \u4e2d&#xff0c;\u8c03\u7528 select() \u524d\u5c06\u6700\u65b0\u6982\u7387\u5730\u56fe\u6ce8\u5165 MI \u9009\u62e9\u5668<\/li>\n<\/ul>\n<\/li>\n<li>\n<p>\u6784\u9020\u5668\u4e2d\u58f0\u660e\u53c2\u6570&#xff1a;<\/p>\n<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ exploration_node.cpp \u2014\u2014 \u5bf9\u5e94 4-2 \u8282<\/span><br \/>\n<span class=\"token class-name\">ExplorationNode<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">ExplorationNode<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span>NodeOptions <span class=\"token operator\">&amp;<\/span> options<span class=\"token punctuation\">)<\/span><br \/>\n  <span class=\"token operator\">:<\/span> rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">LifecycleNode<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;exploration_node&#034;<\/span><span class=\"token punctuation\">,<\/span> options<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;map_topic&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;\/map&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;prob_map_topic&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;\/prob_map&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span>  <span class=\"token comment\">\/\/ \u65b0\u589e<\/span><br \/>\n  <span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;goal_selector_type&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;nearest&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>on_configure() \u4e2d\u6ce8\u518c MI \u9009\u62e9\u5668&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ \u5bf9\u5e94 4-2 \u8282<\/span><br \/>\n<span class=\"token punctuation\">}<\/span> <span class=\"token keyword\">else<\/span> <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>goal_selector_type_ <span class=\"token operator\">&#061;&#061;<\/span> <span class=\"token string\">&#034;mi&#034;<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n  goal_selector_ <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">make_unique<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>MIGoalSelector<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token function\">RCLCPP_INFO<\/span><span class=\"token punctuation\">(<\/span><span class=\"token function\">get_logger<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;Using MIGoalSelector (mutual information)&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>on_activate() \u4e2d\u8ba2\u9605\u6982\u7387\u5730\u56fe&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ \u5bf9\u5e94 4-2 \u8282&#xff1a;\u72ec\u7acb\u8ba2\u9605\u6982\u7387\u5730\u56fe&#xff0c;transient_local QoS<\/span><br \/>\nprob_map_sub_ <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">create_subscription<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><br \/>\n  prob_map_topic_<span class=\"token punctuation\">,<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">QoS<\/span><span class=\"token punctuation\">(<\/span>rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">KeepLast<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">transient_local<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">reliable<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">bind<\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">&amp;<\/span>ExplorationNode<span class=\"token double-colon punctuation\">::<\/span>probMapCallback<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">this<\/span><span class=\"token punctuation\">,<\/span> std<span class=\"token double-colon punctuation\">::<\/span>placeholders<span class=\"token double-colon punctuation\">::<\/span>_1<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<ul>\n<li>selectAndSendGoal() \u4e2d\u5c06\u6982\u7387\u5730\u56fe\u6ce8\u5165\u518d\u8c03\u7528&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ \u5bf9\u5e94 4-2 \u8282&#xff1a;select \u524d\u6ce8\u5165\u6982\u7387\u5730\u56fe<\/span><br \/>\n<span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>goal_selector_type_ <span class=\"token operator\">&#061;&#061;<\/span> <span class=\"token string\">&#034;mi&#034;<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">auto<\/span> <span class=\"token operator\">*<\/span> mi_sel <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">dynamic_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>MIGoalSelector <span class=\"token operator\">*<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>goal_selector_<span class=\"token punctuation\">.<\/span><span class=\"token function\">get<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>mi_sel<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    mi_sel<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">setProbMap<\/span><span class=\"token punctuation\">(<\/span>last_prob_map_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token keyword\">auto<\/span> selected <span class=\"token operator\">&#061;<\/span> goal_selector_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">select<\/span><span class=\"token punctuation\">(<\/span>frontiers<span class=\"token punctuation\">,<\/span> current_pose_<span class=\"token punctuation\">,<\/span> last_map_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<h5>4-3 \u542f\u52a8\u6d41\u7a0b<\/h5>\n<ul>\n<li>\u4e09\u4e2a\u7ec8\u7aef&#xff0c;\u987a\u5e8f\u542f\u52a8&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\"># \u7ec8\u7aef 1&#xff1a;Gazebo \u4eff\u771f &#043; \u673a\u5668\u4eba<\/span><br \/>\n.\/1_bringup.sh<\/p>\n<p><span class=\"token comment\"># \u7ec8\u7aef 2&#xff1a;SLAM &#043; \u6982\u7387\u5730\u56fe &#043; Nav2 &#043; RViz<\/span><br \/>\n.\/5_prob_map.sh<\/p>\n<p><span class=\"token comment\"># \u7ec8\u7aef 3&#xff1a;MI \u4e92\u4fe1\u606f\u63a2\u7d22<\/span><br \/>\n.\/7_mi_explore.sh<\/p>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160855-6a6a2597b315e.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160856-6a6a25986a939.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<ul>\n<li>5_prob_map.sh \u5185\u90e8\u542f\u52a8\u7684\u8282\u70b9\u62d3\u6251&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u8282\u70b9\u529f\u80fd<\/tr>\n<tbody>\n<tr>\n<td>slam_toolbox<\/td>\n<td>SLAM &#043; \u53d1\u5e03 \/map&#xff08;\u4e09\u503c&#xff09;&#043; \u7ef4\u62a4 map\u2192odom TF<\/td>\n<\/tr>\n<tr>\n<td>prob_mapper<\/td>\n<td>\u6982\u7387\u5efa\u56fe &#043; \u53d1\u5e03 \/prob_map\u3001\/thresholded_map\u3001\/entropy_map<\/td>\n<\/tr>\n<tr>\n<td>Nav2 (planner_server, controller_server, \u2026)<\/td>\n<td>\u6536\u5230\u63a2\u7d22\u76ee\u6807\u540e\u7684\u8def\u5f84\u89c4\u5212\u548c\u8fd0\u52a8\u63a7\u5236<\/td>\n<\/tr>\n<tr>\n<td>rviz2<\/td>\n<td>\u53ef\u89c6\u5316<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li>prob_mapper \u4f9d\u8d56 slam_toolbox \u63d0\u4f9b\u7684 map\u2192laser TF \u6765\u505a\u4f20\u611f\u5668\u5b9a\u4f4d&#xff0c;\u81ea\u5df1\u4e0d\u505a SLAM<\/li>\n<li>prob_map_size&#061;100.0&#xff08;100m \u00d7 100m&#xff09;&#xff0c;max_range&#061;30.0&#xff08;\u5168\u91cf\u7a0b&#xff09;&#xff0c;prob_hit&#061;0.55 \/ prob_miss&#061;0.45&#xff08;\u4fdd\u5b88\u4f20\u611f\u5668\u6a21\u578b&#xff09;<\/li>\n<\/ul>\n<h5>4-4 RViz \u53ef\u89c6\u5316<\/h5>\n<ul>\n<li>RViz \u4e2d\u53ef\u6dfb\u52a0 3 \u4e2a Map Display \u5206\u522b\u67e5\u770b&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u56fe\u5c42Topic\u8272\u677f\u89c6\u89c9\u542b\u4e49<\/tr>\n<tbody>\n<tr>\n<td>\u6982\u7387\u5730\u56fe<\/td>\n<td>\/prob_map<\/td>\n<td align=\"center\">map&#xff08;\u7070\u5ea6&#xff09;<\/td>\n<td>\u9ed1 &#061; \u5360\u636e&#xff0c;\u767d &#061; \u7a7a\u95f2&#xff0c;\u7070 &#061; \u4e0d\u786e\u5b9a<\/td>\n<\/tr>\n<tr>\n<td>\u9608\u503c\u5316\u5730\u56fe<\/td>\n<td>\/thresholded_map<\/td>\n<td align=\"center\">map<\/td>\n<td>\u6807\u51c6\u4e09\u503c\u5360\u636e\u56fe&#xff08;Nav2 \u5b9e\u9645\u7528\u7684&#xff09;<\/td>\n<\/tr>\n<tr>\n<td>\u71b5\u70ed\u529b\u56fe<\/td>\n<td>\/entropy_map<\/td>\n<td align=\"center\">costmap&#xff08;\u63a8\u8350&#xff09;<\/td>\n<td>\u84dd\/\u7eff &#061; \u4f4e\u71b5&#xff0c;\u7ea2 &#061; \u9ad8\u71b5 &#061; \u8be5\u53bb\u63a2\u7d22<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li>\u71b5\u70ed\u529b\u56fe\u7528 costmap \u8272\u677f\u6bd4\u7070\u5ea6\u66f4\u76f4\u89c2\u2014\u2014\u524d\u6cbf\u8fb9\u7f18\u5448\u7ea2\/\u6a59\u8272\u73af\u5e26&#xff0c;\u8868\u660e\u90a3\u91cc\u6709\u5927\u91cf\u4e0d\u786e\u5b9a\u6027\u7b49\u5f85\u6d88\u9664<\/li>\n<\/ul>\n<h5>4-5 \u5b8c\u6574\u6e90\u7801\u6e05\u5355<\/h5>\n<ul>\n<li>\u4ee5\u4e0b\u4e3a\u672c\u6587\u6d89\u53ca\u7684\u5168\u90e8\u6e90\u7801\u6587\u4ef6&#xff0c;\u6309\u6587\u4ef6\u8def\u5f84\u5217\u51fa&#xff0c;\u53ef\u76f4\u63a5\u590d\u5236\u7c98\u8d34\u5230\u5bf9\u5e94\u4f4d\u7f6e\u540e\u7f16\u8bd1\u8fd0\u884c<\/li>\n<\/ul>\n<h6>probability_mapper.hpp<\/h6>\n<p><span class=\"token comment\">\/\/ \u6587\u4ef6: prob_map\/include\/prob_map\/probability_mapper.hpp<\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">ifndef<\/span> <span class=\"token expression\">PROB_MAP__PROBABILITY_MAPPER_HPP_<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">define<\/span> <span class=\"token macro-name\">PROB_MAP__PROBABILITY_MAPPER_HPP_<\/span><\/span><\/p>\n<p><span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;rclcpp\/rclcpp.hpp&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;nav_msgs\/msg\/occupancy_grid.hpp&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;sensor_msgs\/msg\/laser_scan.hpp&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;tf2_ros\/buffer.h&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;tf2_ros\/transform_listener.h&gt;<\/span><\/span><\/p>\n<p><span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;string&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;vector&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;utility&gt;<\/span><\/span><\/p>\n<p><span class=\"token keyword\">namespace<\/span> prob_map<br \/>\n<span class=\"token punctuation\">{<\/span><\/p>\n<p><span class=\"token keyword\">class<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n<span class=\"token keyword\">public<\/span><span class=\"token operator\">:<\/span><br \/>\n  <span class=\"token keyword\">explicit<\/span> <span class=\"token function\">ProbabilityMapper<\/span><span class=\"token punctuation\">(<\/span>rclcpp<span class=\"token double-colon punctuation\">::<\/span>Node <span class=\"token operator\">*<\/span> node<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token operator\">~<\/span><span class=\"token function\">ProbabilityMapper<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token keyword\">default<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p><span class=\"token keyword\">private<\/span><span class=\"token operator\">:<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">laserCallback<\/span><span class=\"token punctuation\">(<\/span>sensor_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>LaserScan<span class=\"token double-colon punctuation\">::<\/span>SharedPtr msg<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">publishTimerCallback<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">initGrid<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">updateMap<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> sensor_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>LaserScan <span class=\"token operator\">&amp;<\/span> scan<span class=\"token punctuation\">,<\/span><br \/>\n                 <span class=\"token keyword\">const<\/span> geometry_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Pose <span class=\"token operator\">&amp;<\/span> sensor_pose<span class=\"token punctuation\">,<\/span><br \/>\n                 <span class=\"token keyword\">double<\/span> sensor_yaw<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">static<\/span> <span class=\"token keyword\">void<\/span> <span class=\"token function\">bresenhamLine<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> x0<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> y0<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> x1<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> y1<span class=\"token punctuation\">,<\/span><br \/>\n                            std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>std<span class=\"token double-colon punctuation\">::<\/span>pair<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;&gt;<\/span> <span class=\"token operator\">&amp;<\/span> out<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">bool<\/span> <span class=\"token function\">worldToGrid<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> wx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> wy<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> <span class=\"token operator\">&amp;<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> <span class=\"token operator\">&amp;<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">gridToWorld<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> my<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> <span class=\"token operator\">&amp;<\/span> wx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> <span class=\"token operator\">&amp;<\/span> wy<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> <span class=\"token function\">gridIndex<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span> <span class=\"token punctuation\">{<\/span> <span class=\"token keyword\">return<\/span> my <span class=\"token operator\">*<\/span> width_ <span class=\"token operator\">&#043;<\/span> mx<span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token keyword\">bool<\/span> <span class=\"token function\">inBounds<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">static<\/span> <span class=\"token keyword\">double<\/span> <span class=\"token function\">logOddsToProb<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> l<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">static<\/span> <span class=\"token keyword\">double<\/span> <span class=\"token function\">probToLogOdds<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">static<\/span> <span class=\"token keyword\">double<\/span> <span class=\"token function\">entropy<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">void<\/span> <span class=\"token function\">populateOccupancyGrid<\/span><span class=\"token punctuation\">(<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid <span class=\"token operator\">&amp;<\/span> grid<span class=\"token punctuation\">,<\/span><br \/>\n                             <span class=\"token keyword\">const<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span>Time <span class=\"token operator\">&amp;<\/span> stamp<span class=\"token punctuation\">,<\/span><br \/>\n                             <span class=\"token keyword\">const<\/span> std<span class=\"token double-colon punctuation\">::<\/span>string <span class=\"token operator\">&amp;<\/span> frame_id<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">publishMaps<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span>Time <span class=\"token operator\">&amp;<\/span> stamp<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">const<\/span> std<span class=\"token double-colon punctuation\">::<\/span>string <span class=\"token operator\">&amp;<\/span> frame_id<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  rclcpp<span class=\"token double-colon punctuation\">::<\/span>Node <span class=\"token operator\">*<\/span> node_<span class=\"token punctuation\">;<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>shared_ptr<span class=\"token operator\">&lt;<\/span>tf2_ros<span class=\"token double-colon punctuation\">::<\/span>Buffer<span class=\"token operator\">&gt;<\/span> tf_buffer_<span class=\"token punctuation\">;<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>shared_ptr<span class=\"token operator\">&lt;<\/span>tf2_ros<span class=\"token double-colon punctuation\">::<\/span>TransformListener<span class=\"token operator\">&gt;<\/span> tf_listener_<span class=\"token punctuation\">;<\/span><\/p>\n<p>  rclcpp<span class=\"token double-colon punctuation\">::<\/span>Subscription<span class=\"token operator\">&lt;<\/span>sensor_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>LaserScan<span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span>SharedPtr laser_sub_<span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span>TimerBase<span class=\"token double-colon punctuation\">::<\/span>SharedPtr publish_timer_<span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span>Publisher<span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span>SharedPtr prob_map_pub_<span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span>Publisher<span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span>SharedPtr thresh_map_pub_<span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span>Publisher<span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span>SharedPtr entropy_map_pub_<span class=\"token punctuation\">;<\/span><\/p>\n<p>  std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span> log_odds_<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> width_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">,<\/span> height_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">double<\/span> map_width_m_<span class=\"token punctuation\">{<\/span><span class=\"token number\">100.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">,<\/span> map_height_m_<span class=\"token punctuation\">{<\/span><span class=\"token number\">100.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">,<\/span> resolution_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.05<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> origin_x_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">,<\/span> origin_y_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> prob_hit_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.55<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">,<\/span> prob_miss_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.45<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> l_occ_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">,<\/span> l_free_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> log_odds_min_<span class=\"token punctuation\">{<\/span><span class=\"token operator\">&#8211;<\/span><span class=\"token number\">4.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">,<\/span> log_odds_max_<span class=\"token punctuation\">{<\/span><span class=\"token number\">4.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> max_range_<span class=\"token punctuation\">{<\/span><span class=\"token number\">30.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">,<\/span> min_range_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.15<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> publish_rate_<span class=\"token punctuation\">{<\/span><span class=\"token number\">5.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">bool<\/span> publish_entropy_<span class=\"token punctuation\">{<\/span><span class=\"token boolean\">true<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  std<span class=\"token double-colon punctuation\">::<\/span>string laser_topic_<span class=\"token punctuation\">{<\/span><span class=\"token string\">&#034;\/scan&#034;<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">,<\/span> map_frame_<span class=\"token punctuation\">{<\/span><span class=\"token string\">&#034;map&#034;<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>string prob_topic_<span class=\"token punctuation\">{<\/span><span class=\"token string\">&#034;\/prob_map&#034;<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">,<\/span> thresh_topic_<span class=\"token punctuation\">{<\/span><span class=\"token string\">&#034;\/thresholded_map&#034;<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>string entropy_topic_<span class=\"token punctuation\">{<\/span><span class=\"token string\">&#034;\/entropy_map&#034;<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">bool<\/span> initialized_<span class=\"token punctuation\">{<\/span><span class=\"token boolean\">false<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> scan_count_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p><span class=\"token punctuation\">}<\/span>  <span class=\"token comment\">\/\/ namespace prob_map<\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">endif<\/span>  <span class=\"token comment\">\/\/ PROB_MAP__PROBABILITY_MAPPER_HPP_<\/span><\/span><\/p>\n<h6>probability_mapper.cpp<\/h6>\n<p><span class=\"token comment\">\/\/ \u6587\u4ef6: prob_map\/src\/probability_mapper.cpp<\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;prob_map\/probability_mapper.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;algorithm&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;cmath&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;limits&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;stdexcept&gt;<\/span><\/span><\/p>\n<p><span class=\"token keyword\">namespace<\/span> prob_map<br \/>\n<span class=\"token punctuation\">{<\/span><\/p>\n<p><span class=\"token keyword\">double<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">logOddsToProb<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> l<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> <span class=\"token number\">1.0<\/span> <span class=\"token operator\">\/<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#043;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">exp<\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">&#8211;<\/span>l<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">double<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">probToLogOdds<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> p<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">log<\/span><span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">\/<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#8211;<\/span> p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">double<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">entropy<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> p<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">&lt;&#061;<\/span> <span class=\"token number\">0.0<\/span> <span class=\"token operator\">||<\/span> p <span class=\"token operator\">&gt;&#061;<\/span> <span class=\"token number\">1.0<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">return<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">*<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">log2<\/span><span class=\"token punctuation\">(<\/span>p<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#8211;<\/span> p<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">log2<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#8211;<\/span> p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">bresenhamLine<\/span><span class=\"token punctuation\">(<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> x0<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> y0<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> x1<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> y1<span class=\"token punctuation\">,<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>std<span class=\"token double-colon punctuation\">::<\/span>pair<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;&gt;<\/span> <span class=\"token operator\">&amp;<\/span> out<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> dx <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">abs<\/span><span class=\"token punctuation\">(<\/span>x1 <span class=\"token operator\">&#8211;<\/span> x0<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> dy <span class=\"token operator\">&#061;<\/span> <span class=\"token operator\">&#8211;<\/span>std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">abs<\/span><span class=\"token punctuation\">(<\/span>y1 <span class=\"token operator\">&#8211;<\/span> y0<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> sx <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>x0 <span class=\"token operator\">&lt;<\/span> x1<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">?<\/span> <span class=\"token number\">1<\/span> <span class=\"token operator\">:<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> sy <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>y0 <span class=\"token operator\">&lt;<\/span> y1<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">?<\/span> <span class=\"token number\">1<\/span> <span class=\"token operator\">:<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> err <span class=\"token operator\">&#061;<\/span> dx <span class=\"token operator\">&#043;<\/span> dy<span class=\"token punctuation\">;<\/span><br \/>\n  out<span class=\"token punctuation\">.<\/span><span class=\"token function\">clear<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">while<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token boolean\">true<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    out<span class=\"token punctuation\">.<\/span><span class=\"token function\">emplace_back<\/span><span class=\"token punctuation\">(<\/span>x0<span class=\"token punctuation\">,<\/span> y0<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>x0 <span class=\"token operator\">&#061;&#061;<\/span> x1 <span class=\"token operator\">&amp;&amp;<\/span> y0 <span class=\"token operator\">&#061;&#061;<\/span> y1<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">break<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">int<\/span> e2 <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">2<\/span> <span class=\"token operator\">*<\/span> err<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>e2 <span class=\"token operator\">&gt;&#061;<\/span> dy<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span> err <span class=\"token operator\">&#043;&#061;<\/span> dy<span class=\"token punctuation\">;<\/span> x0 <span class=\"token operator\">&#043;&#061;<\/span> sx<span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>e2 <span class=\"token operator\">&lt;&#061;<\/span> dx<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span> err <span class=\"token operator\">&#043;&#061;<\/span> dx<span class=\"token punctuation\">;<\/span> y0 <span class=\"token operator\">&#043;&#061;<\/span> sy<span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">ProbabilityMapper<\/span><span class=\"token punctuation\">(<\/span>rclcpp<span class=\"token double-colon punctuation\">::<\/span>Node <span class=\"token operator\">*<\/span> node<span class=\"token punctuation\">)<\/span><br \/>\n  <span class=\"token operator\">:<\/span> <span class=\"token function\">node_<\/span><span class=\"token punctuation\">(<\/span>node<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;map_width_m&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">100.0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;map_height_m&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">100.0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;resolution&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.05<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;prob_hit&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.55<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;prob_miss&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.45<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;max_range&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">30.0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;min_range&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.15<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;publish_rate&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">5.0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;publish_entropy&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token boolean\">true<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;laser_topic&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;\/scan&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;map_frame&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;map&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;prob_topic&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;\/prob_map&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;thresh_topic&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;\/thresholded_map&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;entropy_topic&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;\/entropy_map&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  map_width_m_  <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;map_width_m&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_double<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  map_height_m_ <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;map_height_m&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_double<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  resolution_   <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;resolution&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_double<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  prob_hit_     <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;prob_hit&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_double<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  prob_miss_    <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;prob_miss&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_double<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  max_range_    <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;max_range&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_double<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  min_range_    <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;min_range&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_double<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  publish_rate_ <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;publish_rate&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_double<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  publish_entropy_ <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;publish_entropy&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_bool<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  laser_topic_  <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;laser_topic&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_string<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  map_frame_    <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;map_frame&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_string<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  prob_topic_   <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;prob_topic&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_string<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  thresh_topic_ <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;thresh_topic&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_string<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  entropy_topic_ <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;entropy_topic&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">as_string<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  l_occ_  <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">probToLogOdds<\/span><span class=\"token punctuation\">(<\/span>prob_hit_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  l_free_ <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">probToLogOdds<\/span><span class=\"token punctuation\">(<\/span>prob_miss_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token function\">initGrid<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  tf_buffer_ <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">make_shared<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>tf2_ros<span class=\"token double-colon punctuation\">::<\/span>Buffer<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_clock<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  tf_listener_ <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">make_shared<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>tf2_ros<span class=\"token double-colon punctuation\">::<\/span>TransformListener<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">*<\/span>tf_buffer_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">auto<\/span> latch_qos <span class=\"token operator\">&#061;<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">QoS<\/span><span class=\"token punctuation\">(<\/span>rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">KeepLast<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">transient_local<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">reliable<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  prob_map_pub_ <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token generic-function\"><span class=\"token function\">create_publisher<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><br \/>\n    prob_topic_<span class=\"token punctuation\">,<\/span> latch_qos<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  thresh_map_pub_ <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token generic-function\"><span class=\"token function\">create_publisher<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><br \/>\n    thresh_topic_<span class=\"token punctuation\">,<\/span> latch_qos<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>publish_entropy_<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    entropy_map_pub_ <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token generic-function\"><span class=\"token function\">create_publisher<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><br \/>\n      entropy_topic_<span class=\"token punctuation\">,<\/span> latch_qos<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><\/p>\n<p>  laser_sub_ <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token generic-function\"><span class=\"token function\">create_subscription<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>sensor_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>LaserScan<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><br \/>\n    laser_topic_<span class=\"token punctuation\">,<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">SensorDataQoS<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">bind<\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">&amp;<\/span>ProbabilityMapper<span class=\"token double-colon punctuation\">::<\/span>laserCallback<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">this<\/span><span class=\"token punctuation\">,<\/span> std<span class=\"token double-colon punctuation\">::<\/span>placeholders<span class=\"token double-colon punctuation\">::<\/span>_1<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">auto<\/span> period <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span>chrono<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">duration<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">\/<\/span> publish_rate_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  publish_timer_ <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">create_wall_timer<\/span><span class=\"token punctuation\">(<\/span><br \/>\n    period<span class=\"token punctuation\">,<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">bind<\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">&amp;<\/span>ProbabilityMapper<span class=\"token double-colon punctuation\">::<\/span>publishTimerCallback<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">this<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token function\">RCLCPP_INFO<\/span><span class=\"token punctuation\">(<\/span>node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_logger<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token string\">&#034;ProbabilityMapper: %dx%d (%.2fm x %.2fm) &#064; %.3fm\/cell&#034;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    width_<span class=\"token punctuation\">,<\/span> height_<span class=\"token punctuation\">,<\/span> map_width_m_<span class=\"token punctuation\">,<\/span> map_height_m_<span class=\"token punctuation\">,<\/span> resolution_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">initGrid<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  width_  <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">ceil<\/span><span class=\"token punctuation\">(<\/span>map_width_m_ <span class=\"token operator\">\/<\/span> resolution_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  height_ <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">ceil<\/span><span class=\"token punctuation\">(<\/span>map_height_m_ <span class=\"token operator\">\/<\/span> resolution_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> total_w <span class=\"token operator\">&#061;<\/span> width_ <span class=\"token operator\">*<\/span> resolution_<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> total_h <span class=\"token operator\">&#061;<\/span> height_ <span class=\"token operator\">*<\/span> resolution_<span class=\"token punctuation\">;<\/span><br \/>\n  origin_x_ <span class=\"token operator\">&#061;<\/span> <span class=\"token operator\">&#8211;<\/span>total_w <span class=\"token operator\">\/<\/span> <span class=\"token number\">2.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  origin_y_ <span class=\"token operator\">&#061;<\/span> <span class=\"token operator\">&#8211;<\/span>total_h <span class=\"token operator\">\/<\/span> <span class=\"token number\">2.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  log_odds_<span class=\"token punctuation\">.<\/span><span class=\"token function\">assign<\/span><span class=\"token punctuation\">(<\/span>width_ <span class=\"token operator\">*<\/span> height_<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">bool<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">worldToGrid<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> wx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> wy<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> <span class=\"token operator\">&amp;<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> <span class=\"token operator\">&amp;<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  mx <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>wx <span class=\"token operator\">&#8211;<\/span> origin_x_<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> resolution_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  my <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>wy <span class=\"token operator\">&#8211;<\/span> origin_y_<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> resolution_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> <span class=\"token function\">inBounds<\/span><span class=\"token punctuation\">(<\/span>mx<span class=\"token punctuation\">,<\/span> my<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">gridToWorld<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> my<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> <span class=\"token operator\">&amp;<\/span> wx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> <span class=\"token operator\">&amp;<\/span> wy<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  wx <span class=\"token operator\">&#061;<\/span> origin_x_ <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span>mx <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">0.5<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> resolution_<span class=\"token punctuation\">;<\/span><br \/>\n  wy <span class=\"token operator\">&#061;<\/span> origin_y_ <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span>my <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">0.5<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> resolution_<span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">bool<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">inBounds<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> mx <span class=\"token operator\">&gt;&#061;<\/span> <span class=\"token number\">0<\/span> <span class=\"token operator\">&amp;&amp;<\/span> mx <span class=\"token operator\">&lt;<\/span> width_ <span class=\"token operator\">&amp;&amp;<\/span> my <span class=\"token operator\">&gt;&#061;<\/span> <span class=\"token number\">0<\/span> <span class=\"token operator\">&amp;&amp;<\/span> my <span class=\"token operator\">&lt;<\/span> height_<span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">laserCallback<\/span><span class=\"token punctuation\">(<\/span>sensor_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>LaserScan<span class=\"token double-colon punctuation\">::<\/span>SharedPtr msg<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  geometry_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>TransformStamped tf<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">try<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    tf <span class=\"token operator\">&#061;<\/span> tf_buffer_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">lookupTransform<\/span><span class=\"token punctuation\">(<\/span><br \/>\n      map_frame_<span class=\"token punctuation\">,<\/span> msg<span class=\"token operator\">-&gt;<\/span>header<span class=\"token punctuation\">.<\/span>frame_id<span class=\"token punctuation\">,<\/span> msg<span class=\"token operator\">-&gt;<\/span>header<span class=\"token punctuation\">.<\/span>stamp<span class=\"token punctuation\">,<\/span><br \/>\n      tf2<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">durationFromSec<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.05<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span> <span class=\"token keyword\">catch<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> tf2<span class=\"token double-colon punctuation\">::<\/span>TransformException <span class=\"token operator\">&amp;<\/span> e<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token function\">RCLCPP_WARN_THROTTLE<\/span><span class=\"token punctuation\">(<\/span>node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_logger<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">*<\/span>node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_clock<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">5000<\/span><span class=\"token punctuation\">,<\/span><br \/>\n      <span class=\"token string\">&#034;ProbMapper TF lookup failed: %s&#034;<\/span><span class=\"token punctuation\">,<\/span> e<span class=\"token punctuation\">.<\/span><span class=\"token function\">what<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">return<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><\/p>\n<p>  geometry_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Pose sensor_pose<span class=\"token punctuation\">;<\/span><br \/>\n  sensor_pose<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>x <span class=\"token operator\">&#061;<\/span> tf<span class=\"token punctuation\">.<\/span>transform<span class=\"token punctuation\">.<\/span>translation<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">;<\/span><br \/>\n  sensor_pose<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>y <span class=\"token operator\">&#061;<\/span> tf<span class=\"token punctuation\">.<\/span>transform<span class=\"token punctuation\">.<\/span>translation<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">;<\/span><br \/>\n  sensor_pose<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>z <span class=\"token operator\">&#061;<\/span> tf<span class=\"token punctuation\">.<\/span>transform<span class=\"token punctuation\">.<\/span>translation<span class=\"token punctuation\">.<\/span>z<span class=\"token punctuation\">;<\/span><br \/>\n  sensor_pose<span class=\"token punctuation\">.<\/span>orientation <span class=\"token operator\">&#061;<\/span> tf<span class=\"token punctuation\">.<\/span>transform<span class=\"token punctuation\">.<\/span>rotation<span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">double<\/span> qx <span class=\"token operator\">&#061;<\/span> sensor_pose<span class=\"token punctuation\">.<\/span>orientation<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">,<\/span> qy <span class=\"token operator\">&#061;<\/span> sensor_pose<span class=\"token punctuation\">.<\/span>orientation<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> qz <span class=\"token operator\">&#061;<\/span> sensor_pose<span class=\"token punctuation\">.<\/span>orientation<span class=\"token punctuation\">.<\/span>z<span class=\"token punctuation\">,<\/span> qw <span class=\"token operator\">&#061;<\/span> sensor_pose<span class=\"token punctuation\">.<\/span>orientation<span class=\"token punctuation\">.<\/span>w<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> siny <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">2.0<\/span> <span class=\"token operator\">*<\/span> <span class=\"token punctuation\">(<\/span>qw <span class=\"token operator\">*<\/span> qz <span class=\"token operator\">&#043;<\/span> qx <span class=\"token operator\">*<\/span> qy<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> cosy <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">2.0<\/span> <span class=\"token operator\">*<\/span> <span class=\"token punctuation\">(<\/span>qy <span class=\"token operator\">*<\/span> qy <span class=\"token operator\">&#043;<\/span> qz <span class=\"token operator\">*<\/span> qz<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> sensor_yaw <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">atan2<\/span><span class=\"token punctuation\">(<\/span>siny<span class=\"token punctuation\">,<\/span> cosy<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token function\">updateMap<\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">*<\/span>msg<span class=\"token punctuation\">,<\/span> sensor_pose<span class=\"token punctuation\">,<\/span> sensor_yaw<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token operator\">&#043;&#043;<\/span>scan_count_<span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">updateMap<\/span><span class=\"token punctuation\">(<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> sensor_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>LaserScan <span class=\"token operator\">&amp;<\/span> scan<span class=\"token punctuation\">,<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> geometry_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Pose <span class=\"token operator\">&amp;<\/span> sensor_pose<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> sensor_yaw<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> sx <span class=\"token operator\">&#061;<\/span> sensor_pose<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> sy <span class=\"token operator\">&#061;<\/span> sensor_pose<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">int<\/span> sx_g<span class=\"token punctuation\">,<\/span> sy_g<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token operator\">!<\/span><span class=\"token function\">worldToGrid<\/span><span class=\"token punctuation\">(<\/span>sx<span class=\"token punctuation\">,<\/span> sy<span class=\"token punctuation\">,<\/span> sx_g<span class=\"token punctuation\">,<\/span> sy_g<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">return<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>std<span class=\"token double-colon punctuation\">::<\/span>pair<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;&gt;<\/span> line<span class=\"token punctuation\">;<\/span><br \/>\n  line<span class=\"token punctuation\">.<\/span><span class=\"token function\">reserve<\/span><span class=\"token punctuation\">(<\/span><span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>size_t<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>max_range_ <span class=\"token operator\">\/<\/span> resolution_<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">10<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span>size_t i <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> i <span class=\"token operator\">&lt;<\/span> scan<span class=\"token punctuation\">.<\/span>ranges<span class=\"token punctuation\">.<\/span><span class=\"token function\">size<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">float<\/span> range <span class=\"token operator\">&#061;<\/span> scan<span class=\"token punctuation\">.<\/span>ranges<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">isnan<\/span><span class=\"token punctuation\">(<\/span>range<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">||<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">isinf<\/span><span class=\"token punctuation\">(<\/span>range<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">continue<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>range <span class=\"token operator\">&lt;<\/span> min_range_<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">continue<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">float<\/span> beam_angle <span class=\"token operator\">&#061;<\/span> scan<span class=\"token punctuation\">.<\/span>angle_min <span class=\"token operator\">&#043;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">float<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> scan<span class=\"token punctuation\">.<\/span>angle_increment<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">float<\/span> world_angle <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">float<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>sensor_yaw<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#043;<\/span> beam_angle<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">bool<\/span> hit_valid <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>range <span class=\"token operator\">&lt;<\/span> scan<span class=\"token punctuation\">.<\/span>range_max <span class=\"token operator\">&amp;&amp;<\/span> range <span class=\"token operator\">&lt;&#061;<\/span> max_range_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">float<\/span> effective_range <span class=\"token operator\">&#061;<\/span> hit_valid <span class=\"token operator\">?<\/span> range <span class=\"token operator\">:<\/span> max_range_<span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token keyword\">double<\/span> ex <span class=\"token operator\">&#061;<\/span> sx <span class=\"token operator\">&#043;<\/span> effective_range <span class=\"token operator\">*<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">cos<\/span><span class=\"token punctuation\">(<\/span>world_angle<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> ey <span class=\"token operator\">&#061;<\/span> sy <span class=\"token operator\">&#043;<\/span> effective_range <span class=\"token operator\">*<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">sin<\/span><span class=\"token punctuation\">(<\/span>world_angle<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token keyword\">int<\/span> ex_g<span class=\"token punctuation\">,<\/span> ey_g<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token function\">worldToGrid<\/span><span class=\"token punctuation\">(<\/span>ex<span class=\"token punctuation\">,<\/span> ey<span class=\"token punctuation\">,<\/span> ex_g<span class=\"token punctuation\">,<\/span> ey_g<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    ex_g <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span>ex_g<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> width_ <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    ey_g <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span>ey_g<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> height_ <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token function\">bresenhamLine<\/span><span class=\"token punctuation\">(<\/span>sx_g<span class=\"token punctuation\">,<\/span> sy_g<span class=\"token punctuation\">,<\/span> ex_g<span class=\"token punctuation\">,<\/span> ey_g<span class=\"token punctuation\">,<\/span> line<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span>size_t j <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> j <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">1<\/span> <span class=\"token operator\">&lt;<\/span> line<span class=\"token punctuation\">.<\/span><span class=\"token function\">size<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>j<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      <span class=\"token keyword\">int<\/span> idx <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">gridIndex<\/span><span class=\"token punctuation\">(<\/span>line<span class=\"token punctuation\">[<\/span>j<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">.<\/span>first<span class=\"token punctuation\">,<\/span> line<span class=\"token punctuation\">[<\/span>j<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">.<\/span>second<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      log_odds_<span class=\"token punctuation\">[<\/span>idx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span>log_odds_<span class=\"token punctuation\">[<\/span>idx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#043;<\/span> l_free_<span class=\"token punctuation\">,<\/span><br \/>\n                                   log_odds_min_<span class=\"token punctuation\">,<\/span> log_odds_max_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><\/p>\n<p>    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>hit_valid <span class=\"token operator\">&amp;&amp;<\/span> <span class=\"token operator\">!<\/span>line<span class=\"token punctuation\">.<\/span><span class=\"token function\">empty<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      <span class=\"token keyword\">auto<\/span> <span class=\"token operator\">&amp;<\/span> ep <span class=\"token operator\">&#061;<\/span> line<span class=\"token punctuation\">.<\/span><span class=\"token function\">back<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">int<\/span> idx <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">gridIndex<\/span><span class=\"token punctuation\">(<\/span>ep<span class=\"token punctuation\">.<\/span>first<span class=\"token punctuation\">,<\/span> ep<span class=\"token punctuation\">.<\/span>second<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      log_odds_<span class=\"token punctuation\">[<\/span>idx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span>log_odds_<span class=\"token punctuation\">[<\/span>idx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#043;<\/span> l_occ_<span class=\"token punctuation\">,<\/span><br \/>\n                                   log_odds_min_<span class=\"token punctuation\">,<\/span> log_odds_max_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">populateOccupancyGrid<\/span><span class=\"token punctuation\">(<\/span><br \/>\n  nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid <span class=\"token operator\">&amp;<\/span> grid<span class=\"token punctuation\">,<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span>Time <span class=\"token operator\">&amp;<\/span> stamp<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">const<\/span> std<span class=\"token double-colon punctuation\">::<\/span>string <span class=\"token operator\">&amp;<\/span> frame_id<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  grid<span class=\"token punctuation\">.<\/span>header<span class=\"token punctuation\">.<\/span>stamp <span class=\"token operator\">&#061;<\/span> stamp<span class=\"token punctuation\">;<\/span><br \/>\n  grid<span class=\"token punctuation\">.<\/span>header<span class=\"token punctuation\">.<\/span>frame_id <span class=\"token operator\">&#061;<\/span> frame_id<span class=\"token punctuation\">;<\/span><br \/>\n  grid<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>width  <span class=\"token operator\">&#061;<\/span> width_<span class=\"token punctuation\">;<\/span><br \/>\n  grid<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>height <span class=\"token operator\">&#061;<\/span> height_<span class=\"token punctuation\">;<\/span><br \/>\n  grid<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>resolution <span class=\"token operator\">&#061;<\/span> resolution_<span class=\"token punctuation\">;<\/span><br \/>\n  grid<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>x <span class=\"token operator\">&#061;<\/span> origin_x_<span class=\"token punctuation\">;<\/span><br \/>\n  grid<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>y <span class=\"token operator\">&#061;<\/span> origin_y_<span class=\"token punctuation\">;<\/span><br \/>\n  grid<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>z <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  grid<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>orientation<span class=\"token punctuation\">.<\/span>w <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">1.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">publishMaps<\/span><span class=\"token punctuation\">(<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span>Time <span class=\"token operator\">&amp;<\/span> stamp<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">const<\/span> std<span class=\"token double-colon punctuation\">::<\/span>string <span class=\"token operator\">&amp;<\/span> frame_id<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">int<\/span> total <span class=\"token operator\">&#061;<\/span> width_ <span class=\"token operator\">*<\/span> height_<span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ thresholded map for Nav2<\/span><br \/>\n  <span class=\"token punctuation\">{<\/span><br \/>\n    nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid msg<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token function\">populateOccupancyGrid<\/span><span class=\"token punctuation\">(<\/span>msg<span class=\"token punctuation\">,<\/span> stamp<span class=\"token punctuation\">,<\/span> frame_id<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    msg<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">.<\/span><span class=\"token function\">resize<\/span><span class=\"token punctuation\">(<\/span>total<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> i <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> i <span class=\"token operator\">&lt;<\/span> total<span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      <span class=\"token keyword\">double<\/span> p <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">logOddsToProb<\/span><span class=\"token punctuation\">(<\/span>log_odds_<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">&gt;<\/span> <span class=\"token number\">0.70<\/span><span class=\"token punctuation\">)<\/span>        msg<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">100<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">else<\/span> <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">&lt;<\/span> <span class=\"token number\">0.30<\/span><span class=\"token punctuation\">)<\/span>   msg<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">else<\/span>                  msg<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><br \/>\n    thresh_map_pub_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">publish<\/span><span class=\"token punctuation\">(<\/span>msg<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ continuous probability map for MI<\/span><br \/>\n  <span class=\"token punctuation\">{<\/span><br \/>\n    nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid msg<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token function\">populateOccupancyGrid<\/span><span class=\"token punctuation\">(<\/span>msg<span class=\"token punctuation\">,<\/span> stamp<span class=\"token punctuation\">,<\/span> frame_id<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    msg<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">.<\/span><span class=\"token function\">resize<\/span><span class=\"token punctuation\">(<\/span>total<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> i <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> i <span class=\"token operator\">&lt;<\/span> total<span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      <span class=\"token keyword\">double<\/span> p <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">logOddsToProb<\/span><span class=\"token punctuation\">(<\/span>log_odds_<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      msg<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int8_t<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">round<\/span><span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">*<\/span> <span class=\"token number\">100.0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><br \/>\n    prob_map_pub_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">publish<\/span><span class=\"token punctuation\">(<\/span>msg<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><\/p>\n<p>  <span class=\"token comment\">\/\/ entropy map for RViz<\/span><br \/>\n  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>publish_entropy_ <span class=\"token operator\">&amp;&amp;<\/span> entropy_map_pub_<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid msg<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token function\">populateOccupancyGrid<\/span><span class=\"token punctuation\">(<\/span>msg<span class=\"token punctuation\">,<\/span> stamp<span class=\"token punctuation\">,<\/span> frame_id<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    msg<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">.<\/span><span class=\"token function\">resize<\/span><span class=\"token punctuation\">(<\/span>total<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> i <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> i <span class=\"token operator\">&lt;<\/span> total<span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      <span class=\"token keyword\">double<\/span> p <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">logOddsToProb<\/span><span class=\"token punctuation\">(<\/span>log_odds_<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      msg<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int8_t<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">round<\/span><span class=\"token punctuation\">(<\/span><span class=\"token function\">entropy<\/span><span class=\"token punctuation\">(<\/span>p<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> <span class=\"token number\">100.0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><br \/>\n    entropy_map_pub_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">publish<\/span><span class=\"token punctuation\">(<\/span>msg<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token class-name\">ProbabilityMapper<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">publishTimerCallback<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">auto<\/span> stamp <span class=\"token operator\">&#061;<\/span> node_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_clock<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token operator\">-&gt;<\/span><span class=\"token function\">now<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token function\">publishMaps<\/span><span class=\"token punctuation\">(<\/span>stamp<span class=\"token punctuation\">,<\/span> map_frame_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token punctuation\">}<\/span>  <span class=\"token comment\">\/\/ namespace prob_map<\/span><\/p>\n<h6>mapper_main.cpp<\/h6>\n<p><span class=\"token comment\">\/\/ \u6587\u4ef6: prob_map\/src\/mapper_main.cpp<\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;prob_map\/probability_mapper.hpp&#034;<\/span><\/span><\/p>\n<p><span class=\"token keyword\">int<\/span> <span class=\"token function\">main<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> argc<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">char<\/span> <span class=\"token operator\">*<\/span><span class=\"token operator\">*<\/span> argv<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">init<\/span><span class=\"token punctuation\">(<\/span>argc<span class=\"token punctuation\">,<\/span> argv<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">auto<\/span> node <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">make_shared<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>rclcpp<span class=\"token double-colon punctuation\">::<\/span>Node<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;prob_mapper&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  prob_map<span class=\"token double-colon punctuation\">::<\/span>ProbabilityMapper <span class=\"token function\">mapper<\/span><span class=\"token punctuation\">(<\/span>node<span class=\"token punctuation\">.<\/span><span class=\"token function\">get<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token function\">RCLCPP_INFO<\/span><span class=\"token punctuation\">(<\/span>node<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_logger<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;Probability mapper started. Spinning&#8230;&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">spin<\/span><span class=\"token punctuation\">(<\/span>node<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">shutdown<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<h6>prob_mapper.launch.py<\/h6>\n<p><span class=\"token comment\"># \u6587\u4ef6: prob_map\/launch\/prob_mapper.launch.py<\/span><br \/>\n<span class=\"token comment\"># \u6982\u7387\u5730\u56fe\u5efa\u56fe &#8212; prob_mapper \u72ec\u7acb\u542f\u52a8 &#043; \u53ef\u9009 RViz<\/span><\/p>\n<p><span class=\"token keyword\">from<\/span> launch <span class=\"token keyword\">import<\/span> LaunchDescription<br \/>\n<span class=\"token keyword\">from<\/span> launch<span class=\"token punctuation\">.<\/span>actions <span class=\"token keyword\">import<\/span> DeclareLaunchArgument<br \/>\n<span class=\"token keyword\">from<\/span> launch<span class=\"token punctuation\">.<\/span>conditions <span class=\"token keyword\">import<\/span> IfCondition<br \/>\n<span class=\"token keyword\">from<\/span> launch<span class=\"token punctuation\">.<\/span>substitutions <span class=\"token keyword\">import<\/span> LaunchConfiguration<br \/>\n<span class=\"token keyword\">from<\/span> launch_ros<span class=\"token punctuation\">.<\/span>actions <span class=\"token keyword\">import<\/span> Node<\/p>\n<p><span class=\"token keyword\">def<\/span> <span class=\"token function\">generate_launch_description<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    declare_rviz <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token string\">&#039;rviz&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;true&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        description<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;Launch RViz&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    declare_map_width <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token string\">&#039;map_width_m&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;100.0&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        description<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;Map width in meters&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    declare_map_height <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token string\">&#039;map_height_m&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;100.0&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        description<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;Map height in meters&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    declare_resolution <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token string\">&#039;resolution&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;0.05&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        description<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;Grid resolution (m\/cell)&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    declare_prob_hit <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token string\">&#039;prob_hit&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;0.55&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        description<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;P(occ | hit)&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    declare_prob_miss <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token string\">&#039;prob_miss&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;0.45&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        description<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;P(occ | miss)&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    declare_max_range <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token string\">&#039;max_range&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;30.0&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        description<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;Maximum laser range to use (m)&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    declare_min_range <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token string\">&#039;min_range&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;0.15&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        description<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;Minimum laser range (m)&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    declare_laser_topic <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token string\">&#039;laser_topic&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;\/scan&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        description<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;LaserScan topic&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    declare_map_frame <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token string\">&#039;map_frame&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;map&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        description<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;Map TF frame&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    declare_publish_rate <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token string\">&#039;publish_rate&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;5.0&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        description<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;Map publish rate (Hz)&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    declare_publish_entropy <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token string\">&#039;publish_entropy&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;true&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        description<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;Also publish \/entropy_map&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    declare_use_sim_time <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token string\">&#039;use_sim_time&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;false&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        description<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;Use simulation \/clock time&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    prob_mapper <span class=\"token operator\">&#061;<\/span> Node<span class=\"token punctuation\">(<\/span><br \/>\n        package<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;prob_map&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        executable<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;prob_mapper&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        name<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;prob_mapper&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        output<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;screen&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        parameters<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">[<\/span><span class=\"token punctuation\">{<\/span><br \/>\n            <span class=\"token string\">&#039;use_sim_time&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;use_sim_time&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token string\">&#039;map_width_m&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;map_width_m&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token string\">&#039;map_height_m&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;map_height_m&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token string\">&#039;resolution&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;resolution&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token string\">&#039;prob_hit&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;prob_hit&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token string\">&#039;prob_miss&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;prob_miss&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token string\">&#039;max_range&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;max_range&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token string\">&#039;min_range&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;min_range&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token string\">&#039;laser_topic&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;laser_topic&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token string\">&#039;map_frame&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;map_frame&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token string\">&#039;publish_rate&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;publish_rate&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token string\">&#039;publish_entropy&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;publish_entropy&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        <span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token punctuation\">)<\/span><\/p>\n<p>    <span class=\"token keyword\">return<\/span> LaunchDescription<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">[<\/span><br \/>\n        declare_rviz<span class=\"token punctuation\">,<\/span> declare_map_width<span class=\"token punctuation\">,<\/span> declare_map_height<span class=\"token punctuation\">,<\/span><br \/>\n        declare_resolution<span class=\"token punctuation\">,<\/span> declare_prob_hit<span class=\"token punctuation\">,<\/span> declare_prob_miss<span class=\"token punctuation\">,<\/span><br \/>\n        declare_max_range<span class=\"token punctuation\">,<\/span> declare_min_range<span class=\"token punctuation\">,<\/span> declare_laser_topic<span class=\"token punctuation\">,<\/span><br \/>\n        declare_map_frame<span class=\"token punctuation\">,<\/span> declare_publish_rate<span class=\"token punctuation\">,<\/span> declare_publish_entropy<span class=\"token punctuation\">,<\/span><br \/>\n        declare_use_sim_time<span class=\"token punctuation\">,<\/span> prob_mapper<span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<h6>slam_prob_mapper.launch.py<\/h6>\n<p><span class=\"token comment\"># \u6587\u4ef6: prob_map\/launch\/slam_prob_mapper.launch.py<\/span><br \/>\n<span class=\"token comment\"># slam_toolbox &#043; \u6982\u7387\u5730\u56fe &#043; RViz \u8054\u5408\u542f\u52a8<\/span><\/p>\n<p><span class=\"token keyword\">from<\/span> launch <span class=\"token keyword\">import<\/span> LaunchDescription<span class=\"token punctuation\">,<\/span> LaunchContext<br \/>\n<span class=\"token keyword\">from<\/span> launch<span class=\"token punctuation\">.<\/span>actions <span class=\"token keyword\">import<\/span> DeclareLaunchArgument<span class=\"token punctuation\">,<\/span> IncludeLaunchDescription<br \/>\n<span class=\"token keyword\">from<\/span> launch<span class=\"token punctuation\">.<\/span>conditions <span class=\"token keyword\">import<\/span> IfCondition<br \/>\n<span class=\"token keyword\">from<\/span> launch<span class=\"token punctuation\">.<\/span>substitutions <span class=\"token keyword\">import<\/span> LaunchConfiguration<span class=\"token punctuation\">,<\/span> PathJoinSubstitution<br \/>\n<span class=\"token keyword\">from<\/span> launch<span class=\"token punctuation\">.<\/span>substitutions <span class=\"token keyword\">import<\/span> EnvironmentVariable<br \/>\n<span class=\"token keyword\">from<\/span> launch<span class=\"token punctuation\">.<\/span>launch_description_sources <span class=\"token keyword\">import<\/span> PythonLaunchDescriptionSource<br \/>\n<span class=\"token keyword\">from<\/span> launch_ros<span class=\"token punctuation\">.<\/span>substitutions <span class=\"token keyword\">import<\/span> FindPackageShare<br \/>\n<span class=\"token keyword\">from<\/span> launch_ros<span class=\"token punctuation\">.<\/span>actions <span class=\"token keyword\">import<\/span> Node<\/p>\n<p><span class=\"token keyword\">def<\/span> <span class=\"token function\">generate_launch_description<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    slam_launch_path <span class=\"token operator\">&#061;<\/span> PathJoinSubstitution<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token punctuation\">[<\/span>FindPackageShare<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;slam_toolbox&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;launch&#039;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;online_async_launch.py&#039;<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    slam_config_path <span class=\"token operator\">&#061;<\/span> PathJoinSubstitution<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token punctuation\">[<\/span>FindPackageShare<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;linorobot2_navigation&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;config&#039;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;slam.yaml&#039;<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    lc <span class=\"token operator\">&#061;<\/span> LaunchContext<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ros_distro <span class=\"token operator\">&#061;<\/span> EnvironmentVariable<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;ROS_DISTRO&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    slam_param_name <span class=\"token operator\">&#061;<\/span> <span class=\"token string\">&#039;slam_params_file&#039;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> ros_distro<span class=\"token punctuation\">.<\/span>perform<span class=\"token punctuation\">(<\/span>lc<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#061;&#061;<\/span> <span class=\"token string\">&#039;foxy&#039;<\/span><span class=\"token punctuation\">:<\/span><br \/>\n        slam_param_name <span class=\"token operator\">&#061;<\/span> <span class=\"token string\">&#039;params_file&#039;<\/span><\/p>\n<p>    navigation_launch_path <span class=\"token operator\">&#061;<\/span> PathJoinSubstitution<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token punctuation\">[<\/span>FindPackageShare<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;nav2_bringup&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;launch&#039;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;navigation_launch.py&#039;<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    nav2_config_path <span class=\"token operator\">&#061;<\/span> PathJoinSubstitution<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token punctuation\">[<\/span>FindPackageShare<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;linorobot2_navigation&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;config&#039;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;navigation.yaml&#039;<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    rviz_config_path <span class=\"token operator\">&#061;<\/span> PathJoinSubstitution<span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token punctuation\">[<\/span>FindPackageShare<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;linorobot2_navigation&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;rviz&#039;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;linorobot2_slam.rviz&#039;<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    declare_sim <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;sim&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;false&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    declare_rviz <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;rviz&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;true&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    declare_map_size <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;prob_map_size&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;100.0&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    declare_prob_hit <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;prob_hit&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;0.55&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    declare_prob_miss <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;prob_miss&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;0.45&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    declare_max_range <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;max_range&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;30.0&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    declare_publish_entropy <span class=\"token operator\">&#061;<\/span> DeclareLaunchArgument<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;publish_entropy&#039;<\/span><span class=\"token punctuation\">,<\/span> default_value<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;true&#039;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    <span class=\"token keyword\">return<\/span> LaunchDescription<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">[<\/span><br \/>\n        declare_sim<span class=\"token punctuation\">,<\/span> declare_rviz<span class=\"token punctuation\">,<\/span> declare_map_size<span class=\"token punctuation\">,<\/span><br \/>\n        declare_prob_hit<span class=\"token punctuation\">,<\/span> declare_prob_miss<span class=\"token punctuation\">,<\/span> declare_max_range<span class=\"token punctuation\">,<\/span><br \/>\n        declare_publish_entropy<span class=\"token punctuation\">,<\/span><\/p>\n<p>        IncludeLaunchDescription<span class=\"token punctuation\">(<\/span><br \/>\n            PythonLaunchDescriptionSource<span class=\"token punctuation\">(<\/span>navigation_launch_path<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            launch_arguments<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">{<\/span><br \/>\n                <span class=\"token string\">&#039;use_sim_time&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;sim&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                <span class=\"token string\">&#039;params_file&#039;<\/span><span class=\"token punctuation\">:<\/span> nav2_config_path<span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">.<\/span>items<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><br \/>\n        <span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><\/p>\n<p>        IncludeLaunchDescription<span class=\"token punctuation\">(<\/span><br \/>\n            PythonLaunchDescriptionSource<span class=\"token punctuation\">(<\/span>slam_launch_path<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            launch_arguments<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">{<\/span><br \/>\n                <span class=\"token string\">&#039;use_sim_time&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;sim&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                slam_param_name<span class=\"token punctuation\">:<\/span> slam_config_path<span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">.<\/span>items<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><br \/>\n        <span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><\/p>\n<p>        Node<span class=\"token punctuation\">(<\/span><br \/>\n            package<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;prob_map&#039;<\/span><span class=\"token punctuation\">,<\/span> executable<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;prob_mapper&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            name<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;prob_mapper&#039;<\/span><span class=\"token punctuation\">,<\/span> output<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;screen&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            parameters<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">[<\/span><span class=\"token punctuation\">{<\/span><br \/>\n                <span class=\"token string\">&#039;use_sim_time&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;sim&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                <span class=\"token string\">&#039;map_width_m&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;prob_map_size&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                <span class=\"token string\">&#039;map_height_m&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;prob_map_size&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                <span class=\"token string\">&#039;resolution&#039;<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">0.05<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                <span class=\"token string\">&#039;prob_hit&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;prob_hit&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                <span class=\"token string\">&#039;prob_miss&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;prob_miss&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                <span class=\"token string\">&#039;max_range&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;max_range&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                <span class=\"token string\">&#039;min_range&#039;<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">0.15<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                <span class=\"token string\">&#039;laser_topic&#039;<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token string\">&#039;\/scan&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                <span class=\"token string\">&#039;map_frame&#039;<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token string\">&#039;map&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                <span class=\"token string\">&#039;publish_rate&#039;<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">5.0<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                <span class=\"token string\">&#039;publish_entropy&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;publish_entropy&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            <span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        <span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><\/p>\n<p>        Node<span class=\"token punctuation\">(<\/span><br \/>\n            package<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;rviz2&#039;<\/span><span class=\"token punctuation\">,<\/span> executable<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;rviz2&#039;<\/span><span class=\"token punctuation\">,<\/span> name<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;rviz2&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            output<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;screen&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            condition<span class=\"token operator\">&#061;<\/span>IfCondition<span class=\"token punctuation\">(<\/span>LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;rviz&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            arguments<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">[<\/span><span class=\"token string\">&#039;-d&#039;<\/span><span class=\"token punctuation\">,<\/span> rviz_config_path<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span><br \/>\n            parameters<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">[<\/span><span class=\"token punctuation\">{<\/span><span class=\"token string\">&#039;use_sim_time&#039;<\/span><span class=\"token punctuation\">:<\/span> LaunchConfiguration<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;sim&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        <span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<h6>mi_goal_selector.hpp<\/h6>\n<p><span class=\"token comment\">\/\/ \u6587\u4ef6: exploration\/include\/exploration\/mi_goal_selector.hpp<\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">ifndef<\/span> <span class=\"token expression\">EXPLORATION__MI_GOAL_SELECTOR_HPP_<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">define<\/span> <span class=\"token macro-name\">EXPLORATION__MI_GOAL_SELECTOR_HPP_<\/span><\/span><\/p>\n<p><span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;vector&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;rclcpp\/rclcpp.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;exploration\/goal_selector.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;exploration\/frontier_detector.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;nav_msgs\/msg\/occupancy_grid.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;geometry_msgs\/msg\/pose.hpp&#034;<\/span><\/span><\/p>\n<p><span class=\"token keyword\">namespace<\/span> exploration<br \/>\n<span class=\"token punctuation\">{<\/span><\/p>\n<p><span class=\"token keyword\">class<\/span> <span class=\"token class-name\">MIGoalSelector<\/span> <span class=\"token operator\">:<\/span> <span class=\"token base-clause\"><span class=\"token keyword\">public<\/span> <span class=\"token class-name\">GoalSelector<\/span><\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n<span class=\"token keyword\">public<\/span><span class=\"token operator\">:<\/span><br \/>\n  <span class=\"token function\">MIGoalSelector<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token keyword\">default<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">void<\/span> <span class=\"token function\">setProbMap<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid <span class=\"token operator\">&amp;<\/span> prob_map<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  FrontierCluster <span class=\"token function\">select<\/span><span class=\"token punctuation\">(<\/span><br \/>\n    <span class=\"token keyword\">const<\/span> std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>FrontierCluster<span class=\"token operator\">&gt;<\/span> <span class=\"token operator\">&amp;<\/span> frontiers<span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token keyword\">const<\/span> geometry_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Pose <span class=\"token operator\">&amp;<\/span> robot_pose<span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token keyword\">const<\/span> nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid <span class=\"token operator\">&amp;<\/span> map<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">override<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p><span class=\"token keyword\">private<\/span><span class=\"token operator\">:<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> <span class=\"token function\">computeMI<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> wx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> wy<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> <span class=\"token function\">beamEntropy<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> sx_g<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> sy_g<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> dx_w<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> dy_w<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">static<\/span> <span class=\"token keyword\">double<\/span> <span class=\"token function\">entropy<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">bool<\/span> <span class=\"token function\">worldToGridProb<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> wx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> wy<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> <span class=\"token operator\">&amp;<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> <span class=\"token operator\">&amp;<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span>  <span class=\"token function\">gridIndexProb<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">bool<\/span> <span class=\"token function\">inBoundsProb<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int8_t<\/span> <span class=\"token function\">probAt<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid prob_map_<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">bool<\/span> prob_map_received_<span class=\"token punctuation\">{<\/span><span class=\"token boolean\">false<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">int<\/span>    num_beams_<span class=\"token punctuation\">{<\/span><span class=\"token number\">60<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> mi_max_range_<span class=\"token punctuation\">{<\/span><span class=\"token number\">8.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> occ_threshold_<span class=\"token punctuation\">{<\/span><span class=\"token number\">0.7<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p><span class=\"token punctuation\">}<\/span>  <span class=\"token comment\">\/\/ namespace exploration<\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">endif<\/span>  <span class=\"token comment\">\/\/ EXPLORATION__MI_GOAL_SELECTOR_HPP_<\/span><\/span><\/p>\n<h6>mi_goal_selector.cpp<\/h6>\n<p><span class=\"token comment\">\/\/ \u6587\u4ef6: exploration\/src\/mi_goal_selector.cpp<\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;exploration\/mi_goal_selector.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;algorithm&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;cmath&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;limits&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;queue&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;utility&gt;<\/span><\/span><\/p>\n<p><span class=\"token keyword\">namespace<\/span> exploration<br \/>\n<span class=\"token punctuation\">{<\/span><\/p>\n<p><span class=\"token keyword\">namespace<\/span> <span class=\"token punctuation\">{<\/span><\/p>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token function\">bresenhamLine<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> x0<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> y0<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> x1<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> y1<span class=\"token punctuation\">,<\/span><br \/>\n                   std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>std<span class=\"token double-colon punctuation\">::<\/span>pair<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;&gt;<\/span> <span class=\"token operator\">&amp;<\/span> out<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  out<span class=\"token punctuation\">.<\/span><span class=\"token function\">clear<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> dx <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">abs<\/span><span class=\"token punctuation\">(<\/span>x1 <span class=\"token operator\">&#8211;<\/span> x0<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> dy <span class=\"token operator\">&#061;<\/span> <span class=\"token operator\">&#8211;<\/span>std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">abs<\/span><span class=\"token punctuation\">(<\/span>y1 <span class=\"token operator\">&#8211;<\/span> y0<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> sx <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>x0 <span class=\"token operator\">&lt;<\/span> x1<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">?<\/span> <span class=\"token number\">1<\/span> <span class=\"token operator\">:<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> sy <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>y0 <span class=\"token operator\">&lt;<\/span> y1<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">?<\/span> <span class=\"token number\">1<\/span> <span class=\"token operator\">:<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> err <span class=\"token operator\">&#061;<\/span> dx <span class=\"token operator\">&#043;<\/span> dy<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">while<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token boolean\">true<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    out<span class=\"token punctuation\">.<\/span><span class=\"token function\">emplace_back<\/span><span class=\"token punctuation\">(<\/span>x0<span class=\"token punctuation\">,<\/span> y0<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>x0 <span class=\"token operator\">&#061;&#061;<\/span> x1 <span class=\"token operator\">&amp;&amp;<\/span> y0 <span class=\"token operator\">&#061;&#061;<\/span> y1<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">break<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">int<\/span> e2 <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">2<\/span> <span class=\"token operator\">*<\/span> err<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>e2 <span class=\"token operator\">&gt;&#061;<\/span> dy<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span> err <span class=\"token operator\">&#043;&#061;<\/span> dy<span class=\"token punctuation\">;<\/span> x0 <span class=\"token operator\">&#043;&#061;<\/span> sx<span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>e2 <span class=\"token operator\">&lt;&#061;<\/span> dx<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span> err <span class=\"token operator\">&#043;&#061;<\/span> dx<span class=\"token punctuation\">;<\/span> y0 <span class=\"token operator\">&#043;&#061;<\/span> sy<span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token punctuation\">}<\/span>  <span class=\"token comment\">\/\/ anonymous namespace<\/span><\/p>\n<p><span class=\"token keyword\">double<\/span> <span class=\"token class-name\">MIGoalSelector<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">entropy<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> p<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">&lt;&#061;<\/span> <span class=\"token number\">1e-6<\/span> <span class=\"token operator\">||<\/span> p <span class=\"token operator\">&gt;&#061;<\/span> <span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1e-6<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">return<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">*<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">log2<\/span><span class=\"token punctuation\">(<\/span>p<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#8211;<\/span> p<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">log2<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#8211;<\/span> p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token class-name\">MIGoalSelector<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">setProbMap<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid <span class=\"token operator\">&amp;<\/span> prob_map<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  prob_map_ <span class=\"token operator\">&#061;<\/span> prob_map<span class=\"token punctuation\">;<\/span><br \/>\n  prob_map_received_ <span class=\"token operator\">&#061;<\/span> <span class=\"token boolean\">true<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">bool<\/span> <span class=\"token class-name\">MIGoalSelector<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">worldToGridProb<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> wx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> wy<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> <span class=\"token operator\">&amp;<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> <span class=\"token operator\">&amp;<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> ox <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> oy <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> res <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>resolution<span class=\"token punctuation\">;<\/span><br \/>\n  mx <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>wx <span class=\"token operator\">&#8211;<\/span> ox<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> res<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  my <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>wy <span class=\"token operator\">&#8211;<\/span> oy<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> res<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> <span class=\"token function\">inBoundsProb<\/span><span class=\"token punctuation\">(<\/span>mx<span class=\"token punctuation\">,<\/span> my<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">int<\/span> <span class=\"token class-name\">MIGoalSelector<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">gridIndexProb<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><br \/>\n<span class=\"token punctuation\">{<\/span> <span class=\"token keyword\">return<\/span> my <span class=\"token operator\">*<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>width<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#043;<\/span> mx<span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">bool<\/span> <span class=\"token class-name\">MIGoalSelector<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">inBoundsProb<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> mx <span class=\"token operator\">&gt;&#061;<\/span> <span class=\"token number\">0<\/span> <span class=\"token operator\">&amp;&amp;<\/span> mx <span class=\"token operator\">&lt;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>width<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&amp;&amp;<\/span><br \/>\n         my <span class=\"token operator\">&gt;&#061;<\/span> <span class=\"token number\">0<\/span> <span class=\"token operator\">&amp;&amp;<\/span> my <span class=\"token operator\">&lt;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>height<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">int8_t<\/span> <span class=\"token class-name\">MIGoalSelector<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">probAt<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> mx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><br \/>\n<span class=\"token punctuation\">{<\/span> <span class=\"token keyword\">return<\/span> prob_map_<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">[<\/span><span class=\"token function\">gridIndexProb<\/span><span class=\"token punctuation\">(<\/span>mx<span class=\"token punctuation\">,<\/span> my<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">double<\/span> <span class=\"token class-name\">MIGoalSelector<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">beamEntropy<\/span><span class=\"token punctuation\">(<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> sx_g<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> sy_g<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> dx_w<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> dy_w<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> ox <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> oy <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> res <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>resolution<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">int<\/span> w <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>width<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">int<\/span> h <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>height<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">double<\/span> start_wx <span class=\"token operator\">&#061;<\/span> ox <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span>sx_g <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">0.5<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> res<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> start_wy <span class=\"token operator\">&#061;<\/span> oy <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span>sy_g <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">0.5<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> res<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> end_wx <span class=\"token operator\">&#061;<\/span> start_wx <span class=\"token operator\">&#043;<\/span> dx_w <span class=\"token operator\">*<\/span> mi_max_range_<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> end_wy <span class=\"token operator\">&#061;<\/span> start_wy <span class=\"token operator\">&#043;<\/span> dy_w <span class=\"token operator\">*<\/span> mi_max_range_<span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">int<\/span> ex_g <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>end_wx <span class=\"token operator\">&#8211;<\/span> ox<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> res<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> ey_g <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>end_wy <span class=\"token operator\">&#8211;<\/span> oy<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> res<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  ex_g <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span>ex_g<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> w <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  ey_g <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span>ey_g<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> h <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>std<span class=\"token double-colon punctuation\">::<\/span>pair<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;&gt;<\/span> cells<span class=\"token punctuation\">;<\/span><br \/>\n  cells<span class=\"token punctuation\">.<\/span><span class=\"token function\">reserve<\/span><span class=\"token punctuation\">(<\/span><span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>size_t<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>mi_max_range_ <span class=\"token operator\">\/<\/span> res<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">10<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token function\">bresenhamLine<\/span><span class=\"token punctuation\">(<\/span>sx_g<span class=\"token punctuation\">,<\/span> sy_g<span class=\"token punctuation\">,<\/span> ex_g<span class=\"token punctuation\">,<\/span> ey_g<span class=\"token punctuation\">,<\/span> cells<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">double<\/span> total <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> <span class=\"token keyword\">auto<\/span> <span class=\"token operator\">&amp;<\/span> <span class=\"token punctuation\">[<\/span>mx<span class=\"token punctuation\">,<\/span> my<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">:<\/span> cells<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token operator\">!<\/span><span class=\"token function\">inBoundsProb<\/span><span class=\"token punctuation\">(<\/span>mx<span class=\"token punctuation\">,<\/span> my<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">break<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> p <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">probAt<\/span><span class=\"token punctuation\">(<\/span>mx<span class=\"token punctuation\">,<\/span> my<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> <span class=\"token number\">100.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">&gt;<\/span> occ_threshold_<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">break<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    total <span class=\"token operator\">&#043;&#061;<\/span> <span class=\"token function\">entropy<\/span><span class=\"token punctuation\">(<\/span>p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> total<span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">double<\/span> <span class=\"token class-name\">MIGoalSelector<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">computeMI<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> wx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">double<\/span> wy<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">const<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> ox <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> oy <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> res <span class=\"token operator\">&#061;<\/span> prob_map_<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>resolution<span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">int<\/span> sx_g <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>wx <span class=\"token operator\">&#8211;<\/span> ox<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> res<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> sy_g <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>wy <span class=\"token operator\">&#8211;<\/span> oy<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> res<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token operator\">!<\/span><span class=\"token function\">inBoundsProb<\/span><span class=\"token punctuation\">(<\/span>sx_g<span class=\"token punctuation\">,<\/span> sy_g<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">return<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">double<\/span> mi <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> i <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> i <span class=\"token operator\">&lt;<\/span> num_beams_<span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> angle <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">2.0<\/span> <span class=\"token operator\">*<\/span> M_PI <span class=\"token operator\">*<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> num_beams_<span class=\"token punctuation\">;<\/span><br \/>\n    mi <span class=\"token operator\">&#043;&#061;<\/span> <span class=\"token function\">beamEntropy<\/span><span class=\"token punctuation\">(<\/span>sx_g<span class=\"token punctuation\">,<\/span> sy_g<span class=\"token punctuation\">,<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">cos<\/span><span class=\"token punctuation\">(<\/span>angle<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">sin<\/span><span class=\"token punctuation\">(<\/span>angle<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> mi <span class=\"token operator\">\/<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>num_beams_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">static<\/span> std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span> <span class=\"token function\">dijkstraDistanceMap<\/span><span class=\"token punctuation\">(<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid <span class=\"token operator\">&amp;<\/span> map<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> sx<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span> sy<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">int<\/span> w <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>map<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>width<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">int<\/span> h <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>map<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>height<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span> <span class=\"token function\">dist<\/span><span class=\"token punctuation\">(<\/span>w <span class=\"token operator\">*<\/span> h<span class=\"token punctuation\">,<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token class-name\">numeric_limits<\/span><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">infinity<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">using<\/span> Node <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span>pair<span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>priority_queue<span class=\"token operator\">&lt;<\/span>Node<span class=\"token punctuation\">,<\/span> std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>Node<span class=\"token operator\">&gt;<\/span><span class=\"token punctuation\">,<\/span> std<span class=\"token double-colon punctuation\">::<\/span>greater<span class=\"token operator\">&lt;<\/span>Node<span class=\"token operator\">&gt;&gt;<\/span> pq<span class=\"token punctuation\">;<\/span><\/p>\n<p>  dist<span class=\"token punctuation\">[<\/span>sy <span class=\"token operator\">*<\/span> w <span class=\"token operator\">&#043;<\/span> sx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  pq<span class=\"token punctuation\">.<\/span><span class=\"token function\">emplace<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.0<\/span><span class=\"token punctuation\">,<\/span> sy <span class=\"token operator\">*<\/span> w <span class=\"token operator\">&#043;<\/span> sx<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">int<\/span> dx8<span class=\"token punctuation\">[<\/span><span class=\"token number\">8<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">{<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">int<\/span> dy8<span class=\"token punctuation\">[<\/span><span class=\"token number\">8<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">{<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> diag <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">sqrt<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">2.0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">while<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token operator\">!<\/span>pq<span class=\"token punctuation\">.<\/span><span class=\"token function\">empty<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">auto<\/span> <span class=\"token punctuation\">[<\/span>d<span class=\"token punctuation\">,<\/span> k<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> pq<span class=\"token punctuation\">.<\/span><span class=\"token function\">top<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> pq<span class=\"token punctuation\">.<\/span><span class=\"token function\">pop<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>d <span class=\"token operator\">&gt;<\/span> dist<span class=\"token punctuation\">[<\/span>k<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">1e-6<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">continue<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">int<\/span> cx <span class=\"token operator\">&#061;<\/span> k <span class=\"token operator\">%<\/span> w<span class=\"token punctuation\">,<\/span> cy <span class=\"token operator\">&#061;<\/span> k <span class=\"token operator\">\/<\/span> w<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">int<\/span> dir <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> dir <span class=\"token operator\">&lt;<\/span> <span class=\"token number\">8<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>dir<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      <span class=\"token keyword\">int<\/span> nx <span class=\"token operator\">&#061;<\/span> cx <span class=\"token operator\">&#043;<\/span> dx8<span class=\"token punctuation\">[<\/span>dir<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span> ny <span class=\"token operator\">&#061;<\/span> cy <span class=\"token operator\">&#043;<\/span> dy8<span class=\"token punctuation\">[<\/span>dir<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>nx <span class=\"token operator\">&lt;<\/span> <span class=\"token number\">0<\/span> <span class=\"token operator\">||<\/span> nx <span class=\"token operator\">&gt;&#061;<\/span> w <span class=\"token operator\">||<\/span> ny <span class=\"token operator\">&lt;<\/span> <span class=\"token number\">0<\/span> <span class=\"token operator\">||<\/span> ny <span class=\"token operator\">&gt;&#061;<\/span> h<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">continue<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>map<span class=\"token punctuation\">.<\/span>data<span class=\"token punctuation\">[<\/span>ny <span class=\"token operator\">*<\/span> w <span class=\"token operator\">&#043;<\/span> nx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&gt;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">continue<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">double<\/span> step <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>dir <span class=\"token operator\">%<\/span> <span class=\"token number\">2<\/span> <span class=\"token operator\">&#061;&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">?<\/span> <span class=\"token number\">1.0<\/span> <span class=\"token operator\">:<\/span> diag<span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">double<\/span> nd <span class=\"token operator\">&#061;<\/span> d <span class=\"token operator\">&#043;<\/span> step<span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">int<\/span> nk <span class=\"token operator\">&#061;<\/span> ny <span class=\"token operator\">*<\/span> w <span class=\"token operator\">&#043;<\/span> nx<span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>nd <span class=\"token operator\">&lt;<\/span> dist<span class=\"token punctuation\">[<\/span>nk<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span> dist<span class=\"token punctuation\">[<\/span>nk<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> nd<span class=\"token punctuation\">;<\/span> pq<span class=\"token punctuation\">.<\/span><span class=\"token function\">emplace<\/span><span class=\"token punctuation\">(<\/span>nd<span class=\"token punctuation\">,<\/span> nk<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> dist<span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p>FrontierCluster <span class=\"token class-name\">MIGoalSelector<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">select<\/span><span class=\"token punctuation\">(<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>FrontierCluster<span class=\"token operator\">&gt;<\/span> <span class=\"token operator\">&amp;<\/span> frontiers<span class=\"token punctuation\">,<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> geometry_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Pose <span class=\"token operator\">&amp;<\/span> robot_pose<span class=\"token punctuation\">,<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid <span class=\"token operator\">&amp;<\/span> map<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>frontiers<span class=\"token punctuation\">.<\/span><span class=\"token function\">empty<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">throw<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">runtime_error<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;MIGoalSelector: no frontiers&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token operator\">!<\/span>prob_map_received_<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">throw<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">runtime_error<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;MIGoalSelector: no prob_map&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> rx <span class=\"token operator\">&#061;<\/span> robot_pose<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">,<\/span> ry <span class=\"token operator\">&#061;<\/span> robot_pose<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> res <span class=\"token operator\">&#061;<\/span> map<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>resolution<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">double<\/span> ox <span class=\"token operator\">&#061;<\/span> map<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">,<\/span> oy <span class=\"token operator\">&#061;<\/span> map<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>origin<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">int<\/span> w <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>map<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>width<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> h <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>map<span class=\"token punctuation\">.<\/span>info<span class=\"token punctuation\">.<\/span>height<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">int<\/span> sx <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span><span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>rx <span class=\"token operator\">&#8211;<\/span> ox<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> res<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> w <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> sy <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">clamp<\/span><span class=\"token punctuation\">(<\/span><span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">int<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>ry <span class=\"token operator\">&#8211;<\/span> oy<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> res<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> h <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">auto<\/span> dist_map <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">dijkstraDistanceMap<\/span><span class=\"token punctuation\">(<\/span>map<span class=\"token punctuation\">,<\/span> sx<span class=\"token punctuation\">,<\/span> sy<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">double<\/span> best_utility <span class=\"token operator\">&#061;<\/span> <span class=\"token operator\">&#8211;<\/span>std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token class-name\">numeric_limits<\/span><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">max<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  size_t best_idx <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span>size_t i <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">;<\/span> i <span class=\"token operator\">&lt;<\/span> frontiers<span class=\"token punctuation\">.<\/span><span class=\"token function\">size<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token operator\">&#043;&#043;<\/span>i<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> mi <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">computeMI<\/span><span class=\"token punctuation\">(<\/span>frontiers<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">.<\/span>centroid<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">,<\/span> frontiers<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">.<\/span>centroid<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token keyword\">double<\/span> best_dist <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token class-name\">numeric_limits<\/span><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">max<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> <span class=\"token keyword\">auto<\/span> <span class=\"token operator\">&amp;<\/span> cell <span class=\"token operator\">:<\/span> frontiers<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">.<\/span>cells<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      <span class=\"token keyword\">int<\/span> k <span class=\"token operator\">&#061;<\/span> cell<span class=\"token punctuation\">.<\/span>second <span class=\"token operator\">*<\/span> w <span class=\"token operator\">&#043;<\/span> cell<span class=\"token punctuation\">.<\/span>first<span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>k <span class=\"token operator\">&gt;&#061;<\/span> <span class=\"token number\">0<\/span> <span class=\"token operator\">&amp;&amp;<\/span> <span class=\"token generic-function\"><span class=\"token function\">static_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>size_t<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>k<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&lt;<\/span> dist_map<span class=\"token punctuation\">.<\/span><span class=\"token function\">size<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><br \/>\n        <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>dist_map<span class=\"token punctuation\">[<\/span>k<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&lt;<\/span> best_dist<span class=\"token punctuation\">)<\/span> best_dist <span class=\"token operator\">&#061;<\/span> dist_map<span class=\"token punctuation\">[<\/span>k<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><\/p>\n<p>    <span class=\"token keyword\">double<\/span> nav_cost<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">isinf<\/span><span class=\"token punctuation\">(<\/span>best_dist<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      nav_cost <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">hypot<\/span><span class=\"token punctuation\">(<\/span>frontiers<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">.<\/span>centroid<span class=\"token punctuation\">.<\/span>x <span class=\"token operator\">&#8211;<\/span> rx<span class=\"token punctuation\">,<\/span><br \/>\n                            frontiers<span class=\"token punctuation\">[<\/span>i<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">.<\/span>centroid<span class=\"token punctuation\">.<\/span>y <span class=\"token operator\">&#8211;<\/span> ry<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> <span class=\"token number\">3.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span> <span class=\"token keyword\">else<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      nav_cost <span class=\"token operator\">&#061;<\/span> best_dist <span class=\"token operator\">*<\/span> res<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><\/p>\n<p>    <span class=\"token keyword\">double<\/span> utility <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>nav_cost <span class=\"token operator\">&gt;<\/span> <span class=\"token number\">1e-6<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">?<\/span> mi <span class=\"token operator\">\/<\/span> nav_cost <span class=\"token operator\">:<\/span> mi<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>utility <span class=\"token operator\">&gt;<\/span> best_utility<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span> best_utility <span class=\"token operator\">&#061;<\/span> utility<span class=\"token punctuation\">;<\/span> best_idx <span class=\"token operator\">&#061;<\/span> i<span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><\/p>\n<p>  <span class=\"token keyword\">auto<\/span> result <span class=\"token operator\">&#061;<\/span> frontiers<span class=\"token punctuation\">[<\/span>best_idx<span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> best_cell_d2 <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token class-name\">numeric_limits<\/span><span class=\"token operator\">&lt;<\/span><span class=\"token keyword\">double<\/span><span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">max<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">for<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> <span class=\"token keyword\">auto<\/span> <span class=\"token operator\">&amp;<\/span> cell <span class=\"token operator\">:<\/span> result<span class=\"token punctuation\">.<\/span>cells<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> wx <span class=\"token operator\">&#061;<\/span> ox <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span>cell<span class=\"token punctuation\">.<\/span>first <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">0.5<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> res<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> wy <span class=\"token operator\">&#061;<\/span> oy <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span>cell<span class=\"token punctuation\">.<\/span>second <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">0.5<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> res<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> d2 <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>wx <span class=\"token operator\">&#8211;<\/span> rx<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> <span class=\"token punctuation\">(<\/span>wx <span class=\"token operator\">&#8211;<\/span> rx<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span>wy <span class=\"token operator\">&#8211;<\/span> ry<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> <span class=\"token punctuation\">(<\/span>wy <span class=\"token operator\">&#8211;<\/span> ry<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>d2 <span class=\"token operator\">&lt;<\/span> best_cell_d2<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      best_cell_d2 <span class=\"token operator\">&#061;<\/span> d2<span class=\"token punctuation\">;<\/span><br \/>\n      result<span class=\"token punctuation\">.<\/span>closest<span class=\"token punctuation\">.<\/span>x <span class=\"token operator\">&#061;<\/span> wx<span class=\"token punctuation\">;<\/span> result<span class=\"token punctuation\">.<\/span>closest<span class=\"token punctuation\">.<\/span>y <span class=\"token operator\">&#061;<\/span> wy<span class=\"token punctuation\">;<\/span> result<span class=\"token punctuation\">.<\/span>closest<span class=\"token punctuation\">.<\/span>z <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> result<span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token punctuation\">}<\/span>  <span class=\"token comment\">\/\/ namespace exploration<\/span><\/p>\n<h6>exploration_node.hpp&#xff08;\u5b8c\u6574\u6587\u4ef6&#xff0c;\u542b MI \u6269\u5c55&#xff09;<\/h6>\n<p><span class=\"token comment\">\/\/ \u6587\u4ef6: exploration\/include\/exploration\/exploration_node.hpp<\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">ifndef<\/span> <span class=\"token expression\">EXPLORATION__EXPLORATION_NODE_HPP_<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">define<\/span> <span class=\"token macro-name\">EXPLORATION__EXPLORATION_NODE_HPP_<\/span><\/span><\/p>\n<p><span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;memory&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;string&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;vector&gt;<\/span><\/span><\/p>\n<p><span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;rclcpp\/rclcpp.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;rclcpp_lifecycle\/lifecycle_node.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;rclcpp_action\/rclcpp_action.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;nav_msgs\/msg\/occupancy_grid.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;nav2_msgs\/action\/navigate_to_pose.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;geometry_msgs\/msg\/pose_stamped.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;geometry_msgs\/msg\/pose_with_covariance_stamped.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;visualization_msgs\/msg\/marker_array.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;tf2_ros\/buffer.h&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;tf2_ros\/transform_listener.h&#034;<\/span><\/span><\/p>\n<p><span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;exploration\/frontier_detector.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;exploration\/goal_selector.hpp&#034;<\/span><\/span><\/p>\n<p><span class=\"token keyword\">namespace<\/span> exploration<br \/>\n<span class=\"token punctuation\">{<\/span><\/p>\n<p><span class=\"token keyword\">class<\/span> <span class=\"token class-name\">ExplorationNode<\/span> <span class=\"token operator\">:<\/span> <span class=\"token base-clause\"><span class=\"token keyword\">public<\/span> rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span><span class=\"token class-name\">LifecycleNode<\/span><\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n<span class=\"token keyword\">public<\/span><span class=\"token operator\">:<\/span><br \/>\n  <span class=\"token keyword\">using<\/span> NavigateToPose <span class=\"token operator\">&#061;<\/span> nav2_msgs<span class=\"token double-colon punctuation\">::<\/span>action<span class=\"token double-colon punctuation\">::<\/span>NavigateToPose<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">using<\/span> GoalHandleNav <span class=\"token operator\">&#061;<\/span> rclcpp_action<span class=\"token double-colon punctuation\">::<\/span>ClientGoalHandle<span class=\"token operator\">&lt;<\/span>NavigateToPose<span class=\"token operator\">&gt;<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token function\">ExplorationNode<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span>NodeOptions <span class=\"token operator\">&amp;<\/span> options <span class=\"token operator\">&#061;<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">NodeOptions<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token operator\">~<\/span><span class=\"token function\">ExplorationNode<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token keyword\">default<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p><span class=\"token keyword\">protected<\/span><span class=\"token operator\">:<\/span><br \/>\n  rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>node_interfaces<span class=\"token double-colon punctuation\">::<\/span>LifecycleNodeInterface<span class=\"token double-colon punctuation\">::<\/span>CallbackReturn<br \/>\n  <span class=\"token function\">on_configure<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>State <span class=\"token operator\">&amp;<\/span> state<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">override<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>node_interfaces<span class=\"token double-colon punctuation\">::<\/span>LifecycleNodeInterface<span class=\"token double-colon punctuation\">::<\/span>CallbackReturn<br \/>\n  <span class=\"token function\">on_activate<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>State <span class=\"token operator\">&amp;<\/span> state<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">override<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>node_interfaces<span class=\"token double-colon punctuation\">::<\/span>LifecycleNodeInterface<span class=\"token double-colon punctuation\">::<\/span>CallbackReturn<br \/>\n  <span class=\"token function\">on_deactivate<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>State <span class=\"token operator\">&amp;<\/span> state<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">override<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>node_interfaces<span class=\"token double-colon punctuation\">::<\/span>LifecycleNodeInterface<span class=\"token double-colon punctuation\">::<\/span>CallbackReturn<br \/>\n  <span class=\"token function\">on_cleanup<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>State <span class=\"token operator\">&amp;<\/span> state<span class=\"token punctuation\">)<\/span> <span class=\"token keyword\">override<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p><span class=\"token keyword\">private<\/span><span class=\"token operator\">:<\/span><br \/>\n  <span class=\"token keyword\">enum<\/span> <span class=\"token keyword\">class<\/span> <span class=\"token class-name\">State<\/span> <span class=\"token punctuation\">{<\/span> IDLE<span class=\"token punctuation\">,<\/span> SELECT<span class=\"token punctuation\">,<\/span> MOVING<span class=\"token punctuation\">,<\/span> DONE <span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">static<\/span> <span class=\"token keyword\">const<\/span> <span class=\"token keyword\">char<\/span> <span class=\"token operator\">*<\/span> <span class=\"token function\">stateToString<\/span><span class=\"token punctuation\">(<\/span>State s<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">void<\/span> <span class=\"token function\">mapCallback<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token double-colon punctuation\">::<\/span>SharedPtr msg<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">probMapCallback<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token double-colon punctuation\">::<\/span>SharedPtr msg<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">timerCallback<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">updateRobotPose<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">selectAndSendGoal<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">sendGoal<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> geometry_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>PoseStamped <span class=\"token operator\">&amp;<\/span> goal_pose<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">goalResponseCallback<\/span><span class=\"token punctuation\">(<\/span>GoalHandleNav<span class=\"token double-colon punctuation\">::<\/span>SharedPtr goal_handle<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">resultCallback<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> GoalHandleNav<span class=\"token double-colon punctuation\">::<\/span>WrappedResult <span class=\"token operator\">&amp;<\/span> result<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">void<\/span> <span class=\"token function\">publishMarkers<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> std<span class=\"token double-colon punctuation\">::<\/span>vector<span class=\"token operator\">&lt;<\/span>FrontierCluster<span class=\"token operator\">&gt;<\/span> <span class=\"token operator\">&amp;<\/span> frontiers<span class=\"token punctuation\">,<\/span><br \/>\n                      <span class=\"token keyword\">const<\/span> geometry_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Point <span class=\"token operator\">*<\/span> selected_pos<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  std<span class=\"token double-colon punctuation\">::<\/span>shared_ptr<span class=\"token operator\">&lt;<\/span>tf2_ros<span class=\"token double-colon punctuation\">::<\/span>Buffer<span class=\"token operator\">&gt;<\/span> tf_buffer_<span class=\"token punctuation\">;<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>shared_ptr<span class=\"token operator\">&lt;<\/span>tf2_ros<span class=\"token double-colon punctuation\">::<\/span>TransformListener<span class=\"token operator\">&gt;<\/span> tf_listener_<span class=\"token punctuation\">;<\/span><\/p>\n<p>  rclcpp<span class=\"token double-colon punctuation\">::<\/span>Subscription<span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span>SharedPtr map_sub_<span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span>Subscription<span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span>SharedPtr prob_map_sub_<span class=\"token punctuation\">;<\/span><\/p>\n<p>  rclcpp_action<span class=\"token double-colon punctuation\">::<\/span>Client<span class=\"token operator\">&lt;<\/span>NavigateToPose<span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span>SharedPtr nav_action_client_<span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span>TimerBase<span class=\"token double-colon punctuation\">::<\/span>SharedPtr timer_<span class=\"token punctuation\">;<\/span><\/p>\n<p>  rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>LifecyclePublisher<span class=\"token operator\">&lt;<\/span>visualization_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>MarkerArray<span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span>SharedPtr marker_pub_<span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>LifecyclePublisher<span class=\"token operator\">&lt;<\/span>visualization_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Marker<span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span>SharedPtr tree_marker_pub_<span class=\"token punctuation\">;<\/span><\/p>\n<p>  FrontierDetector frontier_detector_<span class=\"token punctuation\">;<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>unique_ptr<span class=\"token operator\">&lt;<\/span>GoalSelector<span class=\"token operator\">&gt;<\/span> goal_selector_<span class=\"token punctuation\">;<\/span><\/p>\n<p>  State state_<span class=\"token punctuation\">{<\/span>State<span class=\"token double-colon punctuation\">::<\/span>IDLE<span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid last_map_<span class=\"token punctuation\">;<\/span><br \/>\n  nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid last_prob_map_<span class=\"token punctuation\">;<\/span><br \/>\n  geometry_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Pose current_pose_<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">bool<\/span> map_received_<span class=\"token punctuation\">{<\/span><span class=\"token boolean\">false<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">bool<\/span> prob_map_received_<span class=\"token punctuation\">{<\/span><span class=\"token boolean\">false<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">bool<\/span> pose_received_<span class=\"token punctuation\">{<\/span><span class=\"token boolean\">false<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">bool<\/span> goal_active_<span class=\"token punctuation\">{<\/span><span class=\"token boolean\">false<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">bool<\/span> first_goal_<span class=\"token punctuation\">{<\/span><span class=\"token boolean\">true<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">bool<\/span> replanning_<span class=\"token punctuation\">{<\/span><span class=\"token boolean\">false<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span>Time activation_time_<span class=\"token punctuation\">;<\/span><br \/>\n  rclcpp<span class=\"token double-colon punctuation\">::<\/span>Time goal_start_time_<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">double<\/span> goal_timeout_<span class=\"token punctuation\">{<\/span><span class=\"token number\">60.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">double<\/span> goal_reached_dist_<span class=\"token punctuation\">{<\/span><span class=\"token number\">1.0<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">int<\/span> min_frontier_size_<span class=\"token punctuation\">{<\/span><span class=\"token number\">5<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>string map_topic_<span class=\"token punctuation\">{<\/span><span class=\"token string\">&#034;\/map&#034;<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>string prob_map_topic_<span class=\"token punctuation\">{<\/span><span class=\"token string\">&#034;\/prob_map&#034;<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>string goal_selector_type_<span class=\"token punctuation\">{<\/span><span class=\"token string\">&#034;nearest&#034;<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  std<span class=\"token double-colon punctuation\">::<\/span>string robot_base_frame_<span class=\"token punctuation\">{<\/span><span class=\"token string\">&#034;base_link&#034;<\/span><span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  geometry_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Point selected_goal_pos_<span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p><span class=\"token punctuation\">}<\/span>  <span class=\"token comment\">\/\/ namespace exploration<\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">endif<\/span>  <span class=\"token comment\">\/\/ EXPLORATION__EXPLORATION_NODE_HPP_<\/span><\/span><\/p>\n<h6>exploration_node.cpp&#xff08;\u5b8c\u6574\u6587\u4ef6&#xff0c;\u542b MI \u6269\u5c55&#xff09;<\/h6>\n<p><span class=\"token comment\">\/\/ \u6587\u4ef6: exploration\/src\/exploration_node.cpp<\/span><br \/>\n<span class=\"token comment\">\/\/ \u7bc7\u5e45\u6240\u9650&#xff0c;\u4ec5\u5217\u51fa\u672c\u671f\u4fee\u6539\u7684\u5173\u952e\u51fd\u6570&#xff0c;\u5b8c\u6574\u6587\u4ef6\u89c1\u9879\u76ee\u4ed3\u5e93<\/span><br \/>\n<span class=\"token comment\">\/\/ ros2_ws\/src\/exploration\/src\/exploration_node.cpp<\/span><\/p>\n<p><span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;exploration\/exploration_node.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;exploration\/mi_goal_selector.hpp&#034;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;functional&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;string&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;cmath&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&lt;limits&gt;<\/span><\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">include<\/span> <span class=\"token string\">&#034;tf2_geometry_msgs\/tf2_geometry_msgs.hpp&#034;<\/span><\/span><\/p>\n<p><span class=\"token keyword\">namespace<\/span> exploration<br \/>\n<span class=\"token punctuation\">{<\/span><\/p>\n<p><span class=\"token class-name\">ExplorationNode<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">ExplorationNode<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span>NodeOptions <span class=\"token operator\">&amp;<\/span> options<span class=\"token punctuation\">)<\/span><br \/>\n  <span class=\"token operator\">:<\/span> rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">LifecycleNode<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;exploration_node&#034;<\/span><span class=\"token punctuation\">,<\/span> options<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;min_frontier_size&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">5<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;goal_reached_dist&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1.0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;map_topic&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;\/map&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;prob_map_topic&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;\/prob_map&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span>        <span class=\"token comment\">\/\/ \u65b0\u589e<\/span><br \/>\n  <span class=\"token function\">declare_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;goal_selector_type&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;nearest&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p>rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>node_interfaces<span class=\"token double-colon punctuation\">::<\/span>LifecycleNodeInterface<span class=\"token double-colon punctuation\">::<\/span>CallbackReturn<br \/>\n<span class=\"token class-name\">ExplorationNode<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">on_configure<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>State <span class=\"token operator\">&amp;<\/span> <span class=\"token comment\">\/*state*\/<\/span><span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  <span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;min_frontier_size&#034;<\/span><span class=\"token punctuation\">,<\/span> min_frontier_size_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;goal_reached_dist&#034;<\/span><span class=\"token punctuation\">,<\/span> goal_reached_dist_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;map_topic&#034;<\/span><span class=\"token punctuation\">,<\/span> map_topic_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;prob_map_topic&#034;<\/span><span class=\"token punctuation\">,<\/span> prob_map_topic_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span>        <span class=\"token comment\">\/\/ \u65b0\u589e<\/span><br \/>\n  <span class=\"token function\">get_parameter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;goal_selector_type&#034;<\/span><span class=\"token punctuation\">,<\/span> goal_selector_type_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  frontier_detector_<span class=\"token punctuation\">.<\/span><span class=\"token function\">setMinFrontierSize<\/span><span class=\"token punctuation\">(<\/span>min_frontier_size_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>goal_selector_type_ <span class=\"token operator\">&#061;&#061;<\/span> <span class=\"token string\">&#034;nearest&#034;<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    goal_selector_ <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">make_unique<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>NearestGoalSelector<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span> <span class=\"token keyword\">else<\/span> <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>goal_selector_type_ <span class=\"token operator\">&#061;&#061;<\/span> <span class=\"token string\">&#034;utility&#034;<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    goal_selector_ <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">make_unique<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>UtilityGoalSelector<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span> <span class=\"token keyword\">else<\/span> <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>goal_selector_type_ <span class=\"token operator\">&#061;&#061;<\/span> <span class=\"token string\">&#034;rig&#034;<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    goal_selector_ <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">make_unique<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>RIGGoalSelector<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span> <span class=\"token keyword\">else<\/span> <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>goal_selector_type_ <span class=\"token operator\">&#061;&#061;<\/span> <span class=\"token string\">&#034;mi&#034;<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span>                <span class=\"token comment\">\/\/ \u65b0\u589e<\/span><br \/>\n    goal_selector_ <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">make_unique<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>MIGoalSelector<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token function\">RCLCPP_INFO<\/span><span class=\"token punctuation\">(<\/span><span class=\"token function\">get_logger<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;Using MIGoalSelector (mutual information)&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span> <span class=\"token keyword\">else<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token function\">RCLCPP_ERROR<\/span><span class=\"token punctuation\">(<\/span><span class=\"token function\">get_logger<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;Unknown goal_selector_type: %s&#034;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n      goal_selector_type_<span class=\"token punctuation\">.<\/span><span class=\"token function\">c_str<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">return<\/span> rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>node_interfaces<span class=\"token double-colon punctuation\">::<\/span><br \/>\n      LifecycleNodeInterface<span class=\"token double-colon punctuation\">::<\/span>CallbackReturn<span class=\"token double-colon punctuation\">::<\/span>ERROR<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><\/p>\n<p>  tf_buffer_ <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">make_shared<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>tf2_ros<span class=\"token double-colon punctuation\">::<\/span>Buffer<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token function\">get_clock<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  tf_listener_ <span class=\"token operator\">&#061;<\/span> std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">make_shared<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>tf2_ros<span class=\"token double-colon punctuation\">::<\/span>TransformListener<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">*<\/span>tf_buffer_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  marker_pub_ <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">create_publisher<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>visualization_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>MarkerArray<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;~\/frontiers&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">10<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  tree_marker_pub_ <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">create_publisher<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>visualization_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Marker<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;~\/rig_tree&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">10<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  nav_action_client_ <span class=\"token operator\">&#061;<\/span> rclcpp_action<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">create_client<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>NavigateToPose<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">this<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;navigate_to_pose&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>node_interfaces<span class=\"token double-colon punctuation\">::<\/span><br \/>\n    LifecycleNodeInterface<span class=\"token double-colon punctuation\">::<\/span>CallbackReturn<span class=\"token double-colon punctuation\">::<\/span>SUCCESS<span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p>rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>node_interfaces<span class=\"token double-colon punctuation\">::<\/span>LifecycleNodeInterface<span class=\"token double-colon punctuation\">::<\/span>CallbackReturn<br \/>\n<span class=\"token class-name\">ExplorationNode<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">on_activate<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>State <span class=\"token operator\">&amp;<\/span> <span class=\"token comment\">\/*state*\/<\/span><span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  marker_pub_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">on_activate<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  tree_marker_pub_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">on_activate<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  map_sub_ <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">create_subscription<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><br \/>\n    map_topic_<span class=\"token punctuation\">,<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">QoS<\/span><span class=\"token punctuation\">(<\/span>rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">KeepLast<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">reliable<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">bind<\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">&amp;<\/span>ExplorationNode<span class=\"token double-colon punctuation\">::<\/span>mapCallback<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">this<\/span><span class=\"token punctuation\">,<\/span> std<span class=\"token double-colon punctuation\">::<\/span>placeholders<span class=\"token double-colon punctuation\">::<\/span>_1<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  prob_map_sub_ <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">create_subscription<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>  <span class=\"token comment\">\/\/ \u65b0\u589e<\/span><br \/>\n    prob_map_topic_<span class=\"token punctuation\">,<\/span><br \/>\n    rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">QoS<\/span><span class=\"token punctuation\">(<\/span>rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">KeepLast<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">transient_local<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">reliable<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">bind<\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">&amp;<\/span>ExplorationNode<span class=\"token double-colon punctuation\">::<\/span>probMapCallback<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">this<\/span><span class=\"token punctuation\">,<\/span> std<span class=\"token double-colon punctuation\">::<\/span>placeholders<span class=\"token double-colon punctuation\">::<\/span>_1<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  timer_ <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">create_wall_timer<\/span><span class=\"token punctuation\">(<\/span>std<span class=\"token double-colon punctuation\">::<\/span>chrono<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">milliseconds<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">500<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    std<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">bind<\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">&amp;<\/span>ExplorationNode<span class=\"token double-colon punctuation\">::<\/span>timerCallback<span class=\"token punctuation\">,<\/span> <span class=\"token keyword\">this<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>  state_ <span class=\"token operator\">&#061;<\/span> State<span class=\"token double-colon punctuation\">::<\/span>IDLE<span class=\"token punctuation\">;<\/span><br \/>\n  activation_time_ <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">get_clock<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token operator\">-&gt;<\/span><span class=\"token function\">now<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">return<\/span> rclcpp_lifecycle<span class=\"token double-colon punctuation\">::<\/span>node_interfaces<span class=\"token double-colon punctuation\">::<\/span><br \/>\n    LifecycleNodeInterface<span class=\"token double-colon punctuation\">::<\/span>CallbackReturn<span class=\"token double-colon punctuation\">::<\/span>SUCCESS<span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token comment\">\/\/ &#8230; (on_deactivate, on_cleanup, stateToString \u2014 \u540c\u5f80\u671f&#xff0c;\u7565)<\/span><\/p>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token class-name\">ExplorationNode<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">mapCallback<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token double-colon punctuation\">::<\/span>SharedPtr msg<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span> last_map_ <span class=\"token operator\">&#061;<\/span> <span class=\"token operator\">*<\/span>msg<span class=\"token punctuation\">;<\/span> map_received_ <span class=\"token operator\">&#061;<\/span> <span class=\"token boolean\">true<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token class-name\">ExplorationNode<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">probMapCallback<\/span><span class=\"token punctuation\">(<\/span>                                    <span class=\"token comment\">\/\/ \u65b0\u589e<\/span><br \/>\n  <span class=\"token keyword\">const<\/span> nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>OccupancyGrid<span class=\"token double-colon punctuation\">::<\/span>SharedPtr msg<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span> last_prob_map_ <span class=\"token operator\">&#061;<\/span> <span class=\"token operator\">*<\/span>msg<span class=\"token punctuation\">;<\/span> prob_map_received_ <span class=\"token operator\">&#061;<\/span> <span class=\"token boolean\">true<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token class-name\">ExplorationNode<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">selectAndSendGoal<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n  state_ <span class=\"token operator\">&#061;<\/span> State<span class=\"token double-colon punctuation\">::<\/span>SELECT<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">auto<\/span> frontiers <span class=\"token operator\">&#061;<\/span> frontier_detector_<span class=\"token punctuation\">.<\/span><span class=\"token function\">detect<\/span><span class=\"token punctuation\">(<\/span>last_map_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>frontiers<span class=\"token punctuation\">.<\/span><span class=\"token function\">empty<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span> state_ <span class=\"token operator\">&#061;<\/span> State<span class=\"token double-colon punctuation\">::<\/span>DONE<span class=\"token punctuation\">;<\/span> <span class=\"token keyword\">return<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token punctuation\">}<\/span><\/p>\n<p>  <span class=\"token keyword\">try<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token comment\">\/\/ MI \u9009\u62e9\u5668\u6ce8\u5165\u6982\u7387\u5730\u56fe                                    \/\/ \u65b0\u589e<\/span><br \/>\n    <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>goal_selector_type_ <span class=\"token operator\">&#061;&#061;<\/span> <span class=\"token string\">&#034;mi&#034;<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n      <span class=\"token keyword\">auto<\/span> <span class=\"token operator\">*<\/span> mi_sel <span class=\"token operator\">&#061;<\/span> <span class=\"token generic-function\"><span class=\"token function\">dynamic_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>MIGoalSelector <span class=\"token operator\">*<\/span><span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>goal_selector_<span class=\"token punctuation\">.<\/span><span class=\"token function\">get<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n      <span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>mi_sel<span class=\"token punctuation\">)<\/span> mi_sel<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">setProbMap<\/span><span class=\"token punctuation\">(<\/span>last_prob_map_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><br \/>\n    <span class=\"token keyword\">auto<\/span> selected <span class=\"token operator\">&#061;<\/span> goal_selector_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">select<\/span><span class=\"token punctuation\">(<\/span>frontiers<span class=\"token punctuation\">,<\/span> current_pose_<span class=\"token punctuation\">,<\/span> last_map_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    selected_goal_pos_ <span class=\"token operator\">&#061;<\/span> selected<span class=\"token punctuation\">.<\/span>closest<span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token keyword\">auto<\/span> goal_pose <span class=\"token operator\">&#061;<\/span> goal_selector_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">clusterToGoal<\/span><span class=\"token punctuation\">(<\/span>selected<span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;map&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token function\">get_clock<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token operator\">-&gt;<\/span><span class=\"token function\">now<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    goal_pose<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>position <span class=\"token operator\">&#061;<\/span> selected<span class=\"token punctuation\">.<\/span>closest<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token function\">sendGoal<\/span><span class=\"token punctuation\">(<\/span>goal_pose<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span> <span class=\"token keyword\">catch<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token keyword\">const<\/span> std<span class=\"token double-colon punctuation\">::<\/span>exception <span class=\"token operator\">&amp;<\/span> e<span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token function\">RCLCPP_ERROR<\/span><span class=\"token punctuation\">(<\/span><span class=\"token function\">get_logger<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;Goal selection failed: %s&#034;<\/span><span class=\"token punctuation\">,<\/span> e<span class=\"token punctuation\">.<\/span><span class=\"token function\">what<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    state_ <span class=\"token operator\">&#061;<\/span> State<span class=\"token double-colon punctuation\">::<\/span>DONE<span class=\"token punctuation\">;<\/span><br \/>\n  <span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p><span class=\"token comment\">\/\/ &#8230; (timerCallback, updateRobotPose, sendGoal, action callbacks, publishMarkers \u2014 \u540c\u5f80\u671f&#xff0c;\u7565)<\/span><\/p>\n<p><span class=\"token punctuation\">}<\/span>  <span class=\"token comment\">\/\/ namespace exploration<\/span><\/p>\n<ul>\n<li>\u6ce8&#xff1a;exploration_node.cpp \u5b8c\u6574\u7684 550 \u884c\u4ee3\u7801\u8bf7\u53c2\u8003\u9879\u76ee\u4ed3\u5e93 ros2_ws\/src\/exploration\/src\/exploration_node.cpp&#xff0c;\u672c\u6587\u4e3a\u8282\u7701\u7bc7\u5e45\u4ec5\u5217\u51fa\u672c\u671f\u4fee\u6539\u7684\u5173\u952e\u51fd\u6570<\/li>\n<\/ul>\n<h6>5_prob_map.sh<\/h6>\n<p><span class=\"token shebang important\">#!\/bin\/bash<\/span><br \/>\n<span class=\"token comment\"># \u6587\u4ef6: 5_prob_map.sh<\/span><br \/>\n<span class=\"token comment\"># slam_toolbox &#043; \u6982\u7387\u5730\u56fe &#043; RViz \u542f\u52a8\u811a\u672c<\/span><\/p>\n<p><span class=\"token function\">pkill<\/span> <span class=\"token parameter variable\">-f<\/span> <span class=\"token string\">&#034;prob_mapper&#034;<\/span> <span class=\"token operator\"><span class=\"token file-descriptor important\">2<\/span>&gt;<\/span>\/dev\/null<br \/>\n<span class=\"token function\">pkill<\/span> <span class=\"token parameter variable\">-f<\/span> <span class=\"token string\">&#034;slam.launch&#034;<\/span> <span class=\"token operator\"><span class=\"token file-descriptor important\">2<\/span>&gt;<\/span>\/dev\/null<br \/>\n<span class=\"token function\">pkill<\/span> <span class=\"token parameter variable\">-f<\/span> <span class=\"token string\">&#034;rviz2&#034;<\/span> <span class=\"token operator\"><span class=\"token file-descriptor important\">2<\/span>&gt;<\/span>\/dev\/null<br \/>\n<span class=\"token function\">sleep<\/span> <span class=\"token number\">1<\/span><\/p>\n<p><span class=\"token builtin class-name\">source<\/span> \/opt\/ros\/humble\/setup.bash<br \/>\n<span class=\"token builtin class-name\">source<\/span> .\/install\/setup.bash<\/p>\n<p><span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#034;<\/span><br \/>\n<span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034; SLAM      : slam_toolbox (online_async)&#034;<\/span><br \/>\n<span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034; \u6982\u7387\u5730\u56fe  : prob_mapper (100m x 100m)&#034;<\/span><br \/>\n<span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;   \/prob_map        continuous probability [0,100]&#034;<\/span><br \/>\n<span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;   \/thresholded_map ternary map [-1\/0\/100]&#034;<\/span><br \/>\n<span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;   \/entropy_map     Shannon entropy heatmap&#034;<\/span><br \/>\n<span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#034;<\/span><\/p>\n<p>ros2 launch prob_map slam_prob_mapper.launch.py <span class=\"token punctuation\">\\\\<\/span><br \/>\n  sim:<span class=\"token operator\">&#061;<\/span>true <span class=\"token punctuation\">\\\\<\/span><br \/>\n  rviz:<span class=\"token operator\">&#061;<\/span>true <span class=\"token punctuation\">\\\\<\/span><br \/>\n  prob_map_size:<span class=\"token operator\">&#061;<\/span><span class=\"token number\">100.0<\/span> <span class=\"token punctuation\">\\\\<\/span><br \/>\n  prob_hit:<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.55<\/span> <span class=\"token punctuation\">\\\\<\/span><br \/>\n  prob_miss:<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.45<\/span> <span class=\"token punctuation\">\\\\<\/span><br \/>\n  max_range:<span class=\"token operator\">&#061;<\/span><span class=\"token number\">30.0<\/span> <span class=\"token punctuation\">\\\\<\/span><br \/>\n  publish_entropy:<span class=\"token operator\">&#061;<\/span>true<\/p>\n<h6>7_mi_explore.sh<\/h6>\n<p><span class=\"token shebang important\">#!\/bin\/bash<\/span><br \/>\n<span class=\"token comment\"># \u6587\u4ef6: 7_mi_explore.sh<\/span><br \/>\n<span class=\"token comment\"># MI \u4e92\u4fe1\u606f\u63a2\u7d22 \u2014 \u9700\u5148\u542f\u52a8 5_prob_map.sh<\/span><\/p>\n<p><span class=\"token function\">pkill<\/span> <span class=\"token parameter variable\">-f<\/span> <span class=\"token string\">&#034;exploration_node&#034;<\/span> <span class=\"token operator\"><span class=\"token file-descriptor important\">2<\/span>&gt;<\/span>\/dev\/null<br \/>\n<span class=\"token function\">sleep<\/span> <span class=\"token number\">1<\/span><\/p>\n<p><span class=\"token builtin class-name\">source<\/span> \/opt\/ros\/humble\/setup.bash<br \/>\n<span class=\"token builtin class-name\">source<\/span> .\/install\/setup.bash<\/p>\n<p><span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#034;<\/span><br \/>\n<span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034; MI \u4e92\u4fe1\u606f\u63a2\u7d22\u6a21\u5f0f&#034;<\/span><br \/>\n<span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034; \u524d\u7f6e: .\/5_prob_map.sh \u5df2\u8fd0\u884c&#034;<\/span><br \/>\n<span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#034;<\/span><\/p>\n<p>ros2 run exploration explorer &#8211;ros-args <span class=\"token punctuation\">\\\\<\/span><br \/>\n  <span class=\"token parameter variable\">-p<\/span> use_sim_time:<span class=\"token operator\">&#061;<\/span>true <span class=\"token punctuation\">\\\\<\/span><br \/>\n  <span class=\"token parameter variable\">-p<\/span> map_topic:<span class=\"token operator\">&#061;<\/span>\/map <span class=\"token punctuation\">\\\\<\/span><br \/>\n  <span class=\"token parameter variable\">-p<\/span> prob_map_topic:<span class=\"token operator\">&#061;<\/span>\/prob_map <span class=\"token punctuation\">\\\\<\/span><br \/>\n  <span class=\"token parameter variable\">-p<\/span> goal_selector_type:<span class=\"token operator\">&#061;<\/span>mi <span class=\"token punctuation\">\\\\<\/span><br \/>\n  <span class=\"token parameter variable\">-p<\/span> min_frontier_size:<span class=\"token operator\">&#061;<\/span><span class=\"token number\">5<\/span> <span class=\"token punctuation\">\\\\<\/span><br \/>\n  <span class=\"token parameter variable\">-p<\/span> goal_reached_dist:<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">&amp;<\/span><\/p>\n<p><span class=\"token assign-left variable\">EXPLORE_PID<\/span><span class=\"token operator\">&#061;<\/span><span class=\"token variable\">$!<\/span><\/p>\n<p><span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;Waiting for exploration node (5s)&#8230;&#034;<\/span><br \/>\n<span class=\"token function\">sleep<\/span> <span class=\"token number\">5<\/span><\/p>\n<p><span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;Activating lifecycle&#8230;&#034;<\/span><br \/>\nros2 lifecycle <span class=\"token builtin class-name\">set<\/span> exploration_node configure <span class=\"token operator\"><span class=\"token file-descriptor important\">2<\/span>&gt;<\/span>\/dev\/null<br \/>\n<span class=\"token function\">sleep<\/span> <span class=\"token number\">1<\/span><br \/>\nros2 lifecycle <span class=\"token builtin class-name\">set<\/span> exploration_node activate <span class=\"token operator\"><span class=\"token file-descriptor important\">2<\/span>&gt;<\/span>\/dev\/null<\/p>\n<p><span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;&#034;<\/span><br \/>\n<span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061; MI Exploration Running &#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#034;<\/span><br \/>\n<span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;Ctrl&#043;C to stop&#034;<\/span><\/p>\n<p><span class=\"token function\">wait<\/span> <span class=\"token variable\">$EXPLORE_PID<\/span><\/p>\n<hr \/>\n<h3>5 \u5b8c\u6574\u53c2\u6570\u4e0e\u5bf9\u6bd4<\/h3>\n<h5>5-1 prob_mapper \u53c2\u6570\u8868<\/h5>\n<table>\n<tr>\u53c2\u6570\u9ed8\u8ba4\u503c\u542b\u4e49<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">map_width_m<\/td>\n<td align=\"left\">100.0<\/td>\n<td align=\"left\">\u5730\u56fe\u5bbd\u5ea6&#xff08;m&#xff09;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">map_height_m<\/td>\n<td align=\"left\">100.0<\/td>\n<td align=\"left\">\u5730\u56fe\u9ad8\u5ea6&#xff08;m&#xff09;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">resolution<\/td>\n<td align=\"left\">0.05<\/td>\n<td align=\"left\">\u5206\u8fa8\u7387&#xff08;m\/cell&#xff09;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">prob_hit<\/td>\n<td align=\"left\">0.55<\/td>\n<td align=\"left\">\u9006\u4f20\u611f\u5668\u6a21\u578b P(occ | hit)<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">prob_miss<\/td>\n<td align=\"left\">0.45<\/td>\n<td align=\"left\">\u9006\u4f20\u611f\u5668\u6a21\u578b P(occ | miss)<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">max_range<\/td>\n<td align=\"left\">30.0<\/td>\n<td align=\"left\">\u6fc0\u5149\u6700\u5927\u4f7f\u7528\u8ddd\u79bb&#xff08;m&#xff09;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">min_range<\/td>\n<td align=\"left\">0.15<\/td>\n<td align=\"left\">\u6fc0\u5149\u6700\u5c0f\u8ddd\u79bb&#xff08;m&#xff09;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">publish_rate<\/td>\n<td align=\"left\">5.0<\/td>\n<td align=\"left\">\u5730\u56fe\u53d1\u5e03\u9891\u7387&#xff08;Hz&#xff09;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">publish_entropy<\/td>\n<td align=\"left\">true<\/td>\n<td align=\"left\">\u662f\u5426\u989d\u5916\u53d1\u5e03 \/entropy_map<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">laser_topic<\/td>\n<td align=\"left\">\/scan<\/td>\n<td align=\"left\">\u6fc0\u5149\u8bdd\u9898\u540d<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">map_frame<\/td>\n<td align=\"left\">map<\/td>\n<td align=\"left\">\u5730\u56fe\u5750\u6807\u7cfb\u540d<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<h5>5-2 exploration_node MI \u76f8\u5173\u53c2\u6570<\/h5>\n<table>\n<tr>\u53c2\u6570\u9ed8\u8ba4\u503c\u542b\u4e49<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">goal_selector_type<\/td>\n<td align=\"left\">&#034;nearest&#034;<\/td>\n<td align=\"left\">\u6539\u4e3a &#034;mi&#034; \u542f\u7528 MI \u63a2\u7d22<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">prob_map_topic<\/td>\n<td align=\"left\">\/prob_map<\/td>\n<td align=\"left\">\u6982\u7387\u5730\u56fe\u8bdd\u9898\u540d<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">min_frontier_size<\/td>\n<td align=\"left\">5<\/td>\n<td align=\"left\">\u524d\u6cbf\u6700\u5c11 cell \u6570<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">goal_reached_dist<\/td>\n<td align=\"left\">1.0<\/td>\n<td align=\"left\">\u76ee\u6807\u5230\u8fbe\u5224\u636e&#xff08;m&#xff09;<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<h5>5-3 \u56db\u79cd\u63a2\u7d22\u7b56\u7565\u5bf9\u6bd4<\/h5>\n<table>\n<tr>\u7b56\u7565\u5730\u56fe\u4fe1\u606f\u589e\u76ca\u8def\u5f84\u4ee3\u4ef7\u906e\u6321\u5efa\u6a21<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">Nearest<\/td>\n<td align=\"left\">\u4e09\u503c -1\/0\/100<\/td>\n<td align=\"left\">\u65e0<\/td>\n<td align=\"left\">\u6b27\u6c0f\u8ddd\u79bb<\/td>\n<td align=\"center\">\u65e0<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">Utility<\/td>\n<td align=\"left\">\u4e09\u503c -1\/0\/100<\/td>\n<td align=\"left\">4 \u90bb\u57df UNKNOWN \u8ba1\u6570<\/td>\n<td align=\"left\">Dijkstra \u6ce2\u524d<\/td>\n<td align=\"center\">\u65e0<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">RIG<\/td>\n<td align=\"left\">\u4e09\u503c -1\/0\/100<\/td>\n<td align=\"left\">4 \u90bb\u57df UNKNOWN \u8ba1\u6570<\/td>\n<td align=\"left\">RRT \u6811\u8def\u5f84<\/td>\n<td align=\"center\">\u65e0<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">MI&#xff08;\u672c\u671f&#xff09;<\/td>\n<td align=\"left\">\u8fde\u7eed\u6982\u7387 [0, 100]<\/td>\n<td align=\"left\">\u865a\u62df\u5149\u675f\u9999\u519c\u71b5\u7d2f\u52a0<\/td>\n<td align=\"left\">Dijkstra \u6ce2\u524d<\/td>\n<td align=\"center\">\u6709&#xff08;P&gt;0.7 \u505c\u6b62\u5c04\u7ebf&#xff09;<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<hr \/>\n<h3>6 Python \u53ef\u89c6\u5316\u4ee3\u7801<\/h3>\n<ul>\n<li>\u672c\u6587\u4e2d\u6240\u6709\u63d2\u56fe\u5747\u7531\u4ee5\u4e0b\u4e09\u4e2a Python \u811a\u672c\u751f\u6210&#xff0c;\u53ef\u76f4\u63a5\u8fd0\u884c<\/li>\n<\/ul>\n<h5>6-1 \u4e09\u503c vs \u6982\u7387 vs \u71b5 \u5bf9\u6bd4\u56fe&#xff08;1-2 \u8282&#xff09;<\/h5>\n<ul>\n<li>\u751f\u6210\u7b2c 1-2 \u8282\u4e2d\u4e09\u503c\u5730\u56fe\u3001\u6982\u7387\u5730\u56fe\u3001\u71b5\u5730\u56fe\u7684\u4e09\u680f\u5bf9\u6bd4\u56fe<\/li>\n<\/ul>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160857-6a6a25991ca0c.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<p><span class=\"token triple-quoted-string string\">&#034;&#034;&#034;\u4e09\u503c\u5730\u56fe vs \u6982\u7387\u5730\u56fe vs \u71b5\u5730\u56fe \u5bf9\u6bd4\u53ef\u89c6\u5316<\/p>\n<p>\u7528\u6cd5: python3 ternary_vs_prob.py<br \/>\n\u8f93\u51fa: ternary_vs_prob_comparison.png<br \/>\n&#034;&#034;&#034;<\/span><\/p>\n<p><span class=\"token keyword\">import<\/span> numpy <span class=\"token keyword\">as<\/span> np<br \/>\n<span class=\"token keyword\">import<\/span> matplotlib<span class=\"token punctuation\">.<\/span>pyplot <span class=\"token keyword\">as<\/span> plt<br \/>\n<span class=\"token keyword\">from<\/span> matplotlib <span class=\"token keyword\">import<\/span> colors<br \/>\n<span class=\"token keyword\">import<\/span> matplotlib<span class=\"token punctuation\">.<\/span>patches <span class=\"token keyword\">as<\/span> mpatches<\/p>\n<p><span class=\"token keyword\">def<\/span> <span class=\"token function\">build_scenario<\/span><span class=\"token punctuation\">(<\/span>size<span class=\"token operator\">&#061;<\/span><span class=\"token number\">200<\/span><span class=\"token punctuation\">,<\/span> res<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    <span class=\"token triple-quoted-string string\">&#034;&#034;&#034;\u6784\u5efa\u90e8\u5206\u63a2\u7d22\u573a\u666f\u2014\u2014\u673a\u5668\u4eba\u4ece\u4e2d\u5fc3\u51fa\u53d1\u626b\u63cf\u4e00\u5b9a\u8303\u56f4&#034;&#034;&#034;<\/span><br \/>\n    h<span class=\"token punctuation\">,<\/span> w <span class=\"token operator\">&#061;<\/span> size<span class=\"token punctuation\">,<\/span> size<br \/>\n    prob <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>full<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>h<span class=\"token punctuation\">,<\/span> w<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.5<\/span><span class=\"token punctuation\">,<\/span> dtype<span class=\"token operator\">&#061;<\/span><span class=\"token builtin\">float<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    cx<span class=\"token punctuation\">,<\/span> cy <span class=\"token operator\">&#061;<\/span> w <span class=\"token operator\">\/\/<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">,<\/span> h <span class=\"token operator\">\/\/<\/span> <span class=\"token number\">2<\/span><\/p>\n<p>    <span class=\"token comment\"># \u969c\u788d\u7269<\/span><br \/>\n    wall1_y <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">40<\/span><br \/>\n    prob<span class=\"token punctuation\">[<\/span>wall1_y <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">:<\/span>wall1_y <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">30<\/span><span class=\"token punctuation\">:<\/span><span class=\"token number\">70<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.95<\/span><br \/>\n    wall2_x <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">140<\/span><br \/>\n    prob<span class=\"token punctuation\">[<\/span><span class=\"token number\">40<\/span><span class=\"token punctuation\">:<\/span><span class=\"token number\">80<\/span><span class=\"token punctuation\">,<\/span> wall2_x <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">:<\/span>wall2_x <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.95<\/span><br \/>\n    prob<span class=\"token punctuation\">[<\/span><span class=\"token number\">120<\/span><span class=\"token punctuation\">:<\/span><span class=\"token number\">130<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">90<\/span><span class=\"token punctuation\">:<\/span><span class=\"token number\">100<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.95<\/span><\/p>\n<p>    <span class=\"token comment\"># \u5df2\u626b\u63cf\u7a7a\u95f2\u533a\u57df&#xff08;\u4e2d\u5fc3\u5706&#xff09;<\/span><br \/>\n    free_radius <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">55<\/span><br \/>\n    yy<span class=\"token punctuation\">,<\/span> xx <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>ogrid<span class=\"token punctuation\">[<\/span><span class=\"token punctuation\">:<\/span>h<span class=\"token punctuation\">,<\/span> <span class=\"token punctuation\">:<\/span>w<span class=\"token punctuation\">]<\/span><br \/>\n    dist <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>sqrt<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>xx <span class=\"token operator\">&#8211;<\/span> cx<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">**<\/span> <span class=\"token number\">2<\/span> <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span>yy <span class=\"token operator\">&#8211;<\/span> cy<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">**<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    free_mask <span class=\"token operator\">&#061;<\/span> dist <span class=\"token operator\">&lt;<\/span> free_radius<br \/>\n    prob<span class=\"token punctuation\">[<\/span>free_mask<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">-&#061;<\/span> <span class=\"token number\">0.4<\/span><br \/>\n    np<span class=\"token punctuation\">.<\/span>clip<span class=\"token punctuation\">(<\/span>prob<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.01<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.99<\/span><span class=\"token punctuation\">,<\/span> out<span class=\"token operator\">&#061;<\/span>prob<span class=\"token punctuation\">)<\/span><\/p>\n<p>    <span class=\"token comment\"># \u524d\u6cbf\u6e10\u53d8\u5e26<\/span><br \/>\n    frontier_inner<span class=\"token punctuation\">,<\/span> frontier_outer <span class=\"token operator\">&#061;<\/span> free_radius <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">8<\/span><span class=\"token punctuation\">,<\/span> free_radius <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">8<\/span><br \/>\n    ring <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>dist <span class=\"token operator\">&gt;&#061;<\/span> frontier_inner<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&amp;<\/span> <span class=\"token punctuation\">(<\/span>dist <span class=\"token operator\">&lt;<\/span> frontier_outer<span class=\"token punctuation\">)<\/span><br \/>\n    blend <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>dist<span class=\"token punctuation\">[<\/span>ring<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#8211;<\/span> frontier_inner<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> <span class=\"token punctuation\">(<\/span>frontier_outer <span class=\"token operator\">&#8211;<\/span> frontier_inner<span class=\"token punctuation\">)<\/span><br \/>\n    prob<span class=\"token punctuation\">[<\/span>ring<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> prob<span class=\"token punctuation\">[<\/span>ring<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">*<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token number\">1<\/span> <span class=\"token operator\">&#8211;<\/span> blend<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">0.5<\/span> <span class=\"token operator\">*<\/span> blend<\/p>\n<p>    <span class=\"token comment\"># \u969c\u788d\u7269\u4fdd\u6301\u9ad8\u6982\u7387<\/span><br \/>\n    prob<span class=\"token punctuation\">[<\/span>wall1_y <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">:<\/span>wall1_y <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">30<\/span><span class=\"token punctuation\">:<\/span><span class=\"token number\">70<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.95<\/span><br \/>\n    prob<span class=\"token punctuation\">[<\/span><span class=\"token number\">40<\/span><span class=\"token punctuation\">:<\/span><span class=\"token number\">80<\/span><span class=\"token punctuation\">,<\/span> wall2_x <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">:<\/span>wall2_x <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.95<\/span><br \/>\n    prob<span class=\"token punctuation\">[<\/span><span class=\"token number\">120<\/span><span class=\"token punctuation\">:<\/span><span class=\"token number\">130<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">90<\/span><span class=\"token punctuation\">:<\/span><span class=\"token number\">100<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.95<\/span><br \/>\n    <span class=\"token keyword\">return<\/span> prob<\/p>\n<p><span class=\"token keyword\">def<\/span> <span class=\"token function\">prob_to_ternary<\/span><span class=\"token punctuation\">(<\/span>prob<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    <span class=\"token triple-quoted-string string\">&#034;&#034;&#034;\u6982\u7387 -&gt; \u4e09\u503c&#034;&#034;&#034;<\/span><br \/>\n    ternary <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>full_like<span class=\"token punctuation\">(<\/span>prob<span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> dtype<span class=\"token operator\">&#061;<\/span><span class=\"token builtin\">int<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ternary<span class=\"token punctuation\">[<\/span>prob <span class=\"token operator\">&lt;<\/span> <span class=\"token number\">0.30<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0<\/span><br \/>\n    ternary<span class=\"token punctuation\">[<\/span>prob <span class=\"token operator\">&gt;<\/span> <span class=\"token number\">0.70<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">100<\/span><br \/>\n    <span class=\"token keyword\">return<\/span> ternary<\/p>\n<p><span class=\"token keyword\">def<\/span> <span class=\"token function\">entropy<\/span><span class=\"token punctuation\">(<\/span>p<span class=\"token punctuation\">,<\/span> eps<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1e-9<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    p <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>clip<span class=\"token punctuation\">(<\/span>p<span class=\"token punctuation\">,<\/span> eps<span class=\"token punctuation\">,<\/span> <span class=\"token number\">1<\/span> <span class=\"token operator\">&#8211;<\/span> eps<span class=\"token punctuation\">)<\/span><br \/>\n    <span class=\"token keyword\">return<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">*<\/span> np<span class=\"token punctuation\">.<\/span>log2<span class=\"token punctuation\">(<\/span>p<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token number\">1<\/span> <span class=\"token operator\">&#8211;<\/span> p<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> np<span class=\"token punctuation\">.<\/span>log2<span class=\"token punctuation\">(<\/span><span class=\"token number\">1<\/span> <span class=\"token operator\">&#8211;<\/span> p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p><span class=\"token keyword\">def<\/span> <span class=\"token function\">main<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    prob <span class=\"token operator\">&#061;<\/span> build_scenario<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ternary <span class=\"token operator\">&#061;<\/span> prob_to_ternary<span class=\"token punctuation\">(<\/span>prob<span class=\"token punctuation\">)<\/span><br \/>\n    ent <span class=\"token operator\">&#061;<\/span> entropy<span class=\"token punctuation\">(<\/span>prob<span class=\"token punctuation\">)<\/span><\/p>\n<p>    fig<span class=\"token punctuation\">,<\/span> axes <span class=\"token operator\">&#061;<\/span> plt<span class=\"token punctuation\">.<\/span>subplots<span class=\"token punctuation\">(<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">3<\/span><span class=\"token punctuation\">,<\/span> figsize<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">18<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">5.8<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    extent <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">[<\/span><span class=\"token operator\">&#8211;<\/span><span class=\"token number\">10<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">10<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">10<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">10<\/span><span class=\"token punctuation\">]<\/span><\/p>\n<p>    <span class=\"token comment\"># (a) \u4e09\u503c\u5730\u56fe<\/span><br \/>\n    ax0 <span class=\"token operator\">&#061;<\/span> axes<span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span><br \/>\n    cmap_tern <span class=\"token operator\">&#061;<\/span> colors<span class=\"token punctuation\">.<\/span>ListedColormap<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">[<\/span><span class=\"token string\">&#039;#888888&#039;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;#f8f8f8&#039;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;#1a1a1a&#039;<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    bounds_tern <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">[<\/span><span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1.5<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">0.5<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.5<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">100.5<\/span><span class=\"token punctuation\">]<\/span><br \/>\n    norm_tern <span class=\"token operator\">&#061;<\/span> colors<span class=\"token punctuation\">.<\/span>BoundaryNorm<span class=\"token punctuation\">(<\/span>bounds_tern<span class=\"token punctuation\">,<\/span> cmap_tern<span class=\"token punctuation\">.<\/span>N<span class=\"token punctuation\">)<\/span><br \/>\n    ax0<span class=\"token punctuation\">.<\/span>imshow<span class=\"token punctuation\">(<\/span>ternary<span class=\"token punctuation\">,<\/span> extent<span class=\"token operator\">&#061;<\/span>extent<span class=\"token punctuation\">,<\/span> origin<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;lower&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n               cmap<span class=\"token operator\">&#061;<\/span>cmap_tern<span class=\"token punctuation\">,<\/span> norm<span class=\"token operator\">&#061;<\/span>norm_tern<span class=\"token punctuation\">,<\/span> interpolation<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;nearest&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax0<span class=\"token punctuation\">.<\/span>set_title<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;OccupancyGrid (ternary)&#034;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax0<span class=\"token punctuation\">.<\/span>set_xlabel<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;X [m]&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> ax0<span class=\"token punctuation\">.<\/span>set_ylabel<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;Y [m]&#034;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    legend_tern <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">[<\/span><br \/>\n        mpatches<span class=\"token punctuation\">.<\/span>Patch<span class=\"token punctuation\">(<\/span>color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;#f8f8f8&#039;<\/span><span class=\"token punctuation\">,<\/span> label<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;FREE (0)&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        mpatches<span class=\"token punctuation\">.<\/span>Patch<span class=\"token punctuation\">(<\/span>color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;#1a1a1a&#039;<\/span><span class=\"token punctuation\">,<\/span> label<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;OCCUPIED (100)&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        mpatches<span class=\"token punctuation\">.<\/span>Patch<span class=\"token punctuation\">(<\/span>color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;#888888&#039;<\/span><span class=\"token punctuation\">,<\/span> label<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;UNKNOWN (-1)&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token punctuation\">]<\/span><br \/>\n    ax0<span class=\"token punctuation\">.<\/span>legend<span class=\"token punctuation\">(<\/span>handles<span class=\"token operator\">&#061;<\/span>legend_tern<span class=\"token punctuation\">,<\/span> loc<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;lower left&#039;<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">9<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    <span class=\"token comment\"># (b) \u8fde\u7eed\u6982\u7387\u5730\u56fe<\/span><br \/>\n    ax1 <span class=\"token operator\">&#061;<\/span> axes<span class=\"token punctuation\">[<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">]<\/span><br \/>\n    im1 <span class=\"token operator\">&#061;<\/span> ax1<span class=\"token punctuation\">.<\/span>imshow<span class=\"token punctuation\">(<\/span>prob<span class=\"token punctuation\">,<\/span> extent<span class=\"token operator\">&#061;<\/span>extent<span class=\"token punctuation\">,<\/span> origin<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;lower&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                     cmap<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;Greys&#039;<\/span><span class=\"token punctuation\">,<\/span> vmin<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> vmax<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> interpolation<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;bilinear&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax1<span class=\"token punctuation\">.<\/span>set_title<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;Probability Grid  (continuous 0~1)&#034;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax1<span class=\"token punctuation\">.<\/span>set_xlabel<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;X [m]&#034;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    plt<span class=\"token punctuation\">.<\/span>colorbar<span class=\"token punctuation\">(<\/span>im1<span class=\"token punctuation\">,<\/span> ax<span class=\"token operator\">&#061;<\/span>ax1<span class=\"token punctuation\">,<\/span> fraction<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.046<\/span><span class=\"token punctuation\">,<\/span> pad<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.04<\/span><span class=\"token punctuation\">,<\/span> label<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#034;P(occ)&#034;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    <span class=\"token comment\"># (c) \u71b5\u5730\u56fe<\/span><br \/>\n    ax2 <span class=\"token operator\">&#061;<\/span> axes<span class=\"token punctuation\">[<\/span><span class=\"token number\">2<\/span><span class=\"token punctuation\">]<\/span><br \/>\n    im2 <span class=\"token operator\">&#061;<\/span> ax2<span class=\"token punctuation\">.<\/span>imshow<span class=\"token punctuation\">(<\/span>ent<span class=\"token punctuation\">,<\/span> extent<span class=\"token operator\">&#061;<\/span>extent<span class=\"token punctuation\">,<\/span> origin<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;lower&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                     cmap<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;hot&#039;<\/span><span class=\"token punctuation\">,<\/span> vmin<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> vmax<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> interpolation<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;bilinear&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax2<span class=\"token punctuation\">.<\/span>set_title<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;Entropy  H &#061; -p log2(p) &#8211; (1-p) log2(1-p)&#034;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax2<span class=\"token punctuation\">.<\/span>set_xlabel<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;X [m]&#034;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    plt<span class=\"token punctuation\">.<\/span>colorbar<span class=\"token punctuation\">(<\/span>im2<span class=\"token punctuation\">,<\/span> ax<span class=\"token operator\">&#061;<\/span>ax2<span class=\"token punctuation\">,<\/span> fraction<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.046<\/span><span class=\"token punctuation\">,<\/span> pad<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.04<\/span><span class=\"token punctuation\">,<\/span> label<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#034;H [bit]&#034;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    plt<span class=\"token punctuation\">.<\/span>tight_layout<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    plt<span class=\"token punctuation\">.<\/span>savefig<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;ternary_vs_prob_comparison.png&#034;<\/span><span class=\"token punctuation\">,<\/span> dpi<span class=\"token operator\">&#061;<\/span><span class=\"token number\">200<\/span><span class=\"token punctuation\">,<\/span> bbox_inches<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;tight&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    <span class=\"token keyword\">print<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;Saved -&gt; ternary_vs_prob_comparison.png&#034;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p><span class=\"token keyword\">if<\/span> __name__ <span class=\"token operator\">&#061;&#061;<\/span> <span class=\"token string\">&#039;__main__&#039;<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    main<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<h5>6-2 log-odds \u51fd\u6570\u66f2\u7ebf&#xff08;2-1-1 \u8282&#xff09;<\/h5>\n<ul>\n<li>\u751f\u6210 log-odds \u2194 probability \u7684\u53cc\u4fa7\u66f2\u7ebf\u56fe&#xff08;sigmoid &#043; logit&#xff09;<\/li>\n<\/ul>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160858-6a6a259ac88e5.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<p><span class=\"token triple-quoted-string string\">&#034;&#034;&#034;log-odds \u51fd\u6570\u66f2\u7ebf<\/p>\n<p>\u7528\u6cd5: python3 log_odds_curve.py<br \/>\n\u8f93\u51fa: log_odds_curve.png<br \/>\n&#034;&#034;&#034;<\/span><\/p>\n<p><span class=\"token keyword\">import<\/span> numpy <span class=\"token keyword\">as<\/span> np<br \/>\n<span class=\"token keyword\">import<\/span> matplotlib<span class=\"token punctuation\">.<\/span>pyplot <span class=\"token keyword\">as<\/span> plt<br \/>\n<span class=\"token keyword\">import<\/span> matplotlib <span class=\"token keyword\">as<\/span> mpl<\/p>\n<p>mpl<span class=\"token punctuation\">.<\/span>rcParams<span class=\"token punctuation\">[<\/span><span class=\"token string\">&#039;font.family&#039;<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token string\">&#039;AR PL UMing CN&#039;<\/span><br \/>\nmpl<span class=\"token punctuation\">.<\/span>rcParams<span class=\"token punctuation\">[<\/span><span class=\"token string\">&#039;axes.unicode_minus&#039;<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token boolean\">False<\/span><\/p>\n<p>l <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>linspace<span class=\"token punctuation\">(<\/span><span class=\"token operator\">&#8211;<\/span><span class=\"token number\">5<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">5<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">500<\/span><span class=\"token punctuation\">)<\/span><br \/>\np <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">1.0<\/span> <span class=\"token operator\">\/<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#043;<\/span> np<span class=\"token punctuation\">.<\/span>exp<span class=\"token punctuation\">(<\/span><span class=\"token operator\">&#8211;<\/span>l<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>p_range <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>linspace<span class=\"token punctuation\">(<\/span><span class=\"token number\">0.01<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.99<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">500<\/span><span class=\"token punctuation\">)<\/span><br \/>\nl_range <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>log<span class=\"token punctuation\">(<\/span>p_range <span class=\"token operator\">\/<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span> <span class=\"token operator\">&#8211;<\/span> p_range<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>ann_points <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">[<\/span><br \/>\n    <span class=\"token punctuation\">(<\/span><span class=\"token number\">0.01<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">4.60<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;P&#061;0  \u5df2\u79ef\u7d2f\u5927\u91cf l_free&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token punctuation\">(<\/span><span class=\"token number\">0.25<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1.10<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;P&#061;0.25  \u8f83\u786e\u5b9a\u7a7a\u95f2&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token punctuation\">(<\/span><span class=\"token number\">0.50<\/span><span class=\"token punctuation\">,<\/span>  <span class=\"token number\">0.00<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;P&#061;0.5  \u5b8c\u5168\u672a\u77e5&#xff08;\u521d\u59cb\u72b6\u6001&#xff09;&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token punctuation\">(<\/span><span class=\"token number\">0.75<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#043;<\/span><span class=\"token number\">1.10<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;P&#061;0.75  \u8f83\u786e\u5b9a\u5360\u636e&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token punctuation\">(<\/span><span class=\"token number\">0.99<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#043;<\/span><span class=\"token number\">4.60<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;P&#061;1  \u5df2\u79ef\u7d2f\u5927\u91cf l_occ&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n<span class=\"token punctuation\">]<\/span><\/p>\n<p>fig<span class=\"token punctuation\">,<\/span> <span class=\"token punctuation\">(<\/span>ax1<span class=\"token punctuation\">,<\/span> ax2<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#061;<\/span> plt<span class=\"token punctuation\">.<\/span>subplots<span class=\"token punctuation\">(<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">,<\/span> figsize<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">14<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">5.2<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p><span class=\"token comment\"># \u5de6&#xff1a;l -&gt; p (sigmoid)<\/span><br \/>\nax1<span class=\"token punctuation\">.<\/span>plot<span class=\"token punctuation\">(<\/span>l<span class=\"token punctuation\">,<\/span> p<span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;b-&#039;<\/span><span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">2.2<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax1<span class=\"token punctuation\">.<\/span>axhline<span class=\"token punctuation\">(<\/span><span class=\"token number\">0.5<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;gray&#039;<\/span><span class=\"token punctuation\">,<\/span> linestyle<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;&#8211;&#039;<\/span><span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.8<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax1<span class=\"token punctuation\">.<\/span>axvline<span class=\"token punctuation\">(<\/span><span class=\"token number\">0.0<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;gray&#039;<\/span><span class=\"token punctuation\">,<\/span> linestyle<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;&#8211;&#039;<\/span><span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.8<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax1<span class=\"token punctuation\">.<\/span>annotate<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;l&#061;0 -&gt; p&#061;0.5&#039;<\/span><span class=\"token punctuation\">,<\/span> xy<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.5<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> xytext<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">1.2<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.58<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n             arrowprops<span class=\"token operator\">&#061;<\/span><span class=\"token builtin\">dict<\/span><span class=\"token punctuation\">(<\/span>arrowstyle<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;-&gt;&#039;<\/span><span class=\"token punctuation\">,<\/span> lw<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1.2<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">10<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax1<span class=\"token punctuation\">.<\/span>axhline<span class=\"token punctuation\">(<\/span><span class=\"token number\">0.018<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;red&#039;<\/span><span class=\"token punctuation\">,<\/span> linestyle<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;:&#039;<\/span><span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax1<span class=\"token punctuation\">.<\/span>axhline<span class=\"token punctuation\">(<\/span><span class=\"token number\">0.982<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;red&#039;<\/span><span class=\"token punctuation\">,<\/span> linestyle<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;:&#039;<\/span><span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax1<span class=\"token punctuation\">.<\/span>annotate<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;clamp floor  P&#061;0.018  (l&#061;-4)&#039;<\/span><span class=\"token punctuation\">,<\/span> xy<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">&#8211;<\/span><span class=\"token number\">4<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.018<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n             xytext<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">&#8211;<\/span><span class=\"token number\">4.5<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.15<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">9<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;red&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax1<span class=\"token punctuation\">.<\/span>annotate<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;clamp ceil   P&#061;0.982  (l&#061;&#043;4)&#039;<\/span><span class=\"token punctuation\">,<\/span> xy<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">4<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.982<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n             xytext<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">1.0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.88<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">9<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;red&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax1<span class=\"token punctuation\">.<\/span>set_xlabel<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;log-odds  l&#039;<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">12<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax1<span class=\"token punctuation\">.<\/span>set_ylabel<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;probability  P(occ)&#039;<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">12<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax1<span class=\"token punctuation\">.<\/span>set_title<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;l -&gt; p  (sigmoid \/ logistic)&#039;<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">13<\/span><span class=\"token punctuation\">,<\/span> fontweight<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;bold&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax1<span class=\"token punctuation\">.<\/span>set_xlim<span class=\"token punctuation\">(<\/span><span class=\"token operator\">&#8211;<\/span><span class=\"token number\">5<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">5<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> ax1<span class=\"token punctuation\">.<\/span>set_ylim<span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1.05<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax1<span class=\"token punctuation\">.<\/span>grid<span class=\"token punctuation\">(<\/span><span class=\"token boolean\">True<\/span><span class=\"token punctuation\">,<\/span> alpha<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.3<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p><span class=\"token comment\"># \u53f3&#xff1a;p -&gt; l (logit)<\/span><br \/>\nax2<span class=\"token punctuation\">.<\/span>plot<span class=\"token punctuation\">(<\/span>p_range<span class=\"token punctuation\">,<\/span> l_range<span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;darkorange&#039;<\/span><span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">2.2<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax2<span class=\"token punctuation\">.<\/span>axhline<span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;gray&#039;<\/span><span class=\"token punctuation\">,<\/span> linestyle<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;&#8211;&#039;<\/span><span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.8<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax2<span class=\"token punctuation\">.<\/span>axvline<span class=\"token punctuation\">(<\/span><span class=\"token number\">0.5<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;gray&#039;<\/span><span class=\"token punctuation\">,<\/span> linestyle<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;&#8211;&#039;<\/span><span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.8<\/span><span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token keyword\">for<\/span> px<span class=\"token punctuation\">,<\/span> lx<span class=\"token punctuation\">,<\/span> label <span class=\"token keyword\">in<\/span> ann_points<span class=\"token punctuation\">:<\/span><br \/>\n    ax2<span class=\"token punctuation\">.<\/span>plot<span class=\"token punctuation\">(<\/span>px<span class=\"token punctuation\">,<\/span> lx<span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;o&#039;<\/span><span class=\"token punctuation\">,<\/span> markersize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">6<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;#c44&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax2<span class=\"token punctuation\">.<\/span>annotate<span class=\"token punctuation\">(<\/span>label<span class=\"token punctuation\">,<\/span> xy<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span>px<span class=\"token punctuation\">,<\/span> lx<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                 xytext<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.08<\/span><span class=\"token punctuation\">,<\/span> lx <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">0.6<\/span> <span class=\"token keyword\">if<\/span> lx <span class=\"token operator\">&gt;&#061;<\/span> <span class=\"token number\">0<\/span> <span class=\"token keyword\">else<\/span> lx <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1.0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                 fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">9<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;#c44&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                 arrowprops<span class=\"token operator\">&#061;<\/span><span class=\"token builtin\">dict<\/span><span class=\"token punctuation\">(<\/span>arrowstyle<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;-&gt;&#039;<\/span><span class=\"token punctuation\">,<\/span> lw<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1.0<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;#c44&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax2<span class=\"token punctuation\">.<\/span>axhline<span class=\"token punctuation\">(<\/span><span class=\"token operator\">&#8211;<\/span><span class=\"token number\">4<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;red&#039;<\/span><span class=\"token punctuation\">,<\/span> linestyle<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;:&#039;<\/span><span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax2<span class=\"token punctuation\">.<\/span>axhline<span class=\"token punctuation\">(<\/span><span class=\"token operator\">&#043;<\/span><span class=\"token number\">4<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;red&#039;<\/span><span class=\"token punctuation\">,<\/span> linestyle<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;:&#039;<\/span><span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax2<span class=\"token punctuation\">.<\/span>annotate<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;clamp min  -4&#039;<\/span><span class=\"token punctuation\">,<\/span> xy<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.5<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">4<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> xytext<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.55<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">3.2<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n             fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">9<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;red&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax2<span class=\"token punctuation\">.<\/span>annotate<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;clamp max  &#043;4&#039;<\/span><span class=\"token punctuation\">,<\/span> xy<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.5<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">4<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> xytext<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.55<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">3.2<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n             fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">9<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;red&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax2<span class=\"token punctuation\">.<\/span>set_xlabel<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;probability  P(occ)&#039;<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">12<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax2<span class=\"token punctuation\">.<\/span>set_ylabel<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;log-odds  l&#039;<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">12<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax2<span class=\"token punctuation\">.<\/span>set_title<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;p -&gt; l  (logit \/ log-odds)&#039;<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">13<\/span><span class=\"token punctuation\">,<\/span> fontweight<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;bold&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax2<span class=\"token punctuation\">.<\/span>set_xlim<span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> ax2<span class=\"token punctuation\">.<\/span>set_ylim<span class=\"token punctuation\">(<\/span><span class=\"token operator\">&#8211;<\/span><span class=\"token number\">5<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">5<\/span><span class=\"token punctuation\">)<\/span><br \/>\nax2<span class=\"token punctuation\">.<\/span>grid<span class=\"token punctuation\">(<\/span><span class=\"token boolean\">True<\/span><span class=\"token punctuation\">,<\/span> alpha<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.3<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>fig<span class=\"token punctuation\">.<\/span>suptitle<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;log-odds  &lt;&#8211;&gt;  probability  \u4e92\u8f6c\u6362&#039;<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">15<\/span><span class=\"token punctuation\">,<\/span> fontweight<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;bold&#039;<\/span><span class=\"token punctuation\">,<\/span> y<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1.01<\/span><span class=\"token punctuation\">)<\/span><br \/>\nplt<span class=\"token punctuation\">.<\/span>tight_layout<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><br \/>\nplt<span class=\"token punctuation\">.<\/span>savefig<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;log_odds_curve.png&#034;<\/span><span class=\"token punctuation\">,<\/span> dpi<span class=\"token operator\">&#061;<\/span><span class=\"token number\">200<\/span><span class=\"token punctuation\">,<\/span> bbox_inches<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;tight&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token keyword\">print<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;Saved -&gt; log_odds_curve.png&#034;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<h5>6-3 MI \u865a\u62df\u5149\u675f\u6a21\u62df&#xff08;3-2 \/ 3-3 \u8282&#xff09;<\/h5>\n<ul>\n<li>\u751f\u6210\u865a\u62df\u5149\u675f\u4ece\u5019\u9009\u524d\u6cbf\u8d28\u5fc3\u53d1\u5c04\u7684\u6a21\u62df\u56fe\u2014\u2014\u6982\u7387\u7f51\u683c &#043; \u71b5\u7f51\u683c &#043; \u71b5\u51fd\u6570\u66f2\u7ebf\u4e09\u680f<\/li>\n<\/ul>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160900-6a6a259c4d197.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<p><span class=\"token triple-quoted-string string\">&#034;&#034;&#034;MI \u865a\u62df\u5149\u675f\u793a\u610f\u56fe<\/p>\n<p>\u7528\u6cd5: python3 mi_beam_demo.py<br \/>\n\u8f93\u51fa: mi_beam_simulation.png<br \/>\n&#034;&#034;&#034;<\/span><\/p>\n<p><span class=\"token keyword\">import<\/span> numpy <span class=\"token keyword\">as<\/span> np<br \/>\n<span class=\"token keyword\">import<\/span> matplotlib<span class=\"token punctuation\">.<\/span>pyplot <span class=\"token keyword\">as<\/span> plt<br \/>\n<span class=\"token keyword\">from<\/span> matplotlib<span class=\"token punctuation\">.<\/span>lines <span class=\"token keyword\">import<\/span> Line2D<br \/>\n<span class=\"token keyword\">import<\/span> matplotlib <span class=\"token keyword\">as<\/span> mpl<\/p>\n<p>mpl<span class=\"token punctuation\">.<\/span>rcParams<span class=\"token punctuation\">[<\/span><span class=\"token string\">&#039;font.family&#039;<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token string\">&#039;AR PL UMing CN&#039;<\/span><br \/>\nmpl<span class=\"token punctuation\">.<\/span>rcParams<span class=\"token punctuation\">[<\/span><span class=\"token string\">&#039;axes.unicode_minus&#039;<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token boolean\">False<\/span><\/p>\n<p><span class=\"token keyword\">def<\/span> <span class=\"token function\">build_frontier_scenario<\/span><span class=\"token punctuation\">(<\/span>size<span class=\"token operator\">&#061;<\/span><span class=\"token number\">150<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    <span class=\"token triple-quoted-string string\">&#034;&#034;&#034;\u6784\u5efa\u5c40\u90e8\u524d\u6cbf\u573a\u666f\u2014\u2014\u5bf9\u89d2\u5206\u754c &#043; \u5899\u58c1\u969c\u788d\u7269&#034;&#034;&#034;<\/span><br \/>\n    h<span class=\"token punctuation\">,<\/span> w <span class=\"token operator\">&#061;<\/span> size<span class=\"token punctuation\">,<\/span> size<br \/>\n    prob <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>full<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>h<span class=\"token punctuation\">,<\/span> w<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.5<\/span><span class=\"token punctuation\">,<\/span> dtype<span class=\"token operator\">&#061;<\/span><span class=\"token builtin\">float<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    np<span class=\"token punctuation\">.<\/span>random<span class=\"token punctuation\">.<\/span>seed<span class=\"token punctuation\">(<\/span><span class=\"token number\">42<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    <span class=\"token keyword\">for<\/span> y <span class=\"token keyword\">in<\/span> <span class=\"token builtin\">range<\/span><span class=\"token punctuation\">(<\/span>h<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n        <span class=\"token keyword\">for<\/span> x <span class=\"token keyword\">in<\/span> <span class=\"token builtin\">range<\/span><span class=\"token punctuation\">(<\/span>w<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n            boundary_y <span class=\"token operator\">&#061;<\/span> <span class=\"token builtin\">int<\/span><span class=\"token punctuation\">(<\/span>x <span class=\"token operator\">*<\/span> <span class=\"token number\">1.1<\/span> <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">20<\/span> <span class=\"token operator\">&#043;<\/span> np<span class=\"token punctuation\">.<\/span>random<span class=\"token punctuation\">.<\/span>randn<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">)<\/span><br \/>\n            <span class=\"token keyword\">if<\/span> y <span class=\"token operator\">&lt;<\/span> boundary_y<span class=\"token punctuation\">:<\/span><br \/>\n                dist <span class=\"token operator\">&#061;<\/span> boundary_y <span class=\"token operator\">&#8211;<\/span> y<br \/>\n                <span class=\"token keyword\">if<\/span> dist <span class=\"token operator\">&gt;<\/span> <span class=\"token number\">15<\/span><span class=\"token punctuation\">:<\/span><br \/>\n                    prob<span class=\"token punctuation\">[<\/span>y<span class=\"token punctuation\">,<\/span> x<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.05<\/span><br \/>\n                <span class=\"token keyword\">elif<\/span> dist <span class=\"token operator\">&gt;<\/span> <span class=\"token number\">5<\/span><span class=\"token punctuation\">:<\/span><br \/>\n                    t <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">(<\/span>dist <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">5<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">\/<\/span> <span class=\"token number\">10.0<\/span><br \/>\n                    prob<span class=\"token punctuation\">[<\/span>y<span class=\"token punctuation\">,<\/span> x<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.05<\/span> <span class=\"token operator\">&#043;<\/span> t <span class=\"token operator\">*<\/span> <span class=\"token number\">0.35<\/span><br \/>\n                <span class=\"token keyword\">else<\/span><span class=\"token punctuation\">:<\/span><br \/>\n                    t <span class=\"token operator\">&#061;<\/span> dist <span class=\"token operator\">\/<\/span> <span class=\"token number\">5.0<\/span><br \/>\n                    prob<span class=\"token punctuation\">[<\/span>y<span class=\"token punctuation\">,<\/span> x<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.40<\/span> <span class=\"token operator\">&#043;<\/span> t <span class=\"token operator\">*<\/span> <span class=\"token number\">0.10<\/span><\/p>\n<p>    <span class=\"token comment\"># \u5899\u58c1<\/span><br \/>\n    prob<span class=\"token punctuation\">[<\/span><span class=\"token number\">87<\/span><span class=\"token punctuation\">:<\/span><span class=\"token number\">93<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">87<\/span><span class=\"token punctuation\">:<\/span><span class=\"token number\">112<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.92<\/span><br \/>\n    prob<span class=\"token punctuation\">[<\/span><span class=\"token number\">20<\/span><span class=\"token punctuation\">:<\/span><span class=\"token number\">26<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">50<\/span><span class=\"token punctuation\">:<\/span><span class=\"token number\">60<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.90<\/span><br \/>\n    np<span class=\"token punctuation\">.<\/span>clip<span class=\"token punctuation\">(<\/span>prob<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.01<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.99<\/span><span class=\"token punctuation\">,<\/span> out<span class=\"token operator\">&#061;<\/span>prob<span class=\"token punctuation\">)<\/span><br \/>\n    <span class=\"token keyword\">return<\/span> prob<\/p>\n<p><span class=\"token keyword\">def<\/span> <span class=\"token function\">entropy<\/span><span class=\"token punctuation\">(<\/span>p<span class=\"token punctuation\">,<\/span> eps<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1e-9<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    p <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>clip<span class=\"token punctuation\">(<\/span>p<span class=\"token punctuation\">,<\/span> eps<span class=\"token punctuation\">,<\/span> <span class=\"token number\">1<\/span> <span class=\"token operator\">&#8211;<\/span> eps<span class=\"token punctuation\">)<\/span><br \/>\n    <span class=\"token keyword\">return<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token punctuation\">(<\/span>p <span class=\"token operator\">*<\/span> np<span class=\"token punctuation\">.<\/span>log2<span class=\"token punctuation\">(<\/span>p<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#043;<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token number\">1<\/span> <span class=\"token operator\">&#8211;<\/span> p<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">*<\/span> np<span class=\"token punctuation\">.<\/span>log2<span class=\"token punctuation\">(<\/span><span class=\"token number\">1<\/span> <span class=\"token operator\">&#8211;<\/span> p<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p><span class=\"token keyword\">def<\/span> <span class=\"token function\">bresenham_line<\/span><span class=\"token punctuation\">(<\/span>x0<span class=\"token punctuation\">,<\/span> y0<span class=\"token punctuation\">,<\/span> x1<span class=\"token punctuation\">,<\/span> y1<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    cells <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">[<\/span><span class=\"token punctuation\">]<\/span><br \/>\n    dx<span class=\"token punctuation\">,<\/span> dy <span class=\"token operator\">&#061;<\/span> <span class=\"token builtin\">abs<\/span><span class=\"token punctuation\">(<\/span>x1 <span class=\"token operator\">&#8211;<\/span> x0<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token builtin\">abs<\/span><span class=\"token punctuation\">(<\/span>y1 <span class=\"token operator\">&#8211;<\/span> y0<span class=\"token punctuation\">)<\/span><br \/>\n    sx <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">1<\/span> <span class=\"token keyword\">if<\/span> x0 <span class=\"token operator\">&lt;<\/span> x1 <span class=\"token keyword\">else<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><br \/>\n    sy <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">1<\/span> <span class=\"token keyword\">if<\/span> y0 <span class=\"token operator\">&lt;<\/span> y1 <span class=\"token keyword\">else<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">1<\/span><br \/>\n    err <span class=\"token operator\">&#061;<\/span> dx <span class=\"token operator\">&#043;<\/span> dy<br \/>\n    <span class=\"token keyword\">while<\/span> <span class=\"token boolean\">True<\/span><span class=\"token punctuation\">:<\/span><br \/>\n        cells<span class=\"token punctuation\">.<\/span>append<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>x0<span class=\"token punctuation\">,<\/span> y0<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><br \/>\n        <span class=\"token keyword\">if<\/span> x0 <span class=\"token operator\">&#061;&#061;<\/span> x1 <span class=\"token keyword\">and<\/span> y0 <span class=\"token operator\">&#061;&#061;<\/span> y1<span class=\"token punctuation\">:<\/span><br \/>\n            <span class=\"token keyword\">break<\/span><br \/>\n        e2 <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">2<\/span> <span class=\"token operator\">*<\/span> err<br \/>\n        <span class=\"token keyword\">if<\/span> e2 <span class=\"token operator\">&gt;&#061;<\/span> dy<span class=\"token punctuation\">:<\/span><br \/>\n            err <span class=\"token operator\">&#043;&#061;<\/span> dy<span class=\"token punctuation\">;<\/span> x0 <span class=\"token operator\">&#043;&#061;<\/span> sx<br \/>\n        <span class=\"token keyword\">if<\/span> e2 <span class=\"token operator\">&lt;&#061;<\/span> dx<span class=\"token punctuation\">:<\/span><br \/>\n            err <span class=\"token operator\">&#043;&#061;<\/span> dx<span class=\"token punctuation\">;<\/span> y0 <span class=\"token operator\">&#043;&#061;<\/span> sy<br \/>\n    <span class=\"token keyword\">return<\/span> cells<\/p>\n<p><span class=\"token keyword\">def<\/span> <span class=\"token function\">main<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    prob <span class=\"token operator\">&#061;<\/span> build_frontier_scenario<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    h<span class=\"token punctuation\">,<\/span> w <span class=\"token operator\">&#061;<\/span> prob<span class=\"token punctuation\">.<\/span>shape<br \/>\n    ent_map <span class=\"token operator\">&#061;<\/span> entropy<span class=\"token punctuation\">(<\/span>prob<span class=\"token punctuation\">)<\/span><\/p>\n<p>    cand_x<span class=\"token punctuation\">,<\/span> cand_y <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">55<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">45<\/span>  <span class=\"token comment\"># \u5019\u9009\u4f4d\u59ff<\/span><br \/>\n    N <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">60<\/span><br \/>\n    max_range <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">50<\/span><br \/>\n    occ_threshold <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.7<\/span><\/p>\n<p>    fig<span class=\"token punctuation\">,<\/span> axes <span class=\"token operator\">&#061;<\/span> plt<span class=\"token punctuation\">.<\/span>subplots<span class=\"token punctuation\">(<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">3<\/span><span class=\"token punctuation\">,<\/span> figsize<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">20<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">6.2<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    <span class=\"token comment\"># &#8212;- \u5de6&#xff1a;\u6982\u7387\u5730\u56fe &#043; \u5c04\u7ebf &#8212;-<\/span><br \/>\n    ax0 <span class=\"token operator\">&#061;<\/span> axes<span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span><br \/>\n    ax0<span class=\"token punctuation\">.<\/span>imshow<span class=\"token punctuation\">(<\/span>prob<span class=\"token punctuation\">,<\/span> origin<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;lower&#039;<\/span><span class=\"token punctuation\">,<\/span> cmap<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;Greys&#039;<\/span><span class=\"token punctuation\">,<\/span> vmin<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> vmax<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> interpolation<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;bilinear&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax0<span class=\"token punctuation\">.<\/span>plot<span class=\"token punctuation\">(<\/span>cand_x<span class=\"token punctuation\">,<\/span> cand_y<span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;o&#039;<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;cyan&#039;<\/span><span class=\"token punctuation\">,<\/span> markersize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">10<\/span><span class=\"token punctuation\">,<\/span><br \/>\n             markeredgecolor<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;black&#039;<\/span><span class=\"token punctuation\">,<\/span> markeredgewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1.5<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    <span class=\"token keyword\">for<\/span> i <span class=\"token keyword\">in<\/span> <span class=\"token builtin\">range<\/span><span class=\"token punctuation\">(<\/span>N<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n        angle <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">2.0<\/span> <span class=\"token operator\">*<\/span> np<span class=\"token punctuation\">.<\/span>pi <span class=\"token operator\">*<\/span> i <span class=\"token operator\">\/<\/span> N<br \/>\n        dx<span class=\"token punctuation\">,<\/span> dy <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>cos<span class=\"token punctuation\">(<\/span>angle<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> np<span class=\"token punctuation\">.<\/span>sin<span class=\"token punctuation\">(<\/span>angle<span class=\"token punctuation\">)<\/span><br \/>\n        end_x <span class=\"token operator\">&#061;<\/span> <span class=\"token builtin\">int<\/span><span class=\"token punctuation\">(<\/span>cand_x <span class=\"token operator\">&#043;<\/span> dx <span class=\"token operator\">*<\/span> max_range<span class=\"token punctuation\">)<\/span><br \/>\n        end_y <span class=\"token operator\">&#061;<\/span> <span class=\"token builtin\">int<\/span><span class=\"token punctuation\">(<\/span>cand_y <span class=\"token operator\">&#043;<\/span> dy <span class=\"token operator\">*<\/span> max_range<span class=\"token punctuation\">)<\/span><br \/>\n        end_x <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>clip<span class=\"token punctuation\">(<\/span>end_x<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> w <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><br \/>\n        end_y <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>clip<span class=\"token punctuation\">(<\/span>end_y<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> h <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>        cells <span class=\"token operator\">&#061;<\/span> bresenham_line<span class=\"token punctuation\">(<\/span>cand_x<span class=\"token punctuation\">,<\/span> cand_y<span class=\"token punctuation\">,<\/span> end_x<span class=\"token punctuation\">,<\/span> end_y<span class=\"token punctuation\">)<\/span><br \/>\n        total_ent <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.0<\/span><br \/>\n        beam_cells <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">[<\/span><span class=\"token punctuation\">]<\/span><br \/>\n        <span class=\"token keyword\">for<\/span> cx<span class=\"token punctuation\">,<\/span> cy <span class=\"token keyword\">in<\/span> cells<span class=\"token punctuation\">:<\/span><br \/>\n            <span class=\"token keyword\">if<\/span> <span class=\"token number\">0<\/span> <span class=\"token operator\">&lt;&#061;<\/span> cx <span class=\"token operator\">&lt;<\/span> w <span class=\"token keyword\">and<\/span> <span class=\"token number\">0<\/span> <span class=\"token operator\">&lt;&#061;<\/span> cy <span class=\"token operator\">&lt;<\/span> h<span class=\"token punctuation\">:<\/span><br \/>\n                p_val <span class=\"token operator\">&#061;<\/span> prob<span class=\"token punctuation\">[<\/span>cy<span class=\"token punctuation\">,<\/span> cx<span class=\"token punctuation\">]<\/span><br \/>\n                <span class=\"token keyword\">if<\/span> p_val <span class=\"token operator\">&gt;<\/span> occ_threshold<span class=\"token punctuation\">:<\/span><br \/>\n                    beam_cells<span class=\"token punctuation\">.<\/span>append<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>cx<span class=\"token punctuation\">,<\/span> cy<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token keyword\">break<\/span><br \/>\n                total_ent <span class=\"token operator\">&#043;&#061;<\/span> entropy<span class=\"token punctuation\">(<\/span>p_val<span class=\"token punctuation\">)<\/span><br \/>\n                beam_cells<span class=\"token punctuation\">.<\/span>append<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>cx<span class=\"token punctuation\">,<\/span> cy<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><br \/>\n        avg_ent <span class=\"token operator\">&#061;<\/span> total_ent <span class=\"token operator\">\/<\/span> <span class=\"token builtin\">max<\/span><span class=\"token punctuation\">(<\/span><span class=\"token builtin\">len<\/span><span class=\"token punctuation\">(<\/span>beam_cells<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><br \/>\n        color <span class=\"token operator\">&#061;<\/span> plt<span class=\"token punctuation\">.<\/span>cm<span class=\"token punctuation\">.<\/span>RdYlBu_r<span class=\"token punctuation\">(<\/span><span class=\"token number\">0.2<\/span> <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">0.6<\/span> <span class=\"token operator\">*<\/span> avg_ent<span class=\"token punctuation\">)<\/span><\/p>\n<p>        <span class=\"token keyword\">if<\/span> <span class=\"token builtin\">len<\/span><span class=\"token punctuation\">(<\/span>beam_cells<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&gt;&#061;<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">:<\/span><br \/>\n            bx <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">[<\/span>c<span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token keyword\">for<\/span> c <span class=\"token keyword\">in<\/span> beam_cells<span class=\"token punctuation\">]<\/span><br \/>\n            by <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">[<\/span>c<span class=\"token punctuation\">[<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token keyword\">for<\/span> c <span class=\"token keyword\">in<\/span> beam_cells<span class=\"token punctuation\">]<\/span><br \/>\n            ax0<span class=\"token punctuation\">.<\/span>plot<span class=\"token punctuation\">(<\/span>bx<span class=\"token punctuation\">,<\/span> by<span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span>color<span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1.2<\/span><span class=\"token punctuation\">,<\/span> alpha<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.85<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    ax0<span class=\"token punctuation\">.<\/span>set_xlim<span class=\"token punctuation\">(<\/span><span class=\"token number\">10<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">140<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> ax0<span class=\"token punctuation\">.<\/span>set_ylim<span class=\"token punctuation\">(<\/span><span class=\"token number\">10<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">120<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax0<span class=\"token punctuation\">.<\/span>set_title<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;probability grid &#043; virtual beams\\\\n(red&#061;high MI  blue&#061;low MI)&#034;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                  fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">11<\/span><span class=\"token punctuation\">,<\/span> fontweight<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;bold&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax0<span class=\"token punctuation\">.<\/span>set_xlabel<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;grid X&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> ax0<span class=\"token punctuation\">.<\/span>set_ylabel<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;grid Y&#034;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    legend_elements <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">[<\/span><br \/>\n        Line2D<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span>plt<span class=\"token punctuation\">.<\/span>cm<span class=\"token punctuation\">.<\/span>RdYlBu_r<span class=\"token punctuation\">(<\/span><span class=\"token number\">0.8<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> lw<span class=\"token operator\">&#061;<\/span><span class=\"token number\">2<\/span><span class=\"token punctuation\">,<\/span> label<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;high MI (into unknown)&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        Line2D<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span>plt<span class=\"token punctuation\">.<\/span>cm<span class=\"token punctuation\">.<\/span>RdYlBu_r<span class=\"token punctuation\">(<\/span><span class=\"token number\">0.35<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> lw<span class=\"token operator\">&#061;<\/span><span class=\"token number\">2<\/span><span class=\"token punctuation\">,<\/span> label<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;low MI (into explored)&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        Line2D<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span>plt<span class=\"token punctuation\">.<\/span>cm<span class=\"token punctuation\">.<\/span>RdYlBu_r<span class=\"token punctuation\">(<\/span><span class=\"token number\">0.2<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> lw<span class=\"token operator\">&#061;<\/span><span class=\"token number\">2<\/span><span class=\"token punctuation\">,<\/span> label<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;blocked by obstacle&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        Line2D<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span> marker<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;o&#039;<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;w&#039;<\/span><span class=\"token punctuation\">,<\/span> markerfacecolor<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;cyan&#039;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n               markeredgecolor<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;black&#039;<\/span><span class=\"token punctuation\">,<\/span> markersize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">8<\/span><span class=\"token punctuation\">,<\/span> label<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;candidate pose&#039;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n    <span class=\"token punctuation\">]<\/span><br \/>\n    ax0<span class=\"token punctuation\">.<\/span>legend<span class=\"token punctuation\">(<\/span>handles<span class=\"token operator\">&#061;<\/span>legend_elements<span class=\"token punctuation\">,<\/span> loc<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;lower right&#039;<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">8<\/span><span class=\"token punctuation\">,<\/span> framealpha<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.85<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    <span class=\"token comment\"># &#8212;- \u4e2d&#xff1a;\u71b5\u5730\u56fe &#043; \u5c04\u7ebf &#8212;-<\/span><br \/>\n    ax1 <span class=\"token operator\">&#061;<\/span> axes<span class=\"token punctuation\">[<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">]<\/span><br \/>\n    im1 <span class=\"token operator\">&#061;<\/span> ax1<span class=\"token punctuation\">.<\/span>imshow<span class=\"token punctuation\">(<\/span>ent_map<span class=\"token punctuation\">,<\/span> origin<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;lower&#039;<\/span><span class=\"token punctuation\">,<\/span> cmap<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;hot&#039;<\/span><span class=\"token punctuation\">,<\/span> vmin<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> vmax<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">,<\/span> interpolation<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;bilinear&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax1<span class=\"token punctuation\">.<\/span>plot<span class=\"token punctuation\">(<\/span>cand_x<span class=\"token punctuation\">,<\/span> cand_y<span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;o&#039;<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;cyan&#039;<\/span><span class=\"token punctuation\">,<\/span> markersize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">10<\/span><span class=\"token punctuation\">,<\/span><br \/>\n             markeredgecolor<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;black&#039;<\/span><span class=\"token punctuation\">,<\/span> markeredgewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1.5<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    <span class=\"token keyword\">for<\/span> i <span class=\"token keyword\">in<\/span> <span class=\"token builtin\">range<\/span><span class=\"token punctuation\">(<\/span>N<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">:<\/span><br \/>\n        angle <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">2.0<\/span> <span class=\"token operator\">*<\/span> np<span class=\"token punctuation\">.<\/span>pi <span class=\"token operator\">*<\/span> i <span class=\"token operator\">\/<\/span> N<br \/>\n        dx<span class=\"token punctuation\">,<\/span> dy <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>cos<span class=\"token punctuation\">(<\/span>angle<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> np<span class=\"token punctuation\">.<\/span>sin<span class=\"token punctuation\">(<\/span>angle<span class=\"token punctuation\">)<\/span><br \/>\n        end_x <span class=\"token operator\">&#061;<\/span> <span class=\"token builtin\">int<\/span><span class=\"token punctuation\">(<\/span>cand_x <span class=\"token operator\">&#043;<\/span> dx <span class=\"token operator\">*<\/span> max_range<span class=\"token punctuation\">)<\/span><br \/>\n        end_y <span class=\"token operator\">&#061;<\/span> <span class=\"token builtin\">int<\/span><span class=\"token punctuation\">(<\/span>cand_y <span class=\"token operator\">&#043;<\/span> dy <span class=\"token operator\">*<\/span> max_range<span class=\"token punctuation\">)<\/span><br \/>\n        end_x <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>clip<span class=\"token punctuation\">(<\/span>end_x<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> w <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><br \/>\n        end_y <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>clip<span class=\"token punctuation\">(<\/span>end_y<span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> h <span class=\"token operator\">&#8211;<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><br \/>\n        cells <span class=\"token operator\">&#061;<\/span> bresenham_line<span class=\"token punctuation\">(<\/span>cand_x<span class=\"token punctuation\">,<\/span> cand_y<span class=\"token punctuation\">,<\/span> end_x<span class=\"token punctuation\">,<\/span> end_y<span class=\"token punctuation\">)<\/span><br \/>\n        beam_cells <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">[<\/span><span class=\"token punctuation\">]<\/span><br \/>\n        <span class=\"token keyword\">for<\/span> cx<span class=\"token punctuation\">,<\/span> cy <span class=\"token keyword\">in<\/span> cells<span class=\"token punctuation\">:<\/span><br \/>\n            <span class=\"token keyword\">if<\/span> <span class=\"token number\">0<\/span> <span class=\"token operator\">&lt;&#061;<\/span> cx <span class=\"token operator\">&lt;<\/span> w <span class=\"token keyword\">and<\/span> <span class=\"token number\">0<\/span> <span class=\"token operator\">&lt;&#061;<\/span> cy <span class=\"token operator\">&lt;<\/span> h<span class=\"token punctuation\">:<\/span><br \/>\n                <span class=\"token keyword\">if<\/span> prob<span class=\"token punctuation\">[<\/span>cy<span class=\"token punctuation\">,<\/span> cx<span class=\"token punctuation\">]<\/span> <span class=\"token operator\">&gt;<\/span> occ_threshold<span class=\"token punctuation\">:<\/span><br \/>\n                    beam_cells<span class=\"token punctuation\">.<\/span>append<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>cx<span class=\"token punctuation\">,<\/span> cy<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token keyword\">break<\/span><br \/>\n                beam_cells<span class=\"token punctuation\">.<\/span>append<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">(<\/span>cx<span class=\"token punctuation\">,<\/span> cy<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><br \/>\n        <span class=\"token keyword\">if<\/span> <span class=\"token builtin\">len<\/span><span class=\"token punctuation\">(<\/span>beam_cells<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&gt;&#061;<\/span> <span class=\"token number\">2<\/span><span class=\"token punctuation\">:<\/span><br \/>\n            bx <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">[<\/span>c<span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token keyword\">for<\/span> c <span class=\"token keyword\">in<\/span> beam_cells<span class=\"token punctuation\">]<\/span><br \/>\n            by <span class=\"token operator\">&#061;<\/span> <span class=\"token punctuation\">[<\/span>c<span class=\"token punctuation\">[<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">]<\/span> <span class=\"token keyword\">for<\/span> c <span class=\"token keyword\">in<\/span> beam_cells<span class=\"token punctuation\">]<\/span><br \/>\n            ax1<span class=\"token punctuation\">.<\/span>plot<span class=\"token punctuation\">(<\/span>bx<span class=\"token punctuation\">,<\/span> by<span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;cyan&#039;<\/span><span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.8<\/span><span class=\"token punctuation\">,<\/span> alpha<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.6<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    ax1<span class=\"token punctuation\">.<\/span>set_xlim<span class=\"token punctuation\">(<\/span><span class=\"token number\">10<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">140<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> ax1<span class=\"token punctuation\">.<\/span>set_ylim<span class=\"token punctuation\">(<\/span><span class=\"token number\">10<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">120<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax1<span class=\"token punctuation\">.<\/span>set_title<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;entropy grid &#043; virtual beams\\\\n(bright &#061; high uncertainty &#061; high MI)&#034;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                  fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">11<\/span><span class=\"token punctuation\">,<\/span> fontweight<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;bold&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax1<span class=\"token punctuation\">.<\/span>set_xlabel<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;grid X&#034;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    plt<span class=\"token punctuation\">.<\/span>colorbar<span class=\"token punctuation\">(<\/span>im1<span class=\"token punctuation\">,<\/span> ax<span class=\"token operator\">&#061;<\/span>ax1<span class=\"token punctuation\">,<\/span> fraction<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.046<\/span><span class=\"token punctuation\">,<\/span> pad<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.04<\/span><span class=\"token punctuation\">,<\/span> label<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#034;H [bit]&#034;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    <span class=\"token comment\"># &#8212;- \u53f3&#xff1a;\u71b5\u51fd\u6570\u66f2\u7ebf &#8212;-<\/span><br \/>\n    ax2 <span class=\"token operator\">&#061;<\/span> axes<span class=\"token punctuation\">[<\/span><span class=\"token number\">2<\/span><span class=\"token punctuation\">]<\/span><br \/>\n    p_vals <span class=\"token operator\">&#061;<\/span> np<span class=\"token punctuation\">.<\/span>linspace<span class=\"token punctuation\">(<\/span><span class=\"token number\">0.001<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.999<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">500<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    h_vals <span class=\"token operator\">&#061;<\/span> entropy<span class=\"token punctuation\">(<\/span>p_vals<span class=\"token punctuation\">)<\/span><br \/>\n    ax2<span class=\"token punctuation\">.<\/span>plot<span class=\"token punctuation\">(<\/span>p_vals<span class=\"token punctuation\">,<\/span> h_vals<span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#039;darkgreen&#039;<\/span><span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">2.2<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax2<span class=\"token punctuation\">.<\/span>fill_between<span class=\"token punctuation\">(<\/span>p_vals<span class=\"token punctuation\">,<\/span> h_vals<span class=\"token punctuation\">,<\/span> alpha<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;green&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax2<span class=\"token punctuation\">.<\/span>axvline<span class=\"token punctuation\">(<\/span><span class=\"token number\">0.5<\/span><span class=\"token punctuation\">,<\/span> color<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;gray&#039;<\/span><span class=\"token punctuation\">,<\/span> linestyle<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;&#8211;&#039;<\/span><span class=\"token punctuation\">,<\/span> linewidth<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.8<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax2<span class=\"token punctuation\">.<\/span>annotate<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#039;max entropy\\\\nat P&#061;0.5&#039;<\/span><span class=\"token punctuation\">,<\/span> xy<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.5<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1.0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> xytext<span class=\"token operator\">&#061;<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.6<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.92<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                 fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">9<\/span><span class=\"token punctuation\">,<\/span> arrowprops<span class=\"token operator\">&#061;<\/span><span class=\"token builtin\">dict<\/span><span class=\"token punctuation\">(<\/span>arrowstyle<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;-&gt;&#039;<\/span><span class=\"token punctuation\">,<\/span> lw<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax2<span class=\"token punctuation\">.<\/span>set_xlabel<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;probability P(occ)&#034;<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">11<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax2<span class=\"token punctuation\">.<\/span>set_ylabel<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;entropy H [bit]&#034;<\/span><span class=\"token punctuation\">,<\/span> fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">11<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax2<span class=\"token punctuation\">.<\/span>set_title<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;H(p) &#061; -p log2(p) &#8211; (1-p) log2(1-p)&#034;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                  fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">11<\/span><span class=\"token punctuation\">,<\/span> fontweight<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;bold&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax2<span class=\"token punctuation\">.<\/span>set_xlim<span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> ax2<span class=\"token punctuation\">.<\/span>set_ylim<span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1.05<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    ax2<span class=\"token punctuation\">.<\/span>grid<span class=\"token punctuation\">(<\/span><span class=\"token boolean\">True<\/span><span class=\"token punctuation\">,<\/span> alpha<span class=\"token operator\">&#061;<\/span><span class=\"token number\">0.3<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p>    fig<span class=\"token punctuation\">.<\/span>suptitle<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;MI virtual beam simulation  |  N&#061;60  |  blue&#061;explored  red&#061;unknown&#034;<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                 fontsize<span class=\"token operator\">&#061;<\/span><span class=\"token number\">14<\/span><span class=\"token punctuation\">,<\/span> fontweight<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;bold&#039;<\/span><span class=\"token punctuation\">,<\/span> y<span class=\"token operator\">&#061;<\/span><span class=\"token number\">1.01<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    plt<span class=\"token punctuation\">.<\/span>tight_layout<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    plt<span class=\"token punctuation\">.<\/span>savefig<span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;mi_beam_simulation.png&#034;<\/span><span class=\"token punctuation\">,<\/span> dpi<span class=\"token operator\">&#061;<\/span><span class=\"token number\">200<\/span><span class=\"token punctuation\">,<\/span> bbox_inches<span class=\"token operator\">&#061;<\/span><span class=\"token string\">&#039;tight&#039;<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    <span class=\"token keyword\">print<\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;Saved -&gt; mi_beam_simulation.png&#034;<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<p><span class=\"token keyword\">if<\/span> __name__ <span class=\"token operator\">&#061;&#061;<\/span> <span class=\"token string\">&#039;__main__&#039;<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    main<span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><\/p>\n<hr \/>\n<h3>\u603b\u7ed3<\/h3>\n<ul>\n<li>\u672c\u671f\u4ece&#034;\u4e09\u503c\u5730\u56fe\u7684\u5c40\u9650\u6027&#034;\u51fa\u53d1&#xff0c;\u5b8c\u6210\u4e86\u4e24\u4ef6\u4e8b&#xff1a;\n<ul>\n<li>\u624b\u5199\u6982\u7387\u5730\u56fe\u8282\u70b9 prob_map\u2014\u2014\u57fa\u4e8e log-odds \u9012\u63a8\u66f4\u65b0 &#043; \u9006\u4f20\u611f\u5668\u6a21\u578b &#043; Bresenham \u5149\u7ebf\u8ffd\u8e2a&#xff0c;\u72ec\u7acb\u4e8e slam_toolbox \u7ef4\u62a4\u4e00\u4efd\u8fde\u7eed\u6982\u7387\u7f51\u683c&#xff0c;\u5e76\u901a\u8fc7\u4e09\u4e2a Topic&#xff08;\u6982\u7387 \/ \u9608\u503c \/ \u71b5&#xff09;\u53d1\u5e03<\/li>\n<li>\u5b9e\u73b0 MI \u4e92\u4fe1\u606f\u76ee\u6807\u9009\u62e9\u5668 MIGoalSelector\u2014\u2014\u4ece\u5019\u9009\u524d\u6cbf\u8d28\u5fc3\u53d1\u5c04 60 \u675f\u865a\u62df\u5c04\u7ebf&#xff0c;\u7d2f\u52a0\u53ef\u89c1 cell \u7684\u9999\u519c\u71b5\u4f5c\u4e3a\u4fe1\u606f\u589e\u76ca&#xff0c;\u9664\u4ee5 Dijkstra \u8def\u5f84\u4ee3\u4ef7\u5f97\u5230\u524d\u6cbf\u6548\u7528<\/li>\n<\/ul>\n<\/li>\n<li>\u6838\u5fc3\u8981\u70b9\u56de\u987e&#xff1a;\n<ul>\n<li>log-odds \u9012\u63a8&#xff1a;\u8d1d\u53f6\u65af\u66f4\u65b0\u9000\u5316\u4e3a\u52a0\u6cd5&#xff0c;l_t &#061; l_{t-1} &#043; l_occ \u6216 l_t &#061; l_{t-1} &#043; l_free&#xff0c;\u8fd9\u5957\u516c\u5f0f\u4e5f\u662f OctoMap \u4f7f\u7528\u7684<\/li>\n<li>\u4fdd\u5b88\u4f20\u611f\u5668\u6a21\u578b&#xff1a;\u9009\u62e9 P(hit)&#061;0.55 \/ P(miss)&#061;0.45&#xff0c;\u6162\u6536\u655b \u2192 \u524d\u6cbf\u9644\u8fd1 5~10 \u683c\u7070\u8272\u6e10\u53d8\u5e26 \u2192 \u71b5\u503c\u5dee\u5f02\u663e\u8457 \u2192 MI \u7b97\u6cd5\u6709\u8db3\u591f\u7684\u533a\u5206\u5ea6<\/li>\n<li>MI \u4e0d\u662f\u6570\u683c\u5b50&#xff1a;\u5f80\u671f\u7684 computeInfoGain() \u5728\u6570 UNKNOWN \u90bb\u5c45&#xff0c;\u672c\u671f\u7684 computeMI() \u6a21\u62df 360 \u4f20\u611f\u5668\u626b\u63cf&#xff0c;\u517c\u987e\u906e\u6321\u548c\u4e0d\u786e\u5b9a\u6027\u68af\u5ea6<\/li>\n<li>\u5206\u79bb\u5173\u6ce8&#xff1a;\/thresholded_map \u7ed9 Nav2 \u505a\u8def\u5f84\u89c4\u5212&#xff0c;\/prob_map \u7ed9 MI \u505a\u4fe1\u606f\u8bba\u51b3\u7b56&#xff0c;\/entropy_map \u7ed9 RViz \u505a\u53ef\u89c6\u5316\u2014\u2014\u4e00\u4e2a\u7f51\u683c\u4e09\u79cd\u89c6\u89d2&#xff0c;\u5404\u53f8\u5176\u804c<\/li>\n<\/ul>\n<\/li>\n<li>\u5b8c\u6574\u7684\u4ee3\u7801\u5728 ros2_ws\/src\/prob_map\/ \u548c ros2_ws\/src\/exploration\/ \u4e0b&#xff0c;\u53ef\u76f4\u63a5\u7f16\u8bd1\u8fd0\u884c<\/li>\n<li>\u5982\u6709\u9519\u8bef&#xff0c;\u6b22\u8fce\u6307\u51fa&#xff01;<\/li>\n<li>\u611f\u8c22\u89c2\u770b&#xff01;<\/li>\n<\/ul>\n","protected":false},"excerpt":{"rendered":"<p>\u524d\u8a00<br \/>\n\u4e0a\u4e00\u671f\u6211\u4eec\u628a RIGGoalSelector \u5f15\u5165\u4e86\u63a2\u7d22\u7cfb\u7edf\u2014\u2014\u901a\u8fc7 RRT \u504f\u5411\u9ad8\u4fe1\u606f\u524d\u6cbf\u6765\u8bc4\u4f30\u8def\u5f84\u4ee3\u4ef7&#xff0c;\u6bd4\u5355\u7eaf\u7684 Nearest \u548c Utility \u5df2\u7ecf\u6709\u4e86\u660e\u663e\u63d0\u5347\u5f80\u671f\u5185\u5bb9&#xff1a; \u7b2c\u4e00\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u4e00) \u6846\u67b6\u603b\u89c8\u548c\u6982\u5ff5\u89e3\u91ca\u7b2c\u4e8c\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u4e8c) &#xff1a;ROS2gazebo\u963f\u514b\u66fc\u5c0f\u8f66\u6a21\u578b\u642d\u5efa-gazebo_ackermann_drive\u7b49\u63d2\u4ef6\u7684\u914d\u7f6e<\/p>\n","protected":false},"author":2,"featured_media":87178,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[1],"tags":[9861,9862,9863,55,2968],"topic":[],"class_list":["post-87191","post","type-post","status-publish","format-standard","has-post-thumbnail","hentry","category-server","tag-navigation2","tag-9862","tag-9863","tag-c","tag-ros2"],"yoast_head":"<!-- This site is optimized with the Yoast SEO plugin v20.3 - https:\/\/yoast.com\/wordpress\/plugins\/seo\/ -->\n<title>\u3010Navigation2\u8fdb\u9636\u3011(\u5341\u4e09\uff09\uff1a\u624b\u5199\u6982\u7387\u5730\u56fe!\u4ecelog-odds\u63a8\u5bfc\u5230MI\u4e92\u4fe1\u606f\u63a2\u7d22\u4e0eNav2\u63d2\u4ef6\u5b9e\u73b0 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3<\/title>\n<meta name=\"robots\" content=\"index, follow, max-snippet:-1, max-image-preview:large, max-video-preview:-1\" \/>\n<link rel=\"canonical\" href=\"https:\/\/www.wsisp.com\/helps\/87191.html\" \/>\n<meta property=\"og:locale\" content=\"zh_CN\" \/>\n<meta property=\"og:type\" content=\"article\" \/>\n<meta property=\"og:title\" content=\"\u3010Navigation2\u8fdb\u9636\u3011(\u5341\u4e09\uff09\uff1a\u624b\u5199\u6982\u7387\u5730\u56fe!\u4ecelog-odds\u63a8\u5bfc\u5230MI\u4e92\u4fe1\u606f\u63a2\u7d22\u4e0eNav2\u63d2\u4ef6\u5b9e\u73b0 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\" \/>\n<meta property=\"og:description\" content=\"\u524d\u8a00 \u4e0a\u4e00\u671f\u6211\u4eec\u628a RIGGoalSelector \u5f15\u5165\u4e86\u63a2\u7d22\u7cfb\u7edf\u2014\u2014\u901a\u8fc7 RRT \u504f\u5411\u9ad8\u4fe1\u606f\u524d\u6cbf\u6765\u8bc4\u4f30\u8def\u5f84\u4ee3\u4ef7&#xff0c;\u6bd4\u5355\u7eaf\u7684 Nearest \u548c Utility \u5df2\u7ecf\u6709\u4e86\u660e\u663e\u63d0\u5347\u5f80\u671f\u5185\u5bb9&#xff1a; \u7b2c\u4e00\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u4e00) \u6846\u67b6\u603b\u89c8\u548c\u6982\u5ff5\u89e3\u91ca\u7b2c\u4e8c\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u4e8c) &#xff1a;ROS2gazebo\u963f\u514b\u66fc\u5c0f\u8f66\u6a21\u578b\u642d\u5efa-gazebo_ackermann_drive\u7b49\u63d2\u4ef6\u7684\u914d\u7f6e\" \/>\n<meta property=\"og:url\" content=\"https:\/\/www.wsisp.com\/helps\/87191.html\" \/>\n<meta property=\"og:site_name\" content=\"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\" \/>\n<meta property=\"article:published_time\" content=\"2026-07-29T16:09:03+00:00\" \/>\n<meta property=\"og:image\" content=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160844-6a6a258cef735.png\" \/>\n<meta name=\"author\" content=\"admin\" \/>\n<meta name=\"twitter:card\" content=\"summary_large_image\" \/>\n<meta name=\"twitter:label1\" content=\"\u4f5c\u8005\" \/>\n\t<meta name=\"twitter:data1\" content=\"admin\" \/>\n\t<meta name=\"twitter:label2\" content=\"\u9884\u8ba1\u9605\u8bfb\u65f6\u95f4\" \/>\n\t<meta name=\"twitter:data2\" content=\"51 \u5206\" \/>\n<script type=\"application\/ld+json\" class=\"yoast-schema-graph\">{\"@context\":\"https:\/\/schema.org\",\"@graph\":[{\"@type\":\"WebPage\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/87191.html\",\"url\":\"https:\/\/www.wsisp.com\/helps\/87191.html\",\"name\":\"\u3010Navigation2\u8fdb\u9636\u3011(\u5341\u4e09\uff09\uff1a\u624b\u5199\u6982\u7387\u5730\u56fe!\u4ecelog-odds\u63a8\u5bfc\u5230MI\u4e92\u4fe1\u606f\u63a2\u7d22\u4e0eNav2\u63d2\u4ef6\u5b9e\u73b0 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\",\"isPartOf\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/#website\"},\"datePublished\":\"2026-07-29T16:09:03+00:00\",\"dateModified\":\"2026-07-29T16:09:03+00:00\",\"author\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41\"},\"breadcrumb\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/87191.html#breadcrumb\"},\"inLanguage\":\"zh-Hans\",\"potentialAction\":[{\"@type\":\"ReadAction\",\"target\":[\"https:\/\/www.wsisp.com\/helps\/87191.html\"]}]},{\"@type\":\"BreadcrumbList\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/87191.html#breadcrumb\",\"itemListElement\":[{\"@type\":\"ListItem\",\"position\":1,\"name\":\"\u9996\u9875\",\"item\":\"https:\/\/www.wsisp.com\/helps\"},{\"@type\":\"ListItem\",\"position\":2,\"name\":\"\u3010Navigation2\u8fdb\u9636\u3011(\u5341\u4e09\uff09\uff1a\u624b\u5199\u6982\u7387\u5730\u56fe!\u4ecelog-odds\u63a8\u5bfc\u5230MI\u4e92\u4fe1\u606f\u63a2\u7d22\u4e0eNav2\u63d2\u4ef6\u5b9e\u73b0\"}]},{\"@type\":\"WebSite\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/#website\",\"url\":\"https:\/\/www.wsisp.com\/helps\/\",\"name\":\"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\",\"description\":\"\u9999\u6e2f\u670d\u52a1\u5668_\u9999\u6e2f\u4e91\u670d\u52a1\u5668\u8d44\u8baf_\u670d\u52a1\u5668\u5e2e\u52a9\u6587\u6863_\u670d\u52a1\u5668\u6559\u7a0b\",\"potentialAction\":[{\"@type\":\"SearchAction\",\"target\":{\"@type\":\"EntryPoint\",\"urlTemplate\":\"https:\/\/www.wsisp.com\/helps\/?s={search_term_string}\"},\"query-input\":\"required name=search_term_string\"}],\"inLanguage\":\"zh-Hans\"},{\"@type\":\"Person\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41\",\"name\":\"admin\",\"image\":{\"@type\":\"ImageObject\",\"inLanguage\":\"zh-Hans\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/image\/\",\"url\":\"https:\/\/gravatar.wp-china-yes.net\/avatar\/?s=96&d=mystery\",\"contentUrl\":\"https:\/\/gravatar.wp-china-yes.net\/avatar\/?s=96&d=mystery\",\"caption\":\"admin\"},\"sameAs\":[\"http:\/\/wp.wsisp.com\"],\"url\":\"https:\/\/www.wsisp.com\/helps\/author\/admin\"}]}<\/script>\n<!-- \/ Yoast SEO plugin. -->","yoast_head_json":{"title":"\u3010Navigation2\u8fdb\u9636\u3011(\u5341\u4e09\uff09\uff1a\u624b\u5199\u6982\u7387\u5730\u56fe!\u4ecelog-odds\u63a8\u5bfc\u5230MI\u4e92\u4fe1\u606f\u63a2\u7d22\u4e0eNav2\u63d2\u4ef6\u5b9e\u73b0 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","robots":{"index":"index","follow":"follow","max-snippet":"max-snippet:-1","max-image-preview":"max-image-preview:large","max-video-preview":"max-video-preview:-1"},"canonical":"https:\/\/www.wsisp.com\/helps\/87191.html","og_locale":"zh_CN","og_type":"article","og_title":"\u3010Navigation2\u8fdb\u9636\u3011(\u5341\u4e09\uff09\uff1a\u624b\u5199\u6982\u7387\u5730\u56fe!\u4ecelog-odds\u63a8\u5bfc\u5230MI\u4e92\u4fe1\u606f\u63a2\u7d22\u4e0eNav2\u63d2\u4ef6\u5b9e\u73b0 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","og_description":"\u524d\u8a00 \u4e0a\u4e00\u671f\u6211\u4eec\u628a RIGGoalSelector \u5f15\u5165\u4e86\u63a2\u7d22\u7cfb\u7edf\u2014\u2014\u901a\u8fc7 RRT \u504f\u5411\u9ad8\u4fe1\u606f\u524d\u6cbf\u6765\u8bc4\u4f30\u8def\u5f84\u4ee3\u4ef7&#xff0c;\u6bd4\u5355\u7eaf\u7684 Nearest \u548c Utility \u5df2\u7ecf\u6709\u4e86\u660e\u663e\u63d0\u5347\u5f80\u671f\u5185\u5bb9&#xff1a; \u7b2c\u4e00\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u4e00) \u6846\u67b6\u603b\u89c8\u548c\u6982\u5ff5\u89e3\u91ca\u7b2c\u4e8c\u671f&#xff1a;\u301010\u5929\u901f\u901aNavigation2\u3011(\u4e8c) &#xff1a;ROS2gazebo\u963f\u514b\u66fc\u5c0f\u8f66\u6a21\u578b\u642d\u5efa-gazebo_ackermann_drive\u7b49\u63d2\u4ef6\u7684\u914d\u7f6e","og_url":"https:\/\/www.wsisp.com\/helps\/87191.html","og_site_name":"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","article_published_time":"2026-07-29T16:09:03+00:00","og_image":[{"url":"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260729160844-6a6a258cef735.png"}],"author":"admin","twitter_card":"summary_large_image","twitter_misc":{"\u4f5c\u8005":"admin","\u9884\u8ba1\u9605\u8bfb\u65f6\u95f4":"51 \u5206"},"schema":{"@context":"https:\/\/schema.org","@graph":[{"@type":"WebPage","@id":"https:\/\/www.wsisp.com\/helps\/87191.html","url":"https:\/\/www.wsisp.com\/helps\/87191.html","name":"\u3010Navigation2\u8fdb\u9636\u3011(\u5341\u4e09\uff09\uff1a\u624b\u5199\u6982\u7387\u5730\u56fe!\u4ecelog-odds\u63a8\u5bfc\u5230MI\u4e92\u4fe1\u606f\u63a2\u7d22\u4e0eNav2\u63d2\u4ef6\u5b9e\u73b0 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","isPartOf":{"@id":"https:\/\/www.wsisp.com\/helps\/#website"},"datePublished":"2026-07-29T16:09:03+00:00","dateModified":"2026-07-29T16:09:03+00:00","author":{"@id":"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41"},"breadcrumb":{"@id":"https:\/\/www.wsisp.com\/helps\/87191.html#breadcrumb"},"inLanguage":"zh-Hans","potentialAction":[{"@type":"ReadAction","target":["https:\/\/www.wsisp.com\/helps\/87191.html"]}]},{"@type":"BreadcrumbList","@id":"https:\/\/www.wsisp.com\/helps\/87191.html#breadcrumb","itemListElement":[{"@type":"ListItem","position":1,"name":"\u9996\u9875","item":"https:\/\/www.wsisp.com\/helps"},{"@type":"ListItem","position":2,"name":"\u3010Navigation2\u8fdb\u9636\u3011(\u5341\u4e09\uff09\uff1a\u624b\u5199\u6982\u7387\u5730\u56fe!\u4ecelog-odds\u63a8\u5bfc\u5230MI\u4e92\u4fe1\u606f\u63a2\u7d22\u4e0eNav2\u63d2\u4ef6\u5b9e\u73b0"}]},{"@type":"WebSite","@id":"https:\/\/www.wsisp.com\/helps\/#website","url":"https:\/\/www.wsisp.com\/helps\/","name":"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","description":"\u9999\u6e2f\u670d\u52a1\u5668_\u9999\u6e2f\u4e91\u670d\u52a1\u5668\u8d44\u8baf_\u670d\u52a1\u5668\u5e2e\u52a9\u6587\u6863_\u670d\u52a1\u5668\u6559\u7a0b","potentialAction":[{"@type":"SearchAction","target":{"@type":"EntryPoint","urlTemplate":"https:\/\/www.wsisp.com\/helps\/?s={search_term_string}"},"query-input":"required name=search_term_string"}],"inLanguage":"zh-Hans"},{"@type":"Person","@id":"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41","name":"admin","image":{"@type":"ImageObject","inLanguage":"zh-Hans","@id":"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/image\/","url":"https:\/\/gravatar.wp-china-yes.net\/avatar\/?s=96&d=mystery","contentUrl":"https:\/\/gravatar.wp-china-yes.net\/avatar\/?s=96&d=mystery","caption":"admin"},"sameAs":["http:\/\/wp.wsisp.com"],"url":"https:\/\/www.wsisp.com\/helps\/author\/admin"}]}},"_links":{"self":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/posts\/87191","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/users\/2"}],"replies":[{"embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/comments?post=87191"}],"version-history":[{"count":0,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/posts\/87191\/revisions"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/media\/87178"}],"wp:attachment":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/media?parent=87191"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/categories?post=87191"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/tags?post=87191"},{"taxonomy":"topic","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/topic?post=87191"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}