{"id":57357,"date":"2025-08-15T11:29:51","date_gmt":"2025-08-15T03:29:51","guid":{"rendered":"https:\/\/www.wsisp.com\/helps\/57357.html"},"modified":"2025-08-15T11:29:51","modified_gmt":"2025-08-15T03:29:51","slug":"gtsam%e4%b8%ad%e5%85%88%e9%aa%8c%e4%bd%8d%e5%a7%bf%ef%bc%88prior-factor%ef%bc%89%e8%af%a6%e8%a7%a3%e5%ba%94%e7%94%a8","status":"publish","type":"post","link":"https:\/\/www.wsisp.com\/helps\/57357.html","title":{"rendered":"GTSAM\u4e2d\u5148\u9a8c\u4f4d\u59ff\uff08Prior Factor\uff09\u8be6\u89e3\u5e94\u7528"},"content":{"rendered":"<h2>GTSAM\u5148\u9a8c\u4f4d\u59ff\u7684\u503c\u5e94\u8be5\u662f\u4ec0\u4e48<\/h2>\n<hr \/>\n<h3>1. \u5148\u9a8c\u4f4d\u59ff\u7684\u6765\u6e90<\/h3>\n<p>\u5728\u56e0\u5b50\u56fe\u4f18\u5316\u91cc&#xff0c;\u901a\u5e38\u9700\u8981\u7ed9\u7b2c\u4e00\u4e2a\u4f4d\u59ff\u8282\u70b9 X0 \u52a0\u4e00\u4e2a PriorFactor&#xff08;\u5148\u9a8c\u56e0\u5b50&#xff09;&#xff0c;\u9632\u6b62\u56fe\u5728\u4f18\u5316\u4e2d\u6f02\u79fb\u3002<br \/>\n\u8fd9\u4e2a\u5148\u9a8c\u7684\u503c\u662f\u4f60\u4eba\u4e3a\u6307\u5b9a\u7684&#xff0c;\u6bd4\u5982&#xff1a;<\/p>\n<p>poseNoise <span class=\"token operator\">&#061;<\/span> noiseModel<span class=\"token double-colon punctuation\">::<\/span><span class=\"token class-name\">Diagonal<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">Sigmas<\/span><span class=\"token punctuation\">(<\/span><span class=\"token class-name\">Vector6<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">Constant<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">1e-6<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\ngraph<span class=\"token punctuation\">.<\/span><span class=\"token function\">add<\/span><span class=\"token punctuation\">(<\/span><span class=\"token generic-function\"><span class=\"token function\">PriorFactor<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>Pose3<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token function\">Pose3<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> poseNoise<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>\u8fd9\u91cc Pose3() \u662f\u9ed8\u8ba4\u6784\u9020\u51fd\u6570&#xff0c;\u786e\u5b9e\u662f\u5355\u4f4d\u77e9\u9635&#xff08;\u5e73\u79fb\u4e3a\u96f6&#xff0c;\u65cb\u8f6c\u4e3a\u5355\u4f4d\u65cb\u8f6c&#xff09;\u3002<br \/>\n\u8fd9\u5c31\u610f\u5473\u7740 \u5982\u679c\u4f60\u6ca1\u6709\u7279\u522b\u6307\u5b9a&#xff0c;\u5355\u4f4d\u77e9\u9635\u4f1a\u4f5c\u4e3a\u7b2c\u4e00\u4e2a\u4f4d\u59ff\u7684\u5148\u9a8c\u3002<\/p>\n<hr \/>\n<h3>2. \u4e3a\u4ec0\u4e48\u5e38\u7528\u5355\u4f4d\u77e9\u9635\u4f5c\u4e3a\u5148\u9a8c&#xff1f;<\/h3>\n<ul>\n<li>\u56e0\u4e3a\u7b2c\u4e00\u5e27\u901a\u5e38\u8bbe\u4e3a\u5750\u6807\u7cfb\u539f\u70b9&#xff08;\u4e16\u754c\u5750\u6807\u7cfb&#xff09;\u3002<\/li>\n<li>\u8fd9\u6837\u540e\u7eed\u4f18\u5316\u7ed3\u679c\u662f\u76f8\u5bf9\u4e8e\u7b2c\u4e00\u5e27\u7684\u4e16\u754c\u5750\u6807\u8f93\u51fa\u7684&#xff0c;\u65b9\u4fbf\u53ef\u89c6\u5316\u4e0e\u5904\u7406\u3002<\/li>\n<li>\u5982\u679c\u4f60\u8f93\u5165\u7684\u5148\u9a8c\u4e0d\u662f\u5355\u4f4d\u77e9\u9635&#xff0c;\u6574\u4e2a\u4f18\u5316\u7ed3\u679c\u4f1a\u76f8\u5bf9\u4e8e\u4f60\u7ed9\u7684\u521d\u59cb\u5148\u9a8c\u53d1\u751f\u521a\u4f53\u53d8\u6362\u3002<\/li>\n<\/ul>\n<hr \/>\n<h3>3. \u53ef\u4ee5\u4e0d\u662f\u5355\u4f4d\u77e9\u9635<\/h3>\n<ul>\n<li>\u5982\u679c\u4f60\u7684\u7b2c\u4e00\u5e27\u5e76\u4e0d\u662f\u5750\u6807\u7cfb\u539f\u70b9&#xff0c;\u6bd4\u5982\u7528 GPS\/\u5176\u4ed6\u5b9a\u4f4d\u7cfb\u7edf\u7ed9\u51fa\u4e86\u521d\u59cb\u4f30\u8ba1&#xff1a;<\/li>\n<\/ul>\n<p>Pose3 <span class=\"token function\">init_pose<\/span><span class=\"token punctuation\">(<\/span><span class=\"token class-name\">Rot3<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">RzRyRx<\/span><span class=\"token punctuation\">(<\/span>yaw<span class=\"token punctuation\">,<\/span> pitch<span class=\"token punctuation\">,<\/span> roll<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token function\">Point3<\/span><span class=\"token punctuation\">(<\/span>x<span class=\"token punctuation\">,<\/span> y<span class=\"token punctuation\">,<\/span> z<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\ngraph<span class=\"token punctuation\">.<\/span><span class=\"token function\">add<\/span><span class=\"token punctuation\">(<\/span><span class=\"token generic-function\"><span class=\"token function\">PriorFactor<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>Pose3<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> init_pose<span class=\"token punctuation\">,<\/span> poseNoise<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<ul>\n<li>\u4f18\u5316\u4f1a\u4ee5\u8fd9\u4e2a\u5148\u9a8c\u4f5c\u4e3a\u53c2\u8003\u539f\u70b9&#xff0c;\u800c\u4e0d\u662f\u5355\u4f4d\u77e9\u9635\u3002<\/li>\n<\/ul>\n<hr \/>\n<h3>4. \u6ce8\u610f\u4e8b\u9879<\/h3>\n<ul>\n<li>\u5982\u679c\u5b8c\u5168\u4e0d\u52a0\u5148\u9a8c\u4f4d\u59ff\u56e0\u5b50&#xff08;PriorFactor&#xff09;&#xff0c;\u56e0\u5b50\u56fe\u4f1a\u6709\u81ea\u7531\u5ea6&#xff0c;\u4f18\u5316\u7ed3\u679c\u4f1a\u6f02\u79fb&#xff08;\u6574\u4e2a\u56fe\u7684\u7edd\u5bf9\u4f4d\u7f6e\/\u671d\u5411\u4e0d\u786e\u5b9a&#xff09;\u3002<\/li>\n<li>\u5982\u679c\u7b2c\u4e00\u5e27\u5df2\u7ecf\u88ab\u5176\u4ed6\u5f3a\u7ea6\u675f&#xff08;\u5982 GPS \u56e0\u5b50&#xff09;\u9501\u5b9a&#xff0c;\u4e5f\u53ef\u4ee5\u4e0d\u52a0\u5355\u4f4d\u77e9\u9635\u5148\u9a8c\u3002<\/li>\n<\/ul>\n<hr \/>\n<h3>5.\u5c0f\u7ed3<\/h3>\n<p>\u5728 GTSAM \u4e2d&#xff0c;\u201c\u5148\u9a8c\u4f4d\u59ff\u201d\u5e76\u4e0d\u662f\u56fa\u5b9a\u5355\u4f4d\u77e9\u9635&#xff0c;\u4f46\u5927\u591a\u6570 SLAM \/ LiDAR-IMU \u7cfb\u7edf\u4e3a\u4e86\u7b80\u5316&#xff0c;\u4f1a\u628a\u7b2c\u4e00\u5e27\u7684\u5148\u9a8c\u8bbe\u4e3a \u5355\u4f4d\u77e9\u9635 Pose3()&#xff0c;\u8fd9\u6837\u4f18\u5316\u7ed3\u679c\u5c31\u662f\u76f8\u5bf9\u7b2c\u4e00\u5e27\u7684\u5750\u6807\u7cfb\u3002<br \/>\n\u603b\u7ed3\u4e00\u53e5\u5c31\u662f&#xff1a;\u5728 GTSAM \u4e2d&#xff0c;\u4f18\u5316\u65f6\u7684\u201c\u5148\u9a8c\u4f4d\u59ff\u201d\u662f\u5426\u662f\u5355\u4f4d\u77e9\u9635&#xff0c;\u53d6\u51b3\u4e8e\u4f60\u7684\u56e0\u5b50\u56fe\u5efa\u6a21\u65b9\u5f0f&#xff0c;\u5e76\u4e0d\u662f\u56fa\u5b9a\u4e3a\u5355\u4f4d\u77e9\u9635\u3002<\/p>\n<hr \/>\n<h2>GTSAM\u4e2d\u6dfb\u52a0\u5148\u9a8c\u4f4d\u59ff<\/h2>\n<p>\u5728 GTSAM \u4e2d\u6dfb\u52a0\u5148\u9a8c\u4f4d\u59ff&#xff08;Prior Factor&#xff09;&#xff0c;\u5176\u5b9e\u5c31\u662f\u5f80\u56e0\u5b50\u56fe NonlinearFactorGraph \u91cc\u63d2\u5165\u4e00\u4e2a PriorFactor&lt;Pose3&gt;\u3002<\/p>\n<hr \/>\n<h3>1. \u5148\u9a8c\u4f4d\u59ff\u5728 GTSAM \u4e2d\u7684\u610f\u4e49<\/h3>\n<ul>\n<li>\n<p>\u56e0\u5b50\u56fe\u4f18\u5316 \u672c\u8d28\u4e0a\u9700\u8981\u4e00\u4e2a\u53c2\u8003\u7cfb\u539f\u70b9&#xff0c;\u5426\u5219\u95ee\u9898\u662f\u201c\u60ac\u7a7a\u7684\u201d&#xff08;\u65e0\u7edd\u5bf9\u53c2\u8003&#xff09;&#xff0c;\u4f18\u5316\u4f1a\u51fa\u73b0\u65e0\u7a77\u591a\u89e3&#xff08;\u4f8b\u5982\u6574\u4f53\u5e73\u79fb\u3001\u65cb\u8f6c\u81ea\u7531\u5ea6&#xff09;\u3002<\/p>\n<\/li>\n<li>\n<p>\u5148\u9a8c\u4f4d\u59ff&#xff08;Prior Factor&#xff09; \u5c31\u662f\u7ed9\u67d0\u4e2a\u8282\u70b9&#xff08;\u901a\u5e38\u662f\u7b2c\u4e00\u5e27&#xff09;\u52a0\u4e00\u4e2a\u7edd\u5bf9\u7ea6\u675f&#xff0c;\u5b9a\u4e49\u5b83\u5728\u5168\u5c40\u5750\u6807\u7cfb\u4e0b\u7684\u4f4d\u7f6e\u548c\u59ff\u6001\u3002<\/p>\n<\/li>\n<li>\n<p>\u6570\u5b66\u5f62\u5f0f&#xff1a;<\/p>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\">fprior(Xi)&#061;\u2225Xi\u2212Xprior\u2225\u03a3<br \/>\nf_{\\\\text{prior}}(X_i) &#061; \\\\| X_i &#8211; X_{prior} \\\\|_{\\\\Sigma}<br \/>\n<\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1.0361em;vertical-align: -0.2861em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.1076em\">f<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3175em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.1076em;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 text mtight\"><span class=\"mord mtight\">prior<\/span><\/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=\"mopen\">(<\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0785em\">X<\/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.0785em;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=\"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\">\u2225<\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0785em\">X<\/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.0785em;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.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: 1.0361em;vertical-align: -0.2861em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0785em\">X<\/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.0785em;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\">p<\/span><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0278em\">r<\/span><span class=\"mord mathnormal mtight\">i<\/span><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0278em\">or<\/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=\"mord\"><span class=\"mord\">\u2225<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3283em\"><span class=\"\" style=\"top: -2.55em;margin-left: 0em;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\">\u03a3<\/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><\/span><\/p>\n<p>\u5176\u4e2d&#xff1a;<\/p>\n<ul>\n<li><span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\">XiX_i<\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.8333em;vertical-align: -0.15em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0785em\">X<\/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.0785em;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><\/span><\/span><\/span> \u662f\u7b2c <span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\">ii<\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.6595em\"><\/span><span class=\"mord mathnormal\">i<\/span><\/span><\/span><\/span><\/span> \u4e2a\u4f4d\u59ff\u53d8\u91cf&#xff08;Pose3&#xff09;<\/li>\n<li><span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\">XpriorX_{prior}<\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.9694em;vertical-align: -0.2861em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0785em\">X<\/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.0785em;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\">p<\/span><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0278em\">r<\/span><span class=\"mord mathnormal mtight\">i<\/span><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0278em\">or<\/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><\/span><\/span><\/span> \u662f\u5df2\u77e5\u5148\u9a8c\u4f4d\u59ff<\/li>\n<li><span class=\"katex--inline\"><span class=\"katex\"><span class=\"katex-mathml\">\u03a3\\\\Sigma<\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.6833em\"><\/span><span class=\"mord\">\u03a3<\/span><\/span><\/span><\/span><\/span> \u662f\u566a\u58f0\u534f\u65b9\u5dee\u77e9\u9635&#xff08;\u566a\u58f0\u6a21\u578b&#xff09;<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<hr \/>\n<h3>2. \u6dfb\u52a0\u5148\u9a8c\u4f4d\u59ff\u7684\u57fa\u672c\u6b65\u9aa4<\/h3>\n<h4>2.1 \u51c6\u5907\u6570\u636e\u7ed3\u6784<\/h4>\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;gtsam\/slam\/PriorFactor.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;gtsam\/geometry\/Pose3.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;gtsam\/nonlinear\/NonlinearFactorGraph.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;gtsam\/nonlinear\/Values.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;gtsam\/noiseModel\/Diagonal.h&gt;<\/span><\/span><\/p>\n<h4>1.2 \u521b\u5efa\u56e0\u5b50\u56fe\u4e0e\u53d8\u91cf<\/h4>\n<p>gtsam<span class=\"token double-colon punctuation\">::<\/span>NonlinearFactorGraph graph<span class=\"token punctuation\">;<\/span><br \/>\ngtsam<span class=\"token double-colon punctuation\">::<\/span>Values initialEstimate<span class=\"token punctuation\">;<\/span><\/p>\n<h4>2.3 \u5b9a\u4e49\u5148\u9a8c\u566a\u58f0\u6a21\u578b<\/h4>\n<p><span class=\"token keyword\">auto<\/span> priorNoise <span class=\"token operator\">&#061;<\/span> gtsam<span class=\"token double-colon punctuation\">::<\/span>noiseModel<span class=\"token double-colon punctuation\">::<\/span><span class=\"token class-name\">Diagonal<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">Sigmas<\/span><span class=\"token punctuation\">(<\/span><br \/>\n    <span class=\"token punctuation\">(<\/span>gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">Vector<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">6<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&lt;&lt;<\/span><br \/>\n        <span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token comment\">\/\/ \u5e73\u79fb\u65b9\u5411 x,y,z \u65b9\u5dee&#xff08;\u7c73&#xff09;<\/span><br \/>\n        <span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.1<\/span>  <span class=\"token comment\">\/\/ \u65cb\u8f6c\u65b9\u5411 roll, pitch, yaw \u65b9\u5dee&#xff08;\u5f27\u5ea6&#xff09;<\/span><br \/>\n    <span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">finished<\/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<ul>\n<li>\n<p>\u6ce8\u610f\u8fd9\u91cc\u7684\u5355\u4f4d&#xff1a;<\/p>\n<ul>\n<li>\u5e73\u79fb\u5355\u4f4d&#xff1a;\u7c73<\/li>\n<li>\u65cb\u8f6c\u5355\u4f4d&#xff1a;\u5f27\u5ea6<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<hr \/>\n<h4>2.4 \u521b\u5efa\u5148\u9a8c\u4f4d\u59ff\u5e76\u52a0\u5165\u56e0\u5b50\u56fe<\/h4>\n<p>\u5047\u8bbe\u6211\u4eec\u8981\u5bf9 \u7b2c 0 \u4e2a\u8282\u70b9 \u6dfb\u52a0\u5148\u9a8c&#xff1a;<\/p>\n<p>gtsam<span class=\"token double-colon punctuation\">::<\/span>Pose3 <span class=\"token function\">priorPose<\/span><span class=\"token punctuation\">(<\/span><br \/>\n    gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token class-name\">Rot3<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">RzRyRx<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.0<\/span><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\">\/\/ yaw, pitch, roll<\/span><br \/>\n    gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">Point3<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">)<\/span>        <span class=\"token comment\">\/\/ x, y, z<\/span><br \/>\n<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>graph<span class=\"token punctuation\">.<\/span><span class=\"token function\">add<\/span><span class=\"token punctuation\">(<\/span>gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">PriorFactor<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>gtsam<span class=\"token double-colon punctuation\">::<\/span>Pose3<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> priorPose<span class=\"token punctuation\">,<\/span> priorNoise<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<ul>\n<li>\u7b2c\u4e00\u4e2a\u53c2\u6570 0 \u662f\u53d8\u91cf\u7684 Key&#xff08;\u4f4d\u59ff ID&#xff09;\u3002<\/li>\n<li>priorPose \u662f\u5df2\u77e5\u7684\u5148\u9a8c\u4f4d\u59ff\u3002<\/li>\n<li>priorNoise \u63a7\u5236\u5148\u9a8c\u7ea6\u675f\u7684\u6743\u91cd\u3002<\/li>\n<\/ul>\n<hr \/>\n<h4>2.5 \u7ed9\u53d8\u91cf\u4e00\u4e2a\u521d\u59cb\u503c<\/h4>\n<p>initialEstimate<span class=\"token punctuation\">.<\/span><span class=\"token function\">insert<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">Pose3<\/span><span class=\"token punctuation\">(<\/span><br \/>\n    gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token class-name\">Rot3<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">RzRyRx<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.05<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">0.02<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.01<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token comment\">\/\/ \u521d\u59cb\u4f30\u8ba1\u503c&#xff08;\u7a0d\u5fae\u6709\u504f\u5dee&#xff09;<\/span><br \/>\n    gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">Point3<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.05<\/span><span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<hr \/>\n<h4>2.6 \u5b8c\u6574\u4f18\u5316\u793a\u4f8b<\/h4>\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;gtsam\/nonlinear\/LevenbergMarquardtOptimizer.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;iostream&gt;<\/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 punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    gtsam<span class=\"token double-colon punctuation\">::<\/span>NonlinearFactorGraph graph<span class=\"token punctuation\">;<\/span><br \/>\n    gtsam<span class=\"token double-colon punctuation\">::<\/span>Values initialEstimate<span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token keyword\">auto<\/span> priorNoise <span class=\"token operator\">&#061;<\/span> gtsam<span class=\"token double-colon punctuation\">::<\/span>noiseModel<span class=\"token double-colon punctuation\">::<\/span><span class=\"token class-name\">Diagonal<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">Sigmas<\/span><span class=\"token punctuation\">(<\/span><br \/>\n        <span class=\"token punctuation\">(<\/span>gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">Vector<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">6<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&lt;&lt;<\/span> <span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.1<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">.<\/span><span class=\"token function\">finished<\/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>    gtsam<span class=\"token double-colon punctuation\">::<\/span>Pose3 <span class=\"token function\">priorPose<\/span><span class=\"token punctuation\">(<\/span>gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token class-name\">Rot3<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">RzRyRx<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                           gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">Point3<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0<\/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><\/p>\n<p>    graph<span class=\"token punctuation\">.<\/span><span class=\"token function\">add<\/span><span class=\"token punctuation\">(<\/span>gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">PriorFactor<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>gtsam<span class=\"token double-colon punctuation\">::<\/span>Pose3<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> priorPose<span class=\"token punctuation\">,<\/span> priorNoise<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>    initialEstimate<span class=\"token punctuation\">.<\/span><span class=\"token function\">insert<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">Pose3<\/span><span class=\"token punctuation\">(<\/span><br \/>\n        gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token class-name\">Rot3<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">RzRyRx<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.05<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">0.02<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.01<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        gtsam<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">Point3<\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token operator\">&#8211;<\/span><span class=\"token number\">0.1<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.05<\/span><span class=\"token punctuation\">)<\/span><br \/>\n    <span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>    gtsam<span class=\"token double-colon punctuation\">::<\/span>LevenbergMarquardtOptimizer <span class=\"token function\">optimizer<\/span><span class=\"token punctuation\">(<\/span>graph<span class=\"token punctuation\">,<\/span> initialEstimate<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    gtsam<span class=\"token double-colon punctuation\">::<\/span>Values result <span class=\"token operator\">&#061;<\/span> optimizer<span class=\"token punctuation\">.<\/span><span class=\"token function\">optimize<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>    std<span class=\"token double-colon punctuation\">::<\/span>cout <span class=\"token operator\">&lt;&lt;<\/span> <span class=\"token string\">&#034;\u4f18\u5316\u7ed3\u679c:\\\\n&#034;<\/span> <span class=\"token operator\">&lt;&lt;<\/span> result<span class=\"token punctuation\">.<\/span><span class=\"token generic-function\"><span class=\"token function\">at<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>gtsam<span class=\"token double-colon punctuation\">::<\/span>Pose3<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&lt;&lt;<\/span> std<span class=\"token double-colon punctuation\">::<\/span>endl<span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<hr \/>\n<h3>3. \u5e38\u89c1\u5751\u70b9<\/h3>\n<li>\n<p>\u53ea\u80fd\u52a0\u4e00\u4e2a\u5f3a\u5148\u9a8c<\/p>\n<ul>\n<li>\u901a\u5e38\u53ea\u5728\u7b2c\u4e00\u4e2a\u8282\u70b9\u52a0\u5148\u9a8c&#xff0c;\u5982\u679c\u7ed9\u591a\u4e2a\u8282\u70b9\u52a0\u5f3a\u5148\u9a8c&#xff0c;\u53ef\u80fd\u4f1a\u5bfc\u81f4\u7cfb\u7edf\u8fc7\u7ea6\u675f\u6216\u4f18\u5316\u5931\u8d25\u3002<\/li>\n<\/ul>\n<\/li>\n<li>\n<p>\u566a\u58f0\u4e0d\u8981\u592a\u5c0f<\/p>\n<ul>\n<li>\u5982\u679c\u566a\u58f0\u8fc7\u5c0f&#xff08;\u6743\u91cd\u8fc7\u5927&#xff09;&#xff0c;\u4f18\u5316\u4f1a\u5f3a\u884c\u9501\u5b9a\u8be5\u8282\u70b9&#xff0c;\u5bfc\u81f4\u8bef\u5dee\u65e0\u6cd5\u88ab\u4f20\u64ad\u4fee\u6b63\u3002<\/li>\n<\/ul>\n<\/li>\n<li>\n<p>\u6ce8\u610f Key \u5bf9\u5e94\u7684\u8282\u70b9\u5b58\u5728\u6027<\/p>\n<ul>\n<li>\u5fc5\u987b\u5148 insert \u6216\u5728\u4f18\u5316\u524d\u786e\u4fdd Key \u5b58\u5728&#xff0c;\u5426\u5219\u4f1a\u62a5\u9519\u3002<\/li>\n<\/ul>\n<\/li>\n<li>\n<p>\u5355\u4f4d\u6362\u7b97<\/p>\n<ul>\n<li>\u5e73\u79fb\u662f\u7c73&#xff0c;\u65cb\u8f6c\u662f\u5f27\u5ea6\u3002\u7ecf\u5e38\u6709\u4eba\u628a\u89d2\u5ea6\u76f4\u63a5\u653e\u8fdb\u53bb\u5bfc\u81f4\u4f18\u5316\u7ed3\u679c\u5f02\u5e38\u3002<\/li>\n<\/ul>\n<\/li>\n<hr \/>\n<h2>GTSAM\u5220\u9664\u591a\u4f59\u7684\u5148\u9a8c\u4f4d\u59ff<\/h2>\n<p>\u5728 GTSAM \u91cc&#xff0c;\u201c\u5220\u9664\u591a\u4f59\u7684\u5148\u9a8c\u4f4d\u59ff\u201d \u672c\u8d28\u4e0a\u662f\u79fb\u9664\u56e0\u5b50\u56fe\u91cc\u7684\u591a\u4f59 PriorFactor&#xff0c;\u56e0\u4e3a\u5148\u9a8c\u662f\u4f5c\u4e3a\u4e00\u4e2a\u56e0\u5b50\u52a0\u5165 NonlinearFactorGraph \u7684\u3002<\/p>\n<p>\u4e0b\u9762\u6211\u4eec\u5206\u4e3a\u4e24\u79cd\u60c5\u51b5\u8bf4\u660e&#xff1a;<\/p>\n<hr \/>\n<h3>1. \u5982\u679c\u662f\u81ea\u5df1\u4ee3\u7801\u91cc\u8bef\u52a0\u4e86\u591a\u4f59\u5148\u9a8c<\/h3>\n<p>\u6700\u7b80\u5355\u7684\u65b9\u6cd5&#xff1a;<\/p>\n<ul>\n<li>\u4e0d\u8981\u5f80\u56e0\u5b50\u56fe\u91cc\u6dfb\u52a0\u591a\u4f59\u7684 PriorFactor&#xff0c;\u6216\u8005\u5728\u6dfb\u52a0\u65f6\u7528\u6761\u4ef6\u5224\u65ad\u53ea\u52a0\u4e00\u6b21\u3002<\/li>\n<\/ul>\n<p>\u793a\u4f8b&#xff1a;<\/p>\n<p><span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>pose_id <span class=\"token operator\">&#061;&#061;<\/span> <span class=\"token number\">0<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span> <span class=\"token comment\">\/\/ \u53ea\u5728\u7b2c\u4e00\u5e27\u52a0\u5148\u9a8c<\/span><br \/>\n    graph<span class=\"token punctuation\">.<\/span><span class=\"token function\">add<\/span><span class=\"token punctuation\">(<\/span><span class=\"token generic-function\"><span class=\"token function\">PriorFactor<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>Pose3<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token function\">Pose3<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> poseNoise<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>\u8fd9\u6837\u6839\u672c\u4e0d\u4f1a\u6709\u591a\u4f59\u7684\u5148\u9a8c\u8fdb\u5165\u56e0\u5b50\u56fe\u3002<\/p>\n<hr \/>\n<h3>2. \u5982\u679c\u56e0\u5b50\u56fe\u5df2\u7ecf\u6709\u591a\u4f59\u5148\u9a8c&#xff08;\u8fd0\u884c\u4e2d\u52a8\u6001\u5220\u9664&#xff09;<\/h3>\n<p>\u5728 NonlinearFactorGraph \u4e2d&#xff0c;\u6bcf\u4e2a\u56e0\u5b50\u90fd\u6709\u7d22\u5f15&#xff0c;\u53ef\u4ee5\u6309\u6761\u4ef6\u5220\u9664\u3002<\/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;gtsam\/slam\/PriorFactor.h&gt;<\/span><\/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> graph<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\">auto<\/span> factor <span class=\"token operator\">&#061;<\/span> boost<span class=\"token double-colon punctuation\">::<\/span><span class=\"token generic-function\"><span class=\"token function\">dynamic_pointer_cast<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>gtsam<span class=\"token double-colon punctuation\">::<\/span>PriorFactor<span class=\"token operator\">&lt;<\/span>gtsam<span class=\"token double-colon punctuation\">::<\/span>Pose3<span class=\"token operator\">&gt;&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span>graph<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>factor <span class=\"token operator\">&amp;&amp;<\/span> factor<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">key<\/span><span class=\"token punctuation\">(<\/span><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 punctuation\">{<\/span> <span class=\"token comment\">\/\/ \u53ea\u4fdd\u7559\u7b2c\u4e00\u4e2a\u5148\u9a8c<\/span><br \/>\n        graph<span class=\"token punctuation\">.<\/span><span class=\"token function\">remove<\/span><span class=\"token punctuation\">(<\/span>i<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span> <span class=\"token comment\">\/\/ \u79fb\u9664\u8fd9\u4e2a\u5148\u9a8c<\/span><br \/>\n    <span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<p>\u6ce8\u610f&#xff1a;<\/p>\n<ul>\n<li>graph.remove(i) \u4f1a\u628a\u5bf9\u5e94\u56e0\u5b50\u79fb\u9664&#xff08;\u5b9e\u9645\u662f\u7f6e\u7a7a nullptr&#xff0c;\u4e0d\u5f71\u54cd\u7d22\u5f15&#xff09;\u3002<\/li>\n<li>\u5982\u679c\u4f60\u8981\u5f7b\u5e95\u538b\u7f29\u7d22\u5f15&#xff0c;\u53ef\u4ee5\u7528 graph.erase(std::remove(&#8230;)) \u65b9\u5f0f&#xff0c;\u4f46\u4e00\u822c\u6ca1\u5fc5\u8981\u3002<\/li>\n<\/ul>\n<hr \/>\n<h3>3. \u4e3a\u4ec0\u4e48\u8981\u5220\u9664\u591a\u4f59\u5148\u9a8c&#xff1f;<\/h3>\n<ul>\n<li>\u591a\u4f59\u5148\u9a8c \u4f1a\u8fc7\u5ea6\u7ea6\u675f\u56fe&#xff08;\u5c24\u5176\u662f\u591a\u673a\u5668\u4eba\u3001\u591a\u4f20\u611f\u5668\u878d\u5408\u65f6&#xff09;&#xff0c;\u53ef\u80fd\u5bfc\u81f4\u4f18\u5316\u53d1\u6563\u6216\u5f3a\u5236\u5bf9\u9f50\u9519\u8bef\u4f4d\u7f6e\u3002<\/li>\n<li>LiDAR-IMU \u7cfb\u7edf\u4e2d&#xff0c;\u4e00\u822c\u53ea\u5bf9\u7b2c\u4e00\u4e2a\u65f6\u95f4\u8282\u70b9\u52a0\u5148\u9a8c&#xff0c;\u5176\u4ed6\u4f4d\u7f6e\u7684\u7edd\u5bf9\u53c2\u8003\u7528\u56de\u73af\u7ea6\u675f\u3001GPS\u56e0\u5b50\u7b49\u66ff\u4ee3\u3002<\/li>\n<\/ul>\n<hr \/>\n<h3>4. \u5b9e\u6218\u5efa\u8bae<\/h3>\n<p>\u5982\u679c\u662f LiDAR-Inertial-Mapping \u6216 SLAM&#xff1a;<\/p>\n<ul>\n<li>\u5728\u5efa\u56fe\u65f6&#xff0c;\u53ea\u5728\u7b2c\u4e00\u5e27&#xff08;\u6216\u5730\u56fe\u521d\u59cb\u5173\u952e\u5e27&#xff09;\u6dfb\u52a0\u4e00\u4e2a\u5148\u9a8c\u3002<\/li>\n<li>\u5176\u4ed6\u5e27\u7684\u7edd\u5bf9\u4f4d\u7f6e\u7ea6\u675f&#xff0c;\u7528\u56de\u73af\u68c0\u6d4b\u3001GPS\u3001\u5730\u56fe\u5339\u914d\u7b49\u56e0\u5b50\u5b9e\u73b0\u3002<\/li>\n<li>\u5982\u679c\u540e\u5904\u7406\u8981\u79fb\u9664\u67d0\u4e2a\u5148\u9a8c&#xff0c;\u5efa\u8bae\u5728\u4fdd\u5b58\u56e0\u5b50\u56fe&#xff08;graph.save()&#xff09;\u524d\u5c31\u6e05\u7406\u3002<\/li>\n<\/ul>\n<hr \/>\n","protected":false},"excerpt":{"rendered":"<p>\u6587\u7ae0\u6d4f\u89c8\u9605\u8bfb57\u6b21\uff0c\u70b9\u8d5e2\u6b21\uff0c\u6536\u85cf2\u6b21\u30020.1, 0.1, 0.1, \/\/ \u5e73\u79fb\u65b9\u5411 x,y,z \u65b9\u5dee\uff08\u7c73\uff090.1, 0.1, 0.1 \/\/ \u65cb\u8f6c\u65b9\u5411 roll, pitch, yaw \u65b9\u5dee\uff08\u5f27\u5ea6\uff09\u5e73\u79fb\u5355\u4f4d\uff1a\u7c73\u65cb\u8f6c\u5355\u4f4d\uff1a\u5f27\u5ea6\u3002<\/p>\n","protected":false},"author":2,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[1],"tags":[5988,5984,5986,5989,5990,5985,523,5987],"topic":[],"class_list":["post-57357","post","type-post","status-publish","format-standard","hentry","category-server","tag-gtsam","tag-gtsam-g2o-ceres","tag-slam","tag-5989","tag-5985","tag-523","tag-5987"],"yoast_head":"<!-- This site is optimized with the Yoast SEO plugin v20.3 - https:\/\/yoast.com\/wordpress\/plugins\/seo\/ -->\n<title>GTSAM\u4e2d\u5148\u9a8c\u4f4d\u59ff\uff08Prior Factor\uff09\u8be6\u89e3\u5e94\u7528 - \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\/57357.html\" \/>\n<meta property=\"og:locale\" content=\"zh_CN\" \/>\n<meta property=\"og:type\" content=\"article\" \/>\n<meta property=\"og:title\" content=\"GTSAM\u4e2d\u5148\u9a8c\u4f4d\u59ff\uff08Prior Factor\uff09\u8be6\u89e3\u5e94\u7528 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\" \/>\n<meta property=\"og:description\" content=\"\u6587\u7ae0\u6d4f\u89c8\u9605\u8bfb57\u6b21\uff0c\u70b9\u8d5e2\u6b21\uff0c\u6536\u85cf2\u6b21\u30020.1, 0.1, 0.1, \/\/ \u5e73\u79fb\u65b9\u5411 x,y,z \u65b9\u5dee\uff08\u7c73\uff090.1, 0.1, 0.1 \/\/ \u65cb\u8f6c\u65b9\u5411 roll, pitch, yaw \u65b9\u5dee\uff08\u5f27\u5ea6\uff09\u5e73\u79fb\u5355\u4f4d\uff1a\u7c73\u65cb\u8f6c\u5355\u4f4d\uff1a\u5f27\u5ea6\u3002\" \/>\n<meta property=\"og:url\" content=\"https:\/\/www.wsisp.com\/helps\/57357.html\" \/>\n<meta property=\"og:site_name\" content=\"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\" \/>\n<meta property=\"article:published_time\" content=\"2025-08-15T03:29:51+00:00\" \/>\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=\"3 \u5206\" \/>\n<script type=\"application\/ld+json\" class=\"yoast-schema-graph\">{\"@context\":\"https:\/\/schema.org\",\"@graph\":[{\"@type\":\"WebPage\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/57357.html\",\"url\":\"https:\/\/www.wsisp.com\/helps\/57357.html\",\"name\":\"GTSAM\u4e2d\u5148\u9a8c\u4f4d\u59ff\uff08Prior Factor\uff09\u8be6\u89e3\u5e94\u7528 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\",\"isPartOf\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/#website\"},\"datePublished\":\"2025-08-15T03:29:51+00:00\",\"dateModified\":\"2025-08-15T03:29:51+00:00\",\"author\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41\"},\"breadcrumb\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/57357.html#breadcrumb\"},\"inLanguage\":\"zh-Hans\",\"potentialAction\":[{\"@type\":\"ReadAction\",\"target\":[\"https:\/\/www.wsisp.com\/helps\/57357.html\"]}]},{\"@type\":\"BreadcrumbList\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/57357.html#breadcrumb\",\"itemListElement\":[{\"@type\":\"ListItem\",\"position\":1,\"name\":\"\u9996\u9875\",\"item\":\"https:\/\/www.wsisp.com\/helps\"},{\"@type\":\"ListItem\",\"position\":2,\"name\":\"GTSAM\u4e2d\u5148\u9a8c\u4f4d\u59ff\uff08Prior Factor\uff09\u8be6\u89e3\u5e94\u7528\"}]},{\"@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":"GTSAM\u4e2d\u5148\u9a8c\u4f4d\u59ff\uff08Prior Factor\uff09\u8be6\u89e3\u5e94\u7528 - \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\/57357.html","og_locale":"zh_CN","og_type":"article","og_title":"GTSAM\u4e2d\u5148\u9a8c\u4f4d\u59ff\uff08Prior Factor\uff09\u8be6\u89e3\u5e94\u7528 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","og_description":"\u6587\u7ae0\u6d4f\u89c8\u9605\u8bfb57\u6b21\uff0c\u70b9\u8d5e2\u6b21\uff0c\u6536\u85cf2\u6b21\u30020.1, 0.1, 0.1, \/\/ \u5e73\u79fb\u65b9\u5411 x,y,z \u65b9\u5dee\uff08\u7c73\uff090.1, 0.1, 0.1 \/\/ \u65cb\u8f6c\u65b9\u5411 roll, pitch, yaw \u65b9\u5dee\uff08\u5f27\u5ea6\uff09\u5e73\u79fb\u5355\u4f4d\uff1a\u7c73\u65cb\u8f6c\u5355\u4f4d\uff1a\u5f27\u5ea6\u3002","og_url":"https:\/\/www.wsisp.com\/helps\/57357.html","og_site_name":"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","article_published_time":"2025-08-15T03:29:51+00:00","author":"admin","twitter_card":"summary_large_image","twitter_misc":{"\u4f5c\u8005":"admin","\u9884\u8ba1\u9605\u8bfb\u65f6\u95f4":"3 \u5206"},"schema":{"@context":"https:\/\/schema.org","@graph":[{"@type":"WebPage","@id":"https:\/\/www.wsisp.com\/helps\/57357.html","url":"https:\/\/www.wsisp.com\/helps\/57357.html","name":"GTSAM\u4e2d\u5148\u9a8c\u4f4d\u59ff\uff08Prior Factor\uff09\u8be6\u89e3\u5e94\u7528 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","isPartOf":{"@id":"https:\/\/www.wsisp.com\/helps\/#website"},"datePublished":"2025-08-15T03:29:51+00:00","dateModified":"2025-08-15T03:29:51+00:00","author":{"@id":"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41"},"breadcrumb":{"@id":"https:\/\/www.wsisp.com\/helps\/57357.html#breadcrumb"},"inLanguage":"zh-Hans","potentialAction":[{"@type":"ReadAction","target":["https:\/\/www.wsisp.com\/helps\/57357.html"]}]},{"@type":"BreadcrumbList","@id":"https:\/\/www.wsisp.com\/helps\/57357.html#breadcrumb","itemListElement":[{"@type":"ListItem","position":1,"name":"\u9996\u9875","item":"https:\/\/www.wsisp.com\/helps"},{"@type":"ListItem","position":2,"name":"GTSAM\u4e2d\u5148\u9a8c\u4f4d\u59ff\uff08Prior Factor\uff09\u8be6\u89e3\u5e94\u7528"}]},{"@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\/57357","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=57357"}],"version-history":[{"count":0,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/posts\/57357\/revisions"}],"wp:attachment":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/media?parent=57357"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/categories?post=57357"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/tags?post=57357"},{"taxonomy":"topic","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/topic?post=57357"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}