{"id":271,"date":"2024-04-08T19:56:00","date_gmt":"2024-04-08T19:56:00","guid":{"rendered":"https:\/\/boochlin.com\/?p=271"},"modified":"2026-09-16T09:54:00","modified_gmt":"2026-09-16T09:54:00","slug":"slam-note-ndt_tku","status":"publish","type":"post","link":"https:\/\/boochlin.com\/?p=271","title":{"rendered":"SLAM Note \u2013 NDT_TKU"},"content":{"rendered":"<p>** <a href=\"\/\/www.slideshare.net\/boochlin\/ndttku\">NDT-TKU<\/a> ** from <strong><a href=\"\/\/www.slideshare.net\/boochlin\">Booch Lin<\/a><\/strong><\/p>\n<h3>NDT \u2013 TKU Introduction<\/h3>\n<p>\u777d\u9055\u8a31\u4e45\u6c92\u6709\u5beb blog \uff0c\u4e3b\u8981\u9084\u662f\u592a\u5fd9\u3002\u9019\u5468\u7d42\u65bc\u7a7a\u51fa\u6642\u9593\u53ef\u4ee5\u4f86\u88dc\u88dc\u4e4b\u524d\u7684\u7b46\u8a18\uff0c\u7531\u65bc\u6700\u8fd1\u624d\u5c0d\u5167\u90e8\u5206\u4eab\u6b64\u4e3b\u984c\u3002\u6240\u4ee5\u9019\u500b\u4e3b\u984c\u5370\u8c61\u6700\u6df1\u523b\u3002<br \/>\n\u96d6\u7136\u5f88\u60f3\u518d\u6b21\u5c31\u9032\u5165\u6b63\u984c\uff0c\u4f46\u662f\u7531\u65bc\u9019 blog \u7684\u98a8\u683c\u5c31\u662f\u5ee2\u8a71\u591a\uff0c\u6240\u4ee5\u6211\u7e7c\u7e8c\u5beb\u5ee2\u8a71\uff0c\u8ac7\u5230\u7684\u9019\u500b\u4e3b\u984c\uff0c\u7528\u5230\u5927\u91cf\u7684\u6578\u5b78\uff0c\u800c\u5c0f\u5f1f\u6211\u5728\u5927\u5b78\u57fa\u672c\u4e0a\u300e\u6a5f\u7387\u300f\u300e\u7dda\u6027\u4ee3\u6578\u300f\u300e\u5fae\u7a4d\u5206\u300f\u5927\u6982\u90fd\u6709\u91cd\u4fee\u904e\uff0c\u6240\u4ee5\u5982\u679c\u5ba2\u500c\u5c0d\u65bc\u6578\u5b78\u7684\u90e8\u4efd\u6709\u554f\u984c\uff0c\u8acb\u81ea\u884c\u4e0a\u7db2\u67e5\uff0c\u56e0\u70ba\u6211\u7b54\u7684\u4e0d\u6703\u6bd4google \u5927\u795e\u597d\u3002<\/p>\n<p><strong>\u9019\u6b21\u4e3b\u984c\u7684\u6700\u7d42\u76ee\u6a19 SLAM \uff08<\/strong>\u00a0<strong>Simultaneous localization and mapping\uff09 \u540c\u6b65\u5b9a\u4f4d\u8207\u5730\u5716\u69cb\u5efa\uff0c\u662f\u4e00\u500b\u95dc\u65bc\u6a5f\u5668\u4eba\u6216\u662f\u81ea\u52d5\u99d5\u99db\u5fc5\u5b9a\u7814\u7a76\u7684\u4e3b\u984c\uff0c\u800c\u5f04\u61c2\u524d\u4e5f\u5fc5\u9808\u5148\u5177\u6709\u5927\u91cf\u7684\u76f8\u95dc\u77e5\u8b58\uff0c\u524d\u7f6e\u90e8\u4efd\u6211\u6c92\u6709\u8fa6\u6cd5\u7d30\u7d30\u8b1b\u89e3\uff0c\u4e3b\u8981\u4e5f\u662f\u6240\u5b78\u592a\u5c11\uff0c\u56e0\u6b64\u9019\u6b21\u5c31\u53ea\u5c08\u6ce8\u65bc SLAM \u7684\u5176\u4e2d\u4e00\u500b\u65b9\u6cd5 NDT , \u800c TKU\u00a0\u5247\u662f\u518d NDT \u9032\u4e00\u6b65\u7684\u63a8\u5c55\u3002<\/strong><\/p>\n<p>\u5728\u6b64\u4e4b\u524d\uff0c\u6709\u8208\u8da3\u7684\u4eba\u53ef\u4ee5\u5148\u770b\u9019\u7bc7\u8ad6\u6587<\/p>\n<p>****<a href=\"http:\/\/aass.oru.se\/Research\/mro\/publications\/2009\/Magnusson_2009-Doctoral_Thesis-3D_NDT.pdf\">Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform \u2014 an Ef\ufb01cient Representation for Registration, Surface Analysis, and Loop Detection<\/a><\/p>\n<p>\u9019\u5927\u6982\u662f\u76ee\u524d\u5c0d\u65bc NDT \u6700\u5165\u9580\u7684\u6587\u4ef6\uff0c\u5f04\u61c2\u4ed6\u4e0d\u6703\u5403\u8667\uff0c\u751a\u81f3\u4e0d\u7528\u770b\u6211\u7684\u6587\u7ae0\u5566\u3002<\/p>\n<p>SLAM \u7684\u6240\u6709\u6700\u7d42\u76ee\u7684\u5c31\u662f\u8981\u627e\u51fa\u81ea\u8eab\u5230\u5e95\u4f4d\u65bc\u4e16\u754c\u4e2d\u7684\u4f55\u8655\uff0c\u9019\u53ef\u80fd\u5927\u5bb6\u6703\u76f4\u89ba\u8a8d\u70ba\u4ea4\u7d66 GPS \u5c31\u597d\u5566\uff0c\u4f46\u662f\u5be6\u969b\u4e0a\u4ed6\u7684\u4e0d\u78ba\u5b9a\u56e0\u7d20\u5be6\u5728\u592a\u591a\uff0c\u5404\u4f4d\u81ea\u8eab\u7528\u624b\u6a5f\u5c0e\u822a\u90fd\u6703\u5e38\u5e38\u7f75\u8aaa\u5b9a\u4e0d\u6e96\uff0c\u90a3\u9ebc\u8981\u6c42\u66f4\u9ad8\u5b89\u5168\u6027\u7684\u81ea\u52d5\u99d5\u99db\u66f4\u4e0d\u53ef\u80fd\u5b8c\u5168\u4f9d\u8a17\u5728\u9019\u4e0a\u9762\uff0c\u7576\u7136\u76ee\u524d\u5169\u6d3e\u722d\u8ad6\u4e5f\u662f\u6c92\u6709\u505c\u904e\uff0c\u4e0d\u904e\u722d\u8ad6\u7684\u9ede\u4e3b\u8981\u5c31\u662f\u5728\u65bc\u53ea\u8981\u767c\u5c55 GPS \u6280\u8853\u5230\u975e\u5e38\u9ad8\u7cbe\u5ea6\uff0c\u5c31\u53ef\u4ee5\u4e0d\u7528\u7814\u7a76\u5176\u4ed6\u76f8\u95dc\u5b9a\u4f4d\u6280\u8853\u3002\u4f46\u662f\u7528\u819d\u84cb\u60f3\u4e5f\u77e5\u9053\u5c31\u9084\u4e0d\u5920\u7cbe\u5bc6\uff0c\u6240\u4ee5\u5b9a\u4f4d\u6f14\u7b97\u6cd5\u81f3\u4eca\u80fd\u7136\u662f\u4e00\u500b\u4e3b\u8981\u7814\u7a76\u4e3b\u984c\u3002<\/p>\n<p>\u5b9a\u4f4d\u6f14\u7b97\u6cd5\u4e00\u500b\u91cd\u9ede\u5c31\u662f Sensing\uff0c\u6a5f\u5668\u4eba\u5fc5\u9700\u8981\u80fd\u611f\u77e5\u5468\u570d\u74b0\u5883\uff0c\u5f97\u5230\u76f8\u95dc\u60c5\u5831\uff0c\u6700\u5e38\u898b\u7684\u5c31\u662f\u76f8\u6a5f\uff0c\u9019\u78ba\u5be6\u662f\u500b\u4e3b\u529b\uff0c\u800c\u4e14\u76f8\u8f03\u5176\u4ed6\u50b3\u611f\u5668\u78ba\u5be6\u4fbf\u5b9c\uff0c\u4e0d\u904e\u9019\u6b21\u8a0e\u8ad6\u7684\u76ee\u6a19\u4e26\u975e\u76f8\u6a5f\uff0c\u800c\u662f LIDAR (\u5149\u9054)\uff0c\u901a\u5e38\u6383\u63cf\u5230\u7684\u756b\u9762\u9577\u9019\u6a23<\/p>\n<p>\u5f71\u7247\u662f VELODYNE LIDAR \u7684\u793a\u610f\u5716\uff0c\u9806\u4ee3\u4e00\u984c\u9019\u6771\u897f\u8d85\u8cb4\uff0c\u6578\u767e\u842c\u6709\u4e4b\u3002\u76ee\u524d\u4e0d\u5c11\u7814\u7a76\u90fd\u662f\u91dd\u5c0d\u9019\u7a2e\u9ede\u96f2\u8cc7\u6599\u4f5c SLAM \u7814\u7a76\u3002<\/p>\n<p>\u5176\u5be6\u9019\u985e\u8cc7\u6599\u7814\u7a76\u4e3b\u8981\u5c31\u570d\u7e5e\u5728\u300e\u5229\u7528\u9019\u4e00\u6b21\u6383\u7784\u5230\u7684\u9ede\u96f2\u8ddf\u4e0b\u4e00\u6b21\u6383\u7784\u5230\u9ede\u96f2\uff0c\u5224\u65b7\u51fa\u6a5f\u5668\u4eba\u7684\u4f4d\u5b50\u8207\u59ff\u614b\u300f\u3002\u8aaa\u8d77\u4f86\u7c21\u55ae\u662f\u500b\u5339\u914d\u554f\u984c\uff0c\u4f46\u662f\u505a\u8d77\u4f86\u8d85\u7d1a\u9ebb\u7169\uff0c\u5149\u662f\u5206\u985e\u5c31\u53ef\u4ee5\u641e\u6b7b\u81ea\u5df1\u4e86\u3002(\u5beb\u5230\u9019\u908a\u6709\u9ede\u7d2f\u4e86)\u3002<\/p>\n<p>\u770b\u5230\u9019\u88e1\uff0c\u6709\u4eba\u53ef\u80fd\u76f4\u89ba\u9019\u6703\u591a\u96e3\uff0c\u628a\u9019\u6b21 Scan \u6bcf\u500b\u9ede\u62ff\u4f86\u8ddf\u4e0b\u4e00\u500b Scan \u6bd4\u5c0d\u4e0d\u5c31\u597d\u4e86\uff0c\u90a3\u6211\u5c31\u6703\u60f3\u554f\u600e\u9ebc\u6bd4\uff0c\u832b\u832b\u4eba\u6d77\u4e2d\uff0c\u7e3d\u8981\u6709\u500b\u4f9d\u64da\uff0c\u5565-\u4f60\u8aaa\u6311\u8fd1\u7684\u6bd4\u963f\uff0c\u9019\u500b\u6211\u807d\u904e\uff0c\u5c08\u696d\u9ede\u8aaa\u6cd5\u300eICP \uff08 Iterative Closest Point \uff09\u300f\uff0c\u9019\u6211\u5c31\u63d0\u5230\u9019\u88e1\uff0c\u4e0a\u9762\u90a3\u7bc7\u6587\u7ae0\u6709\u4ecb\u7d39\u3002\u4f86\u5f35\u5716\u544a\u8a34\u4f60\u9019\u6c34\u53ef\u6df1\u4e86<\/p>\n<p><a href=\"http:\/\/www.cnblogs.com\/gaoxiang12\/p\/3695962.html\">http:\/\/www.cnblogs.com\/gaoxiang12\/p\/3695962.html<\/a>\u00a0\u4e4b\u547c\u4e0a\u7684\u5927\u795e<\/p>\n<p><a href=\"http:\/\/ivory-cavern.blogspot.tw\/2009\/11\/icp-iterative-closest-point-c.html\">http:\/\/ivory-cavern.blogspot.tw\/2009\/11\/icp-iterative-closest-point-c.html<\/a>\u00a0ICP \u5be6\u505a<\/p>\n<pre class=\"mermaid\">graph LR\n    subgraph ICP [\u50b3\u7d71 ICP: \u9ede\u5c0d\u9ede\u6700\u8fd1\u9130\u6bd4\u5c0d]\n    P1[\u6383\u63cf\u9ede\u96f2 P] -->|\u641c\u5c0b\u6700\u8fd1\u9ede (\u8017\u6642 O(N log N))| Q1[\u53c3\u8003\u9ede\u96f2 Q]\n    end\n    subgraph NDT [NDT: \u9ede\u5c0d\u9ad8\u65af\u5206\u4f48\u6bd4\u5c0d]\n    P2[\u6383\u63cf\u9ede\u96f2 P] -->|\u6295\u5f71\u81f3\u9ad4\u7d20\u6a5f\u7387\u5206\u4f48 (\u76f4\u63a5\u8a55\u4f30\u9023\u7e8c\u7a7a\u9593)| V[3D \u9ad4\u7d20\u5e38\u614b\u5206\u4f48 N(\u03bc, \u03a3)]\n    end<\/pre>\n<p>\u597d\u5566\uff0cICP \u626f\u9060\u4e86\uff0c\u53ea\u662f\u8981\u544a\u8a34\u5927\u5bb6\u4e26\u975e\u53ea\u6709 NDT \u800c\u5df2\uff0c\u56de\u5230\u6b63\u984c\u00a0NDT \u2013 TKU\uff0c\u9019\u90e8\u4efd\u6211\u5c31\u7167\u8005\u6295\u5f71\u7247\u4f86\u8ac7\uff0c\u4e5f\u8acb\u5404\u4f4d\u540c\u6642\u770b\u6295\u5f71\u7247\u6703\u6bd4\u8f03\u597d\u61c2<\/p>\n<h4>PAGE 2:<\/h4>\n<h4>What is ndt_tku<\/h4>\n<p>NDT \u5728 SLAM \u4e0a\u7684\u57fa\u672c\u73a9\u6cd5\u7b97\u662f\u884c\u4e4b\u591a\u5e74\uff0c\u5728 ROS \u7cfb\u7d71\u4e0a\u4e5f\u5df2\u7d93\u6709 PCL \u6574\u6210\u76f8\u7576\u65b9\u4fbf\u7684 CLASS\uff0c<a href=\"http:\/\/Transformhttp:\/\/www.pointclouds.org\/documentation\/tutorials\/normal_distributions_transform.php\">How to use Normal Distributions<\/a>\u9019\u7bc7\u6587\u7ae0\u4e5f\u5df2\u7d93\u8a73\u7d30\u6559\u5b78\u904e\uff0c\u800c NDT-TKU\uff0c\u5247\u662f\u6839\u64da \u7af9\u5167\u5148\u751f(TAKEUCHI) \u6559\u6388\u63d0\u51fa\u7684\u8ad6\u6587\u00a0<a href=\"http:\/\/ieeexplore.ieee.org\/document\/4058864\/\">A 3-D Scan Matching using Improved 3-D Normal Distributions Transform for Mobile Robotic Mapping<\/a>(\u8ad6\u6587\u7db2\u8def\u4e0d\u516c\u958b) NDT \u518d\u9032\u5316\u7248\u672c\uff0c\u76f8\u95dc\u5be6\u505a\u5247\u662f\u5728\u4ed6\u9580\u7684 Open source project\u300e<a href=\"https:\/\/github.com\/CPFL\/Autoware\">Autoware<\/a>\u300f\u4e2d\u7684 <a href=\"https:\/\/github.com\/CPFL\/Autoware\/tree\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\">ROS node<\/a><\/p>\n<h4>PAGE3:<\/h4>\n<h4>Why is ndt_tku<\/h4>\n<p>\u7531\u65bc\u6211\u662f\u7814\u7a76\u4ed6\u9580\u7684\u6574\u500b Autoware \u5c08\u6848\uff0c\u88e1\u9762\u5176\u5be6\u5df2\u7d93\u6709\u5be6\u505a PCL NDT \u7684 Node \uff0c\u5728\u975e\u5e38\u597d\u5947\u7684\u60c5\u6cc1\u4e0b\u6df7\u9032\u4ed6\u9580\u7684 slack \uff0c\u63d0\u554f\u70ba\u4f55\u8981\u81ea\u5df1\u4f5c\u4e00\u5957 NDT Tku \u7248\u672c\uff0c\u4e3b\u8981\u539f\u56e0\u5c31\u662f<\/p>\n<ol>\n<li>\n<p>\u662f\u6307\u5c0e\u6559\u6388\u63d0\u51fa\u7684<\/p>\n<\/li>\n<li>\n<p>PCL \u7684 cuda \u5316\u6975\u5ea6\u9ebb\u7169\uff0c\u5de5\u7a0b\u5e2b\u8868\u793a\u4e0d\u5982\u81ea\u5e79\u4e00\u5957\uff0c\u7136\u5f8c cuda \u5316<\/p>\n<\/li>\n<\/ol>\n<p>1 \u5f88\u597d\u7406\u89e3\uff0c2 \u5247\u662f\u70ba\u4e86\u5229\u7528\u986f\u793a\u5361\u7684 GPU\u52a0\u901f\u5316\u3002\u5c0d\u65bc\u81ea\u52d5\u99d5\u99db\u4f86\u8aaa\uff0c\u80fd\u5feb\u4e00\u9ede\u7b97\u51fa\uff0c\u5c31\u53ef\u4ee5\u8b93\u5f8c\u7e8c\u7684\u5224\u65b7\u66f4\u5bb9\u6613\u57f7\u884c\uff0c\u63d0\u9ad8\u5b89\u5168\u6027<\/p>\n<h4>PAGE4:<\/h4>\n<h4>Outline<\/h4>\n<p>\u7406\u89e3\u9019\u6771\u897f\u662f\u8981\u7167\u9806\u5e8f\u4f86\uff0c\u5148\u77e5\u9053 Normal-Distributions \uff0c\u624d\u80fd\u77e5\u9053\u00a0Normal-Distributions Transform \u5230\u5e95\u5728\u73a9\u5565\u3002\u6700\u5f8c\u624d\u80fd\u5c0e\u5230 TKU \u505a\u4e86\u5565\u6539\u9032\u3002<\/p>\n<h4>PAGE 5:<\/h4>\n<h4>Normal Distribution<\/h4>\n<p>\u6b63\u614b\u5206\u4f48 \u2013 \u662f\u4e00\u500b\u5728<a href=\"https:\/\/zh.wikipedia.org\/wiki\/%E6%95%B8%E5%AD%B8\">\u6578\u5b78<\/a>\u3001<a href=\"https:\/\/zh.wikipedia.org\/wiki\/%E7%89%A9%E7%90%86\">\u7269\u7406<\/a>\u53ca<a href=\"https:\/\/zh.wikipedia.org\/wiki\/%E5%B7%A5%E7%A8%8B\">\u5de5\u7a0b<\/a>\u7b49<a href=\"https:\/\/zh.wikipedia.org\/wiki\/%E9%A0%98%E5%9F%9F\">\u9818\u57df<\/a>\u90fd\u975e\u5e38\u91cd\u8981\u7684\u6a5f\u7387\u5206\u4f48\uff0c\u7531\u65bc\u9019\u500b<a href=\"https:\/\/zh.wikipedia.org\/wiki\/%E5%88%86%E4%BD%88%E5%87%BD%E6%95%B8\">\u5206\u5e03\u51fd\u6578<\/a>\u5177\u6709\u5f88\u591a\u975e\u5e38\u6f02\u4eae\u7684\u6027\u8cea.\u4f7f\u5f97\u5176\u5728\u8af8\u591a\u6d89\u53ca\u7d71\u8a08\u79d1\u5b78\u96e2\u6563\u79d1\u5b78\u7b49\u9818\u57df\u7684\u8a31\u591a\u65b9\u9762\u90fd\u6709\u8457\u91cd\u5927\u7684\u5f71\u97ff\u529b<\/p>\n<p><a href=\"https:\/\/en.wikipedia.org\/wiki\/Normal_distribution\">https:\/\/en.wikipedia.org\/wiki\/Normal_distribution<\/a><\/p>\n<p>$$<br \/>\np(\\mathbf{x}) = \\frac{1}{(2\\pi)^{D\/2}\\sqrt{|\\mathbf{\\Sigma}|}} \\exp\\left(-\\frac{(\\mathbf{x}-\\mathbf{\\mu})^T \\mathbf{\\Sigma}^{-1}(\\mathbf{x}-\\mathbf{\\mu})}{2}\\right)<br \/>\n$$<br \/>\n<em>(\u591a\u5143\u5e38\u614b\u6a5f\u7387\u5bc6\u5ea6\u5206\u4f48\u51fd\u6578\uff0c\u5176\u4e2d $\\mathbf{\\mu}$ \u70ba\u9ad4\u7d20\u5167\u9ede\u4e4b\u5747\u503c\u5411\u91cf\uff0c$\\mathbf{\\Sigma}$ \u70ba\u5171\u8b8a\u7570\u6578\u77e9\u9663)<\/em><\/p>\n<p>\u9023\u7e8c\u7684\u6771\u897f\uff0c\u6578\u5b78\u53ea\u8981\u80fd\u8868\u793a\u51fa\u4f86\u5c31\u80fd\u7b97\uff0c\u5148\u7406\u89e3\u9019\u6a23\u7684\u5206\u4f48\u5c0d\u65bc\u9ede\u96f2\u7684\u904b\u7b97\u662f\u6709\u610f\u7fa9\u7684<\/p>\n<h4>PAGE 6:<\/h4>\n<h4><strong>Normal Distribution Transform<\/strong><\/h4>\n<p>\u9019\u88e1\u8ac7\u8ac7\u600e\u9ebc\u4f5c\u4e00\u822c\u7248\u7684 NDT\uff0c\u5c07\u6240\u6709\u7684\u9ede\u96f2\u5148\u5207\u6210\u4e00\u683c\u4e00\u683c ND Voxel<\/p>\n<p>The normal-distributions transform can be\u00a0 described as a method for compactly representing a surface.<\/p>\n<p>\u6b63\u614b\u5206\u4f48\u7d66\u51fa\u4e86\u9ede\u96f2\u7684\u5206\u6bb5\u5e73\u6ed1\u8868\u793a\uff0c\u5177\u6709\u9023\u7e8c\u7684\u5c0e\u6578\u3002 \u6bcf\u500b<a href=\"https:\/\/zh.wikipedia.org\/wiki\/%E6%A9%9F%E7%8E%87%E5%AF%86%E5%BA%A6%E5%87%BD%E6%95%B8\">PDF\uff08\u6a5f\u7387\u5bc6\u5ea6\u51fd\u6578\uff09<\/a>\u53ef\u4ee5\u770b\u4f5c\u662f\u5c40\u90e8\u8868\u9762\u7684\u8fd1\u4f3c\u503c\uff0c\u63cf\u8ff0\u4e86\u8868\u9762\u7684\u4f4d\u7f6e\u4ee5\u53ca\u5176\u53d6\u5411\u548c\u5e73\u6ed1\u5ea6\u3002<\/p>\n<p>A 2D laser scan from a mine tunnel (shown as points) and the PDFs describing the surface shape. Each cell is a square with 2 m side length in this case. Brighter areas represent a higher probability. PDFs have been computed only for cells with more than five points.<\/p>\n<pre class=\"mermaid\">graph TD\n    Cloud[\u8f38\u5165\u5149\u9054 LiDAR \u9ede\u96f2] --> Voxelize[3D \u9ad4\u7d20\u7db2\u683c\u5283\u5206 (Voxel Grid)]\n    Voxelize --> Cell1[Voxel Cell 1: \u8a08\u7b97 \u03bc1, \u03a31]\n    Voxelize --> Cell2[Voxel Cell 2: \u8a08\u7b97 \u03bc2, \u03a32]\n    Voxelize --> CellN[Voxel Cell N: \u8a08\u7b97 \u03bcn, \u03a3n]<\/pre>\n<h4>PAGE7 :<\/h4>\n<h4>NDT in tunnel \u2013 3D<\/h4>\n<p>\u793a\u610f\u5716\uff0c\u81ea\u5df1\u770b\u6295\u5f71\u7247\u5566<\/p>\n<h4>PAGE 8:<\/h4>\n<h4>\u5982\u4f55\u8868\u793a\u9ede\u96f2 Cell \u7684\u6a5f\u7387\u5206\u5e03<\/h4>\n<p>D-dimensional normal random process, the likelihood of having measured ~x is\u00a0 where ~yk=1,\u2026, m are the positions of the reference scan points contained in the cell.<\/p>\n<p>\u6b63\u614b\u5206\u4f48\u7d66\u51fa\u4e86\u9ede\u96f2\u7684\u5206\u6bb5\u5e73\u6ed1\u8868\u793a\uff0c\u5177\u6709\u9023\u7e8c\u7684\u5c0e\u6578\u3002 \u6bcf\u500bPDF\u53ef\u4ee5\u770b\u4f5c\u662f\u5c40\u90e8\u8868\u9762\u7684\u8fd1\u4f3c\u503c\uff0c\u63cf\u8ff0\u4e86\u8868\u9762\u7684\u4f4d\u7f6e\u4ee5\u53ca\u5176\u53d6\u5411\u548c\u5e73\u6ed1\u5ea6\u3002<\/p>\n<p>\u5354\u65b9\u5dee\u77e9\u9663\u7684\u7279\u5fb5\u5411\u91cf\u548c\u7279\u5fb5\u503c\u53ef\u4ee5\u8868\u9054\u8868\u9762\u4fe1\u606f<\/p>\n<p>Each PDF can be seen as an approximation of the local surface, describing the position of the surface as well as its orientation and smoothness.<\/p>\n<h4>PAGE 9, 10:<\/h4>\n<h4>Scan registration<\/h4>\n<p>\u70ba\u4e86\u627e\u51fa\u5169\u6b21 SCAN \u7684 spatial transformation function T(~p, ~x) that moves a point ~x in space by the pose ~p. \u53ef\u4ee5\u5229\u7528\u525b\u525b\u5f97\u5230\u7684 CELL PDF \uff0c\u53bb\u627e\u51fa\u6700\u63a5\u8fd1\uff08max\uff09\u7684\u5206\u4f48\u51fd\u6578<\/p>\n<p>NDT score \u53ef\u4ee5\u7528\u65bc\u725b\u9813\u6cd5\uff0c\u8868\u793a\u662f\u5426\u9054\u5230\u6700\u597d\u7684\u5206\u6578\u3002Gaussian approximation \u5247\u53ef\u4ee5\u6e1b\u5c11\u904b\u7b97<\/p>\n<p>(\u770b\u6700\u4e0a\u9762\u63a8\u85a6\u7684\u8ad6\u6587\u6216\u662f\u6295\u5f71\u7247\uff0c\u6c92\u8fa6\u6cd5\u591a\u52a0\u8a3b\u89e3)<\/p>\n<h4>PAGE 11:<\/h4>\n<h4>Newton\u2019s algorithm for<\/h4>\n<ul>\n<li>\n<p>Newton\u2019s algorithm can be used to find the parameters ~p that optimise s(~p)<\/p>\n<\/li>\n<li>\n<p>Newton\u2019s method iteratively solves the equation H\u2206~p = \u2212~g<\/p>\n<\/li>\n<li>\n<p>g and H are partial differential and second order partial differential of<\/p>\n<\/li>\n<\/ul>\n<p>\u8001\u6a23\u5b50\u8d85\u904e15\u500b\u6578\u5b78\u7b26\u865f\u6211\u5011\u5c31\u5f88\u4e0d\u60f3\u7406\u6703<\/p>\n<p><a href=\"https:\/\/www.youtube.com\/watch?v=Quw4ZHLH2CY\">https:\/\/www.youtube.com\/watch?v=Quw4ZHLH2CY<\/a><\/p>\n<p>\u5229\u7528\u5fae\u5206\u627e\u51fa\u5207\u7dda\uff0c\u758a\u4ee3\u6cd5\u8da8\u8fd1\u627e\u51fa function = 0 \u7684\u6839<\/p>\n<h4>PAGE 12:<\/h4>\n<h4>NDT FLOW<\/h4>\n<pre class=\"mermaid\">flowchart TD\n    Start([\u958b\u59cb\u914d\u6e96]) --> InitT[\u521d\u59cb\u5316\u65cb\u8f49\u5e73\u79fb\u5411\u91cf p = [tx, ty, tz, roll, pitch, yaw]]\n    InitT --> MapPoints[\u5c07\u7576\u524d\u6383\u63cf\u9ede\u4f9d p \u8b8a\u63db\u81f3\u5730\u5716\u5750\u6a19\u7cfb: x' = R*x + t]\n    MapPoints --> CalcScore[\u8a55\u4f30\u9ad4\u7d20\u5167 NDT \u7e3d\u76f8\u4f3c\u5ea6\u5206\u6578: s(p)]\n    CalcScore --> CalcDeriv[\u8a08\u7b97\u68af\u5ea6\u5411\u91cf g \u8207 Hessian \u77e9\u9663 H]\n    CalcDeriv --> NewtonStep[\u725b\u9813\u6cd5\u758a\u4ee3\u66f4\u65b0: \u0394p = - H^-1 * g]\n    NewtonStep --> CheckConv{\u662f\u5426\u6eff\u8db3\u6536\u6582\u689d\u4ef6?}\n    CheckConv -- \u5426 --> UpdateP[p = p + \u0394p] --> MapPoints\n    CheckConv -- \u662f --> Done([\u914d\u6e96\u6210\u529f\uff0c\u8f38\u51fa\u7cbe\u78ba 6DoF \u4f4d\u59ff])<\/pre>\n<h4>PAGE13:<\/h4>\n<h4><strong>\u6211\u77e5\u9053<strong><strong>\u5927\u5bb6<\/strong><\/strong>\u770b\u6578\u5b57<strong><strong>\u5f88<\/strong><\/strong>\u75db\u82e6<\/strong><\/h4>\n<p>\u5176\u5be6\u7576\u521d\u5206\u4eab\u7684\u5c0d\u8c61\u90fd\u4e26\u975e\u6578\u5b78\u76f8\u95dc\u51fa\u8eab\uff0c\u4e5f\u4e26\u975e\u4f5c SLAM \u70ba\u4e3b\uff0c\u6240\u4ee5\u6211\u731c\u8b1b\u5f97\u5728\u7565\u904e\uff0c\u7c21\u55ae\uff0c\u9084\u662f\u4e0d\u597d\u7406\u89e3\uff0c\u56e0\u6b64\u4e0d\u5c11\u6771\u897f\u90fd\u6c92\u6709\u5728\u6df1\u5165\uff0c\u4f46\u662f\u70ba\u4e86\u80fd\u5c0e\u5230 TKU \u7684\u4fee\u6539\uff0c\u9084\u6709\u5be6\u505a\u4e0a\u7684\u53c3\u6578\u8a2d\u5b9a\uff0c\u4e0d\u5f97\u4e0d\u63d0\u3002<\/p>\n<h4>PAGE14:<\/h4>\n<h4>About ND Voxel size<\/h4>\n<ul>\n<li>\n<p>\u592a\u5c0f<\/p>\n<ul>\n<li>\n<p>\u904b\u7b97\u91cf\u5927\uff0cmemory \u6d88\u8017\u5927<\/p>\n<\/li>\n<li>\n<p>\u5339\u914d\u7cbe\u78ba<\/p>\n<\/li>\n<li>\n<p>\u4f46\u5c0f\u65bc\u4e94\u500b\u9ede\uff0c\u5247\u5f88\u96e3\u5f62\u6210\u6b63\u614b\u5206\u4f48<\/p>\n<\/li>\n<\/ul>\n<\/li>\n<li>\n<p>\u592a\u5927<\/p>\n<ul>\n<li>\n<p>\u904b\u7b97\u91cf\u5c11<\/p>\n<\/li>\n<li>\n<p>\u5339\u914d\u4e0d\u7cbe\u78ba<\/p>\n<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<p>\u770b\u5230\u9019\u88e1\uff0c\u5176\u5be6\u53ef\u4ee5\u767c\u73fe NDT SLAM \u4e3b\u8981\u7684\u4f5c\u6cd5\u548c\u91cd\u9ede\u90fd\u5728\u65bc \u8655\u7406 PDF \u7684\u6700\u4f73\u5316\u554f\u984c\uff0c\u5f9e\u7d50\u8ad6\u4f86\u770b\uff0c\u80fd\u4e0b\u624b\u7684\u5c31\u662f Cell \u7684\u53c3\u6578\u8abf\u6574\u3002\u9019\u901a\u5e38\u4e5f\u5f88\u96e3\u6c7a\u5b9a\uff0c\u5fc5\u9808\u53c3\u7167\u74b0\u5883\uff08\u662f\u5426\u9644\u8fd1\u6709\u5f88\u591a\u5efa\u7bc9\u7269\uff0c\u9084\u662f\u5927\u5e73\u539f\uff09\uff0cSensor\u7684\u53c3\u6578\uff08\u6383\u63cf\u5bc6\u5ea6\uff0c\u6642\u9593\uff09\u7b49\u7b49\uff0c\u4f46\u7e3d\u800c\u8a00\u4e4b\u90fd\u662f\u4e0a\u9762\u7684\u6240\u63d0\u5230\u7684\u60c5\u6cc1\uff0c\u592a\u5927\u6216\u592a\u5c0f\u90fd\u4e0d\u597d\u3002<\/p>\n<h4>PAGE 15 :<\/h4>\n<h4>NDT \u2013 TKU version<\/h4>\n<p>\u4f7f\u7528\u683c\u5b50\u91cd\u758a\u7684\u65b9\u6cd5\uff0c\u4f46\u9019\u4e26\u975e TKU \u6240\u63d0\u51fa\u3002<\/p>\n<p>Peter Biber and Wolfgang Stra\u00dfer: \u201cThe Normal Distributions Transform:A New Approach to Laser Scan Matching\u201d, Proceedings of the 2003 IEEE\/RSJ International Conference on Intelligent Robots and Systems, pp. 2743\u20132748, 2003<\/p>\n<p>TKU \u63a1\u7528\u6bcf\u500b Cell \u90fd\u6709\u4e00\u534a\u7684\u91cd\u758a\u3002\u597d\u8655\u58de\u8655\u90fd\u5f88\u660e\u986f\u3002\u9019\u65b9\u767c\u5728\u5206\u985e\u4e0a\u70ba\u300eTrilinear interpolation\u300f\uff0c\u5229\u7528\u91cd\u758a Cell \u7684\u4e0d\u9023\u7e8c\u6027\uff0c\u63d0\u9ad8 Cell \u4ea4\u754c\u8655\u7684\u9ede\u96f2\u5206\u4f48\u7a69\u56fa\u59d3<\/p>\n<h4>PAGE 16:<\/h4>\n<h4>\u5716\u7247\u6709\u611f<\/h4>\n<p>\u81ea\u5df1\u770b\u6295\u5f71\u7247\u5566<\/p>\n<h4>PAGE 17:<\/h4>\n<h4>TKU \u2013 ND Voxel size<\/h4>\n<p>\u5728CELL\u91cd\u758a\u7684\u57fa\u790e\u4e0a\uff0c\u8ffd\u52a0\u8ddd\u96e2\u548c\u6642\u9593\u7684\u53c3\u6578\uff0c\u5148\u5206\u6210\u5169\u500b\u968e\u6bb5\uff0c<\/p>\n<ul>\n<li>\n<p>Converging state<\/p>\n<ul>\n<li>\u6309\u7167\u8ddd\u96e2\u5207\u5206 ND Voxel size\uff0c\u4e26\u904b\u7b97<\/li>\n<\/ul>\n<\/li>\n<li>\n<p>Adjust state<\/p>\n<ul>\n<li>\u5230\u4e00\u5b9a\u6b21\u6578\u5f8c\u5247\u901a\u901a\u7528\u6700\u5c0f\u683c\u5b50\u4f86\u904b\u7b97<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<p>Converging \u968e\u6bb5\uff0c\u5148\u6839\u64da\u8207 scanner \u7684\u8ddd\u96e2\u4f86\u6c7a\u5b9a Cell \u7684\u5927\u5c0f\uff0c\u9019\u662f\u5c6c\u65bc\u7269\u7406\u9650\u5236\uff0c\u96e2 Scanner \u8d8a\u8fd1\uff0c\u9ede\u96f2\u4e00\u5b9a\u8d8a\u5bc6\u96c6\uff0c\u56e0\u6b64\u53ef\u4ee5\u5c07 Cell \u5207\u5c0f\u4e00\u9ede\uff0c\u8d8a\u9060\u8d8a\u5927\uff0c\u9019\u6a23\u53ef\u4ee5\u52a0\u901f\u6536\u6582\u904b\u7b97\u3002<\/p>\n<p>Adjust \u968e\u6bb5\uff0c\u7576 Converging \u6536\u6582\u6b21\u6578\u9054\u5230\u4e00\u5b9a\u5f8c\uff0c\u5247\u53ef\u4ee5\u958b\u59cb\u901a\u901a\u4f7f\u7528\u6700\u5c0f Cell \u4f86\u9032\u884c\u6536\u6582\u904b\u7b97\uff0c\u9019\u6a23\u4e00\u4f86\u4e5f\u53ef\u4ee5\u4fdd\u6301 match \u7cbe\u5ea6\u3002<\/p>\n<p>\u9019\u662f TKU \u6240\u63d0\u51fa\u7684\u4e3b\u8981\u6982\u5ff5\uff0c\u800c\u8a73\u7d30\u5be6\u9a57\u6578\u64da\uff0c\u5247\u8acb\u5404\u70ba\u81ea\u5df1\u53bb\u770b\u8ad6\u6587\uff08\u8457\u4f5c\u6b0a\uff09\u3002<\/p>\n<h4>Page 18<\/h4>\n<h4>Reference:<\/h4>\n<p>1.A 3-D Scan Matching using Improved 3-D Normal Distributions Transform for Mobile Robotic Mapping(\u7db2\u8def\u4e0a\u4e0d\u516c\u958b)<\/p>\n<p>2.The Three-Dimensional Normal-Distributions Transform \u2014 an Efficient Representation for Registration, Surface Analysis, and Loop Detection<\/p>\n<p>3.The Normal Distributions Transform:A New Approach to Laser Scan Matching<\/p>\n<h4>PAGE 19\uff1a<\/h4>\n<h4>Other<\/h4>\n<p>\u9019\u662f\u4e00\u4e9b\u5be6\u505a\u4e0a\u53ef\u4ee5\u8abf\u6574\u7684\u53c3\u6578\uff0c\u6709\u7a7a\u7684\u4eba\u53ef\u4ee5\u81ea\u884c\u53bb\u770b PCL \u5be6\u505a<\/p>\n<h4>PAGE 20:<\/h4>\n<h4>Score detail<\/h4>\n<p>\u9019\u662f\u88dc\u5145 PAGE 10 \u7684 Score \u7684\u904b\u7b97\u3002<\/p>\n<p>Ending<\/p>\n<p>\u5982\u679c\u5404\u4f4d\u770b\u5230\u9019\u908a\u7684\u8a71\uff0c\u4e5f\u7b97\u662f\u5927\u529f\u544a\u6210\uff0c\u4f46\u9019\u7bc7\u771f\u7684\u4e26\u975e\u5165\u9580\u6587\u7ae0\uff0c\u6709\u554f\u984c\u7684\u8a71\u53ef\u662f\u767c\u554f\u770b\u770b\uff0c\u6216\u662f\u81ea\u884c\u53bb\u770b\u8ad6\u6587\u3002<\/p>\n<p>\u63a5\u4e0b\u7684\u90e8\u4efd\uff0c\u771f\u7684\u5c31\u662f\u5c6c\u65bc Autoware \u7684 trace code \u7d00\u9304\u3002<\/p>\n<p>\u2014\u2014\u2014\u2014\u2014\u2014\u5206\u9694\u4e00\u4e0b\u2014\u2014\u2014\u2014\u2014\u2014\u2014<\/p>\n<h3>Parameter \u53ef\u8abf\u63a7\u53c3\u6578<\/h3>\n<h4><strong>Iteration<\/strong><\/h4>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L321\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L321<\/a><\/p>\n<p>\u6700\u5927\u8fed\u4ee3\u6b21\u6578\uff0c\u9810\u8a2d100<\/p>\n<h4><strong>G_MAP_CELLSIZE<\/strong><\/h4>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L11\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L11<\/a><\/p>\n<p>CELL \u55ae\u4f4d\u5927\u5c0f<\/p>\n<p>\u7b49\u50f9\u65bcPCL setResolution\u00a0\u9810\u8a2d1.0<\/p>\n<h4><strong>leaf size<\/strong><\/h4>\n<p>Voxel grid size \u8abf\u6574\u53c3\u6578<\/p>\n<p>setLeafSize<\/p>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L233\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L233<\/a><\/p>\n<h4><strong>scan_points_num<\/strong><\/h4>\n<p>\u6383\u63cf\u5f8c\u7684\u6700\u5927\u9ede\u96f2\u6578\u91cf default 13000\uff0c\u9019\u500b\u8981\u5c0f\u5fc3\u8a2d\u5b9a\uff0c\u9ede\u6578\u592a\u591a\u6703\u5831\u6389<\/p>\n<h4>Epsilon<\/h4>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L334\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L334<\/a><\/p>\n<p>\u6536\u6582\u503c\u5f97\u6700\u5c0f\u8b8a\u52d5\u503c\uff0c\u8d8a\u5c0f\u8d8a\u7cbe\u78ba\uff0c\u8d8a\u5927\u7b97\u8d8a\u4e45\uff0c\u901a\u5e38\u4e0d\u6539<\/p>\n<h4><strong>add_point_map(NDmap, &amp;map_points[i]);<\/strong><\/h4>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L501\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L501<\/a><\/p>\n<p>\u9019\u4e00\u884c\u901a\u5e38\u8a3b\u89e3\u6389\uff0c\u4e3b\u8981\u7528\u65bc\u908a match \u908a\u66f4\u65b0\u5730\u5716\uff0c\u7b97\u662f MATCHING AND MAPPING \u7684\u7d50\u5408\uff0c\u4f46\u662f\u9084\u6c92\u6709\u5b8c\u6210\u3002<\/p>\n<h4><strong>_downsampler_num<\/strong><\/h4>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/lib\/ndt_tku\/src\/newton.cpp#L418\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/lib\/ndt_tku\/src\/newton.cpp#L418<\/a><\/p>\n<p>\u9019\u662f\u9810\u8a2d voxel filter \u7684\u8a2d\u5b9a\uff0c0=distance\uff0c1= voxel_grid\uff0c\u901a\u5e38\u662f 1 \uff0c\u6b63\u5e38\u4f86\u8aaa\u9019\u61c9\u8a72\u662f\u5916\u90e8\u53ef\u53c3\u6578\u5316\u7684\u90e8\u5206\u3002\u4f46\u5b83\u5011\u9084\u6c92\u5b8c\u6210\u3002<\/p>\n<h4><strong>g_use_gnss<\/strong><\/h4>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L76\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L76<\/a><\/p>\n<p>\u9810\u8a2d\u6c92\u6709\u555f\u52d5 GPS , \u7167\u7406\u8aaa\u4e5f\u61c9\u8a72\u5ba4\u5916\u90e8\u53ef\u8abf\u63a7\u53c3\u6578\uff0c\u4f46\u662f\u4ed6\u9580\u9084\u5728\u4f5c\u3002<\/p>\n<h4><strong>layer_select<\/strong><\/h4>\n<p>\u61c9\u8a72\u662f\u70ba\u4e86\u4e4b\u5f8c\u7684TKU\u5207\u5272\u683c\u5b50\u800c\u8a2d\u8a08\u7684\uff0c\u9084\u4e0d\u6e05\u695a\u600e\u9ebc\u7528\uff0c\u554f\u4ed6\u5011\u4e5f\u6c92\u56de\u7b54<\/p>\n<p>\u00a0<\/p>\n<h3><strong>CODE \u8b1b\u89e3<\/strong><\/h3>\n<p>\u4e3b\u8981\u4e09\u5927\u6a94\u6848<\/p>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp<\/a><\/p>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/lib\/ndt_tku\/src\/algebra.cpp\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/lib\/ndt_tku\/src\/algebra.cpp<\/a><\/p>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/lib\/ndt_tku\/src\/newton.cpp\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/lib\/ndt_tku\/src\/newton.cpp<\/a><\/p>\n<h4><strong>add_point_map<\/strong><\/h4>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L629\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L629<\/a><\/p>\n<p>\u4e00\u6b21\u4efd\u7684\u9ede\u96f2\u532f\u5165\u589e\u91cf\u9ad4\u7d20\u66f4\u65b0\u6d41\u7a0b\uff1a<\/p>\n<pre class=\"mermaid\">flowchart LR\n    NewPoint[\u65b0\u6383\u63cf\u9ede x_new] --> Locate[\u5b9a\u4f4d\u6240\u5c6c 3D \u9ad4\u7d20 Cell]\n    Locate --> CheckCount{\u9ad4\u7d20\u5167\u9ede\u6578\u662f\u5426\u8db3\u5920?}\n    CheckCount -- \u672a\u6eff\u81e8\u754c\u503c --> Cache[\u66ab\u5b58\u65bc\u968a\u5217]\n    CheckCount -- \u8db3\u5920 --> IncUpdate[\u589e\u91cf\u66f4\u65b0\u5747\u503c\u5411\u91cf pk \u8207\u5171\u8b8a\u7570\u6578\u77e9\u9663 \u03a3k]<\/pre>\n<h4><strong>adjust3d<\/strong><\/h4>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/lib\/ndt_tku\/src\/newton.cpp#L195\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/lib\/ndt_tku\/src\/newton.cpp#L195<\/a><\/p>\n<p>\u5305\u542b\u4e00\u6b21\u4efd\u7684 \u6536\u6582\u8a08\u7b97\uff0c\u4e0a\u9762\u7684\u4e00\u6b21while \u904b\u7b97<\/p>\n<p>$$<br \/>\n\\mathbf{g} = -\\sum_{i=1}^{n} \\nabla s_i(\\mathbf{x}), \\quad \\mathbf{H} = -\\sum_{i=1}^{n} \\nabla^2 s_i(\\mathbf{x})<br \/>\n$$<\/p>\n<p>gsum , hsum \u5206\u5225\u8868\u793a\u5e95\u4e0b\u7684\u4e00\u6b21\u5fae\u5206\uff0c\u548c\u4e8c\u6b21\u5fae\u5206\u7684 hessian matrix<\/p>\n<p>$$<br \/>\n\\Delta \\mathbf{p} = -\\mathbf{H}^{-1} \\mathbf{g}<br \/>\n$$<br \/>\n<em>(\u725b\u9813\u6cd5\u758a\u4ee3\u4fee\u6b63\u5411\u91cf)<\/em><\/p>\n<p>\u6bcf\u6b21\u7684\u904b\u7b97\u90fd\u6703\u547c\u53eb get_ND \u53bb\u66f4\u65b0\u503c<\/p>\n<h4><strong>get_ND<\/strong><\/h4>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L685\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L685<\/a><\/p>\n<p>\u9019\u88e1\u6703\u9032\u884c newton \u6cd5\u7684\u66f4\u65b0\u503c\u554f\u984c\uff0c\u4e0a\u9762\u7684 for all points \u4e00\u6b21\u904b\u7b97<\/p>\n<h4>**update_covariance **<\/h4>\n<p>\u66f4\u65b0\u6bcf\u500b ND \u7684MEAN AND CONVARIANCE<\/p>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L595\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L595<\/a><\/p>\n<p>\u8ad6\u6587\u88e1\u6709\u63d0\u5230\uff0c\u6c42\u6700\u5c0f eigen value of covariance \u53ef\u80fd\u6703\u9047\u5230\u6536\u6582\u8cea\u96d6\u7136\u5f88\u5c0f\uff0c\u4f46\u662f\u53ef\u4ee5\u7e7c\u7e8c\u6536\u6582\u7684\u60c5\u6cc1\uff0c\u8ad6\u6587\u5efa\u8b70\u4f4e\u65bc0.001\u6642\uff0c\u5247\u4e0d\u518d\u7e7c\u7e8c 0.001\uff0c\u5efa\u8b70\u4e0d\u8981\u4e82\u52d5\uff0c\u6e2c\u8a66\u5f8c\u4e26\u6c92\u6709\u7279\u5225\u6548\u679c\uff0c\u56e0\u70ba\u9019\u662f\u6975\u9650\u60c5\u6cc1<\/p>\n<p>A measure of this problem in [1] is to modify the minimum eigen value of the covariance matrix with 0.001 times as much as the maximum eigen value, if the minimum one is smaller than 0.001 times as much as the maximum one. The authors take same approaches. However, the number of overlapping ND voxel becomes eight in 3-D space.<\/p>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L882\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L882<\/a><\/p>\n<p>\u8ad6\u6587\u63d0\u5230\u7684\u6e1b\u5c11\u904b\u7b97\u91cf\u7684\u65b9\u6cd5\uff0c\u4e0d\u9032\u884c\u5168\u76e4\u7684 mean \u503c\u8a08\u7b97\uff0c\u800c\u662f\u6709\u65b0\u9ede\u52a0\u5165\u6642\u5224\u65b7\u662f\u5426\u6709\u9700\u8981\u55ce?\uff0c\u76f4\u63a5\u7528\u7c97\u7565\u7684\u65b9\u5f0f\u52a0\u5165\u65b0\u7684\u503c\u3002<\/p>\n<ol>\n<li>Incremental update of ND voxel The number of reference scan points Mk in the ND voxel k is increased as the environment map is expanded. Therefore, the computational load to obtain mean vectors pk(k = 1, \u2026, Mk) and covariance matrix \u03a3k(k = 1, \u2026, Mk) will increase according to equations (1), and (2). To decrease this load, the authors take the following incremental update equations to apply NDT to reference scan in ND voxel. When ND voxel k gets new reference points xki, then mean vector pk and covariance matrix \u03a3k are updated by following equations;<\/li>\n<\/ol>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L595\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L595<\/a><\/p>\n<h4><strong>set_sincos<\/strong><\/h4>\n<h4><strong>set_sincos2<\/strong><\/h4>\n<h4><strong>scan_transrate<\/strong><\/h4>\n<p>\u9019\u4e09\u500b\u90fd\u662f\u7528\u65bc\u8868\u793a \u505a transform \u7684 \u77e9\u9663\u65cb\u8f49\u5f0f<\/p>\n<p>$$<br \/>\nT(\\mathbf{p}, \\mathbf{x}) = \\mathbf{R}<em>{z}(\\psi) \\mathbf{R}<\/em>{y}(\\theta) \\mathbf{R}_{x}(\\phi) \\mathbf{x} + \\mathbf{t}<br \/>\n$$<\/p>\n<h4><strong>calc_summand3d<\/strong><\/h4>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/lib\/ndt_tku\/src\/newton.cpp#L38\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/lib\/ndt_tku\/src\/newton.cpp#L38<\/a><\/p>\n<p>$$<br \/>\ns(\\mathbf{p}) = -\\sum_{k=1}^{M} \\exp\\left(-\\frac{(\\mathbf{x}&#39;_k &#8211; \\mathbf{\\mu}_k)^T \\mathbf{\\Sigma}_k^{-1}(\\mathbf{x}&#39;_k &#8211; \\mathbf{\\mu}_k)}{2}\\right)<br \/>\n$$<\/p>\n<p>\u8a08\u7b97 ndt \u7684\u6a5f\u7387\u5206\u5e03\u7e3d\u5206\u6578\uff0c\u6703\u7528\u5230 probability_on_ND<\/p>\n<h4><strong>probability_on_ND<\/strong><\/h4>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L905\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/packages\/ndt_localizer\/nodes\/ndt_matching_tku\/ndt_matching_tku.cpp#L905<\/a><\/p>\n<p>\u6295\u5f71\u7247\u6709\u63d0\u5230\u904e\uff0c\u7528\u65bc\u8a08\u7b97\u5206\u6578\uff0c\u9ede\u96f2\u843d\u65bc\u6bcf\u500bnd\u88e1\u7684\u6a5f\u7387<\/p>\n<h4><strong>jacobi_matrix3d<\/strong><\/h4>\n<p><a href=\"https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/lib\/ndt_tku\/src\/algebra.cpp#L354\">https:\/\/github.com\/CPFL\/Autoware\/blob\/master\/ros\/src\/computing\/perception\/localization\/lib\/ndt_tku\/src\/algebra.cpp#L354<\/a><\/p>\n<p>\u4e9e\u53ef\u6bd4\u77e9\u9663\uff0c\u7528\u65bc\u725b\u9813\u6cd5\uff0c\u6c42 eigen value.<\/p>\n<p>\u00a0<\/p>\n<h3><strong>code flow<\/strong><\/h3>\n<pre class=\"mermaid\">flowchart LR\n    LiDAR[\u5149\u9054\u9ede\u96f2] --> Pre[\u964d\u63a1\u6a23\u904e\u6ffe] --> NDT_Localizer[Autoware NDT Localizer]\n    Map[3D PCD \u9ede\u96f2\u5730\u5716] --> NDT_Localizer\n    NDT_Localizer --> Odom[\u8f38\u51fa\u7cbe\u78ba\u8eca\u8f1b 3D \u5b9a\u4f4d\u8207 Odometry]<\/pre>\n<p>\u00a0<\/p>\n<p>\u5982\u6709\u4efb\u4f55\u4fb5\u6b0a\uff0c\u8acb\u544a\u77e5\u5c0f\u5f1f\uff0c\u5c0f\u5f1f\u6703\u62ff\u4e0b\uff0c\u4e5f\u8acb\u5404\u4f4d\u6ce8\u610f\u76f8\u95dc\u8457\u4f5c\u6b0a\u3002<\/p>\n","protected":false},"excerpt":{"rendered":"<p>\u777d\u9055\u8a31\u4e45\u6c92\u6709\u5beb blog \uff0c\u4e3b\u8981\u9084\u662f\u592a\u5fd9\u3002\u9019\u5468\u7d42\u65bc\u7a7a\u51fa\u6642\u9593\u53ef\u4ee5\u4f86\u88dc\u88dc\u4e4b\u524d\u7684\u7b46\u8a18\uff0c\u7531\u65bc\u6700\u8fd1\u624d\u5c0d\u5167\u90e8\u5206\u4eab\u6b64\u4e3b\u984c\u3002\u6240\u4ee5\u9019\u500b\u4e3b\u984c\u5370\u8c61\u6700\u6df1\u523b\u3002<br \/>\n\u96d6\u7136\u5f88\u60f3\u518d\u6b21\u5c31\u9032\u5165\u6b63\u984c\uff0c\u4f46\u662f\u7531\u65bc\u9019 blog \u7684\u98a8\u683c\u5c31\u662f\u5ee2\u8a71\u591a\uff0c\u6240\u4ee5\u6211\u7e7c\u7e8c\u5beb\u5ee2\u8a71\uff0c\u8ac7\u5230\u7684\u9019\u500b\u4e3b\u984c\uff0c\u7528\u5230\u5927\u91cf\u7684\u6578\u5b78\uff0c\u800c\u5c0f\u5f1f\u6211\u5728\u5927\u5b78\u57fa\u672c\u4e0a\u300e\u6a5f\u7387\u300f\u300e\u7dda\u6027\u4ee3\u6578\u300f\u300e\u5fae\u7a4d\u5206\u300f\u5927\u6982\u90fd\u6709\u91cd\u4fee\u904e\uff0c\u6240\u4ee5\u5982\u679c\u5ba2\u500c\u5c0d\u65bc\u6578\u5b78\u7684\u90e8\u4efd\u6709\u554f\u984c\uff0c\u8acb\u81ea\u884c\u4e0a\u7db2\u67e5\uff0c\u56e0\u70ba\u6211\u7b54\u7684\u4e0d\u6703\u6bd4google \u5927\u795e\u597d\u3002<\/p>\n","protected":false},"author":1,"featured_media":1264,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[166],"tags":[254,261,272,146],"_links":{"self":[{"href":"https:\/\/boochlin.com\/index.php?rest_route=\/wp\/v2\/posts\/271"}],"collection":[{"href":"https:\/\/boochlin.com\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/boochlin.com\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/boochlin.com\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/boochlin.com\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=271"}],"version-history":[{"count":5,"href":"https:\/\/boochlin.com\/index.php?rest_route=\/wp\/v2\/posts\/271\/revisions"}],"predecessor-version":[{"id":1396,"href":"https:\/\/boochlin.com\/index.php?rest_route=\/wp\/v2\/posts\/271\/revisions\/1396"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/boochlin.com\/index.php?rest_route=\/wp\/v2\/media\/1264"}],"wp:attachment":[{"href":"https:\/\/boochlin.com\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=271"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/boochlin.com\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=271"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/boochlin.com\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=271"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}